说来惭愧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 } 代码有很多都不理解，资料是从论坛大神那里查到的。
转载于:https://www.cnblogs.com/pengwenzheng/p/7943931.html