//换算系数：(256/10) * (25*2*pi/(64*12))=20.944；
//--------------------------------------
int oommm; int ooommm; //int oooommmm; //double OLH= 10,ORH = 10;
int TN1=22; int TN2=23; int TN3=24; int TN4=25; int ENA=2; int ENB=3;
MPU6050 accelgyro; //#include <LiquidCrystal.h>; //LiquidCrystal lcd( 12, 11, 10, 9, 8,7);
#define Gry_offset -20 // 陀螺仪偏移量 //#define Gyr_Gain 0.04 // 满量程2000dps时灵敏度(dps/digital) #define Gyr_Gain 65.5 #define pi 3.14159
angleA= atan2(ay , az) * 180 / pi-0.2; // 根据加速度分量得到的角度(degree) //180度至0至-180（360度）取0度为坚直时中立点 因为坚直时为有偏差，所以减去0.2度....
//omega= Gyr_Gain * (gx + Gry_offset); // 当前角速度(degree/s) omega=(gx + Gry_offset)/Gyr_Gain; // 当前角速度(degree/s) if (abs(angleA)<30) { //这个是误差达到一定程度后的系统关闭开关.
//------------------------------------void setup() { Wire.begin(); //lcd.begin(16, 2); //lcd.print("hello, world!"); //delay(1000);
accelgyro.initialize(); delay(500);
int16_t ax, ay, az; int16_t gx, gy, gz;
/********** 互补滤波器参数 *********/ //unsigned long preTime = 0; // 采样时间 //float f_angle = 0.0; // 滤波处理后的角度值
ห้องสมุดไป่ตู้
*********** PID控制器参数 *********/ //unsigned long lastTime; float Output; //;, Setpoint,Input; //double errSum, lastErr; float kp, ki, kd,kpp; //int SampleTime = 0.1; //1 sec //float Outputa = 0.0; float angleA,omega; //double Kp, Ki, Kd; float P[2][2] = {{ 1, 0 },{ 0, 1 }}; float Pdot[4] ={ 0,0,0,0}; static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dtt=0.007,C_0 = 1; float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; float angle, angle_dot; // aaxdot,aax; float position_dot,position_dot_filter,positiono; //double Speed_Need=0;
Kalman_Filter(angleA,omega); PIDD(); PWMB(); //LCDD();
} else {
analogWrite(ENA, 0); //PWM调速a==0-255 analogWrite(ENB, 0);
//float K_angle=2; //float K_angle_dot=0.5; //float K_position=0.1; 256对应10V； //float K_position_dot=1;
//换算系数：256/10 =25.6； //换算系数：(256/10) * (2*pi/(64*12))=0.20944；//256/10:电压换算至PWM，
void loop() { accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//-----------------------------------------------------------------------------------------------------------------
自制两轮平衡车
制作：朱兆丰
控制器：ATmega16；8MHz； 加速度传感器：MMA2260；陀螺仪： EWTS82； 传感器的融合：卡尔曼滤波；
马达：EN_2342CR（速比64）+双路12脉 冲编码器+CD40106对信号整形； 驱动板芯片：CD4001+IR2111+IRF1404 （驱动电流可以很大）；
制作资料在压缩包里面，供参考；
• 速度传感器是mma7361
• 陀螺仪是ENC-03 包邮第八届 飞思卡尔智能车 MC9S12XS128MAA 80脚单片机最小系统 板
//卡耳漫滤波 PD融合控制代码:
#include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h"
pinMode(22,OUTPUT); pinMode(23,OUTPUT); pinMode(24,OUTPUT); pinMode(25,OUTPUT); pinMode(2,OUTPUT); pinMode(3,OUTPUT);
delay(100); //Serial.begin(9600); }