平衡车终于成功了

  1 #include  "Wire.h"`
  2 #include <U8g2lib.h>
  3 #include <SPI.h>
  4 const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
  5 const uint16_t I2C_TIMEOUT = 100; // Used to check for errors in I2C communication
  6 
  7 uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
  8   return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
  9 }
 10 
 11 uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
 12     Wire.beginTransmission(IMUAddress);
 13     Wire.write(registerAddress);
 14     Wire.write(data, length);
 15     uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
 16     if (rcode) {
 17       Serial.print(F("i2cWrite failed: "));
 18       Serial.println(rcode);
 19     }
 20     return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
 21 }
 22 
 23 uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
 24     uint32_t timeOutTimer;
 25     Wire.beginTransmission(IMUAddress);
 26     Wire.write(registerAddress);
 27     uint8_t rcode = Wire.endTransmission(false); // Don‘t release the bus
 28     if (rcode) {
 29       Serial.print(F("i2cRead failed: "));
 30       Serial.println(rcode);
 31       return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
 32     }
 33     Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
 34     for (uint8_t i = 0; i < nbytes; i++) {
 35       if (Wire.available())
 36         data[i] = Wire.read();
 37       else {
 38         timeOutTimer = micros();
 39         while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
 40         if (Wire.available())
 41           data[i] = Wire.read();
 42         else {
 43           Serial.println(F("i2cRead timeout"));
 44           return 5; // This error value is not already taken by endTransmission
 45         }
 46       }
 47     }
 48     return 0; // Success
 49 }
 50 
 51 /******************************************************/
 52 
 53 //2560 pin map  引脚定义好即可，然后改变一下PID的几个值（kp，kd，ksp，ksi）即可，剩下的全部都是固定的程序，
 54 //可能小车会有一点重心不在中点的现象，加一下角度值或者减一点即可
 55 //至于每个MPU6050的误差，自己调节一下即可，不是很难
 56 //调试时先将速度环的ksp，ksi=0，调到基本可以站起来，然后可能会出现倒，或者自动跑起来的时候加上速度环
 57 //这时就会很稳定的站起来，然后用小力气的手推不会倒。
 58 
 59 int ENA=10;
 60 int ENB=11;
 61 int IN1=4;
 62 int IN2=5;
 63 int IN3=6;
 64 int IN4=7;
 65 int MAS,MBS;
 66 /* IMU Data */
 67 double accX, accY, accZ;
 68 double gyroX, gyroY, gyroZ;
 69 int16_t tempRaw;
 70 double gyroXangle, gyroYangle; // Angle calculate using the gyro only
 71 double compAngleX, compAngleY; // Calculated angle using a complementary filter
 72 double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
 73 uint8_t i2cData[14]; // Buffer for I2C data
 74 uint32_t timer;
 75 unsigned long lastTime;      
 76 /***************************************/
 77 double P[2][2] = {{ 1, 0 },{ 0, 1 }};
 78 double Pdot[4] ={ 0,0,0,0};
 79 static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dtt=0.005,C_0 = 1;
 80 double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
 81 double angle,angle_dot,aaxdot,aax;
 82 double position_dot,position_dot_filter,positiono;
 83 /*-------------Encoder---------------*/
 84 
 85 #define LF 0
 86 #define RT 1
 87 
 88 //The balance PID
 89 float kp,kd,ksp,ksi;
 90 
 91 int Lduration,Rduration;
 92 boolean LcoderDir,RcoderDir;
 93 const byte encoder0pinA = 2;
 94 const byte encoder0pinB = 8;
 95 byte encoder0PinALast;
 96 const byte encoder1pinA = 3;
 97 const byte encoder1pinB = 9;
 98 byte encoder1PinALast;
 99 
