说来惭愧2017-12-0118:13:27
并非原创，代码资料也是从论坛搜刮的。自己做了适配性的调整。
这个小车断断续续造了将近1个月！
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 }
代码有很多都不理解，资料是从论坛大神那里查到的。