100 int RotationCoder[2];
101 int turn_flag=0;
102 float move_flag=3.5;//////////////////////////////////
103 float right_need = 0, left_need = 0;;
104 
105 int pwm;
106 int pwm_R,pwm_L;
107 float range;
108 float range_error_all;
109 float wheel_speed;
110 float last_wheel;
111 float error_a=0;
112 U8G2_SSD1306_128X64_NONAME_F_SW_I2C u8g2(U8G2_R0, /* clock=*/ 12, /* data=*/ 13, /* reset=*/ U8X8_PIN_NONE);   // 四角显示屏
113 void Kalman_Filter(double angle_m,double gyro_m)
114 {
115     angle+=(gyro_m-q_bias) * dtt;
116     Pdot[0]=Q_angle - P[0][1] - P[1][0];
117     Pdot[1]=- P[1][1];
118     Pdot[2]=- P[1][1];
119     Pdot[3]=Q_gyro;
120     P[0][0] += Pdot[0] * dtt;
121     P[0][1] += Pdot[1] * dtt;
122     P[1][0] += Pdot[2] * dtt;
123     P[1][1] += Pdot[3] * dtt;
124     angle_err = angle_m - angle;
125     PCt_0 = C_0 * P[0][0];
126     PCt_1 = C_0 * P[1][0];
127     E = R_angle + C_0 * PCt_0;
128     K_0 = PCt_0 / E;
129     K_1 = PCt_1 / E;
130     t_0 = PCt_0;
131     t_1 = C_0 * P[0][1];
132     P[0][0] -= K_0 * t_0;
133     P[0][1] -= K_0 * t_1;
134     P[1][0] -= K_1 * t_0;
135     P[1][1] -= K_1 * t_1;
136     angle+= K_0 * angle_err;
137     q_bias += K_1 * angle_err;
138     angle_dot = gyro_m-q_bias;//也许应该用last_angle-angle
139 }
140  
141 void setup() {
142     Wire.begin();
143     u8g2.begin();   //选择U8G2模式，或者U8X8模式
144     Serial.begin(9600);
145     pinMode(IN1, OUTPUT);
146     pinMode(IN2, OUTPUT);
147     pinMode(IN3, OUTPUT);
148     pinMode(IN4, OUTPUT);  
149     pinMode(ENA, OUTPUT);
150     pinMode(ENB, OUTPUT);
151     TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
152  
153     i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
154     i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
155     i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
156     i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
157     while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
158     while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
159  
160     while (i2cRead(0x75, i2cData, 1));
161     if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
162         Serial.print(F("Error reading sensor"));
163         while (1);
164     }
165 
166     delay(20); // Wait for sensor to stabilize
167 
168     while (i2cRead(0x3B, i2cData, 6));
169     accX = (i2cData[0] << 8) | i2cData[1];
170     accY = (i2cData[2] << 8) | i2cData[3];
171     accZ = (i2cData[4] << 8) | i2cData[5];
172 
173 
174     double roll  = atan2(accX, accZ) * RAD_TO_DEG;
175     EnCoderInit();
176     timer = micros();
177 
178       //The balance PID
179     kp= 42;//42;//24.80;43
180     kd= 2.0;//1.9;//9.66;1.4
181     ksp=8.5;//8.5;//4.14;
182     ksi=2.1;//2.1;//0.99; 0.550
183      u8g2.firstPage();
184   do {
185      u8g2.setFont(u8g2_font_ncenB14_tr); //设置字体/////u8g2_font_5x7_tr
186      u8g2.setCursor(0, 15);    //设置光标处
187      u8g2.print("MiniBlack");  //输出内容
188      //u8g2.setCursor(0,30);    //设置光标处
189      //u8g2.print("T:");  //输出内容
190   } while ( u8g2.nextPage() );
191 }
192 
193 void EnCoderInit()
194 {
195     pinMode(8,INPUT);
196     pinMode(9,INPUT);
197     attachInterrupt(LF, LwheelSpeed, RISING);
198     attachInterrupt(RT, RwheelSpeed, RISING);
199 }
200 
201 void pwm_calculate()
202 {
203     unsigned long  now = millis();       // 当前时间(ms)
204     int Time = now - lastTime;
205     int range_error;
206     range += (Lduration + Rduration) * 0.5;
207     range *= 0.9;
208     range_error = Lduration - Rduration;
209     range_error_all += range_error;
210     
211     wheel_speed = range - last_wheel;   
212     //wheel_speed = constrain(wheel_speed,-800,800);
213     //Serial.println(wheel_speed);
214     last_wheel = range;  
215     pwm = (angle + 0.825) * kp + angle_dot * kd + range * ksp + wheel_speed * ksi;     
216     if(pwm > 255)pwm = 255;
217     if(pwm < -255)pwm = -255;
218     
219     if(turn_flag == 0)
220     {
221          pwm_R = pwm + range_error_all;
222          pwm_L = pwm - range_error_all;
223     }
224     else if(turn_flag == 1)     //左转
225     {
226         pwm_R = pwm ;  //Direction PID control
227         pwm_L = pwm + left_need * 68;
228         range_error_all = 0;     //clean
229     }
230     else if(turn_flag == 2)
231     {
232         pwm_R = pwm + right_need * 68;  //Direction PID control
233         pwm_L = pwm ;
234         range_error_all = 0;     //clean
235     }
236        
237        Lduration = 0;//clean
238        Rduration = 0;
239        lastTime = now;
240 }
241 
242 void PWMD()
243 {  
244       if(pwm>0)
245       {
246           digitalWrite(IN1, HIGH);
247           digitalWrite(IN2, LOW);
248           digitalWrite(IN3, LOW);
249           digitalWrite(IN4, HIGH);    
250       }
251       else if(pwm<0)
252       {
253           digitalWrite(IN1, LOW);
254           digitalWrite(IN2, HIGH);
255           digitalWrite(IN3, HIGH);
256           digitalWrite(IN4, LOW);
257       }
258       int PWMr = abs(pwm);
259       int PWMl = abs(pwm);
260     
261       analogWrite(ENB, max(PWMl,51)); //PWM调速a==0-255  51
262       analogWrite(ENA, max(PWMr,54)); //PWM调速a==0-255  54
263       
264 }
265 
266 void LwheelSpeed()
267 {
268       if(digitalRead(encoder0pinB))
269         Lduration++;
270       else Lduration--;
271 }
272 
273 
274 void RwheelSpeed()
275 {
276       if(digitalRead(encoder1pinB))
277         Rduration--;
278       else Rduration++;
279 }
280 
281 void control()
282 {
283     if(Serial.available()){
284       int val;
285       val=Serial.read();
286       switch(val){
287         case ‘w‘:
288           if(move_flag<5)move_flag += 0.5;
289           else  move_flag = 0;
290           break;
291         case ‘s‘:
292           if(move_flag<5)move_flag -= 0.5;
293           else  move_flag = 0;
294          Serial.println("back");
295           break;
296         case ‘a‘:
297           turn_flag = 1;
298           left_need = 0.6;
299           Serial.println("zuo");
300           break;
301         case ‘d‘:
302           turn_flag = 2;
303           right_need = 0.6;
304           Serial.println("you");
305           break;
306         case ‘t‘:
307           move_flag=0;
308           turn_flag=0;
309           right_need = left_need = 0;
310           Serial.println("stop");
311           break;
312         default:
313           break;
314           }
315       }
316 }
317 
318 void loop()
319 {
320 
321     control();
322     while (i2cRead(0x3B, i2cData, 14));
323     accX = ((i2cData[0] << 8) | i2cData[1]);
324     //accY = ((i2cData[2] << 8) | i2cData[3]);
325     accZ = ((i2cData[4] << 8) | i2cData[5]);
326     //gyroX = (i2cData[8] << 8) | i2cData[9];
327     gyroY = (i2cData[10] << 8) | i2cData[11];
328     //gyroZ = (i2cData[12] << 8) | i2cData[13];
329 
330     double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
331     timer = micros();
332  
333     double roll  = atan2(accX, accZ) * RAD_TO_DEG - move_flag;
334 
335 
336     double gyroXrate = gyroX / 131.0; // Convert to deg/s
337     double gyroYrate = -gyroY / 131.0; // Convert to deg/s
338 
339     //gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
340     //gyroYangle += gyroYrate * dt;
341 
342     Kalman_Filter(roll,gyroYrate);   
343     if(abs(angle)<35){
344         //Serial.println(angle); 
345        ////////////////////////////////////////////
346        
347         //////////////////////////////////////  
348         pwm_calculate();
349         PWMD();
350     }
351     else{
352         analogWrite(ENB, 0); //PWM调速a==0-255
353         analogWrite(ENA, 0); //PWM调速a==0-255
354     }  
355     delay(2);
356 }