让你的自衡小车更快的站起来

葱拌豆腐

简单说就是先测出小车的死区，逐渐调高PWM的输出，电机开始转动时的PWM值就是小车的死区，然后在PID输出加上这个死区，这样就能更好的保持小车的稳定性。
原理分析（个人分析）：以前做伺服控制时，伺服电机有一个“使能”的状态，当伺服电机上使能后，电机轴就处在一种“抱死”的状态，在自衡小车的控制信号中增加死区值，也是相当于将小车始终保持在一个“抱死”的状态，小车的稳定性也就会更高。小车还没站立起来的可以试一下。
本人的小车基本站起来的，算法就是按照官方手册里来的，纯角度控制，没用速度控制，改天上视频。

一下代码是临时版的代码，注释很烂，程序也很烂，如果觉得有参考可以看一下，否则可以直接无视，O(∩_∩)O~

1. 
   
2. #include <I2Cdev.h>
   
3. #include <L298N.h>
   
4. #include <MPU6050.h>
   
5. #include <Wire.h>
   
6. #include <stdlib.h>
   
7. 
   
8. float ax,az,gy;//get acceleration_x,acceleration_z,rotation_y
   
9. long OffsetGy,OffsetSum;
   
10. float OffsetAccSumX,OffsetAccSumZ,OffsetAcc;
    
11. float AngleAcc,AngleRotation,AngleMerge;//angel computed by acceleration and rotation
    
12. //============define variable for freescale refrence manual=========
    
13. float AngleMergeLast,Tg,Pangle,Dangle,MotorDeadZone;
    
14. //==================
    
15. 
    
16. float Kp,Kd,Ki,pid_SP,pid_CV;
    
17. float Err,ErrLast,ErrAfter;//PID
    
18. long LastTime,NowTime,TimeSpan,SampleTime,PIDLastTime;//
    
19. ////===========kalman filter parameter define============
    
20. //float A,B,H,Q,R,P00,P10,X00,X10,Z,U,Kg;
    
21. ////==========add serial communication=========
    
22. String SerialString;
    
23. boolean mark=0;
    
24. char *comdata;
    
25. 
    
26. bool blinkState=false;
    
27. L298N l298n;
    
28. MPU6050 mpu;
    
29. 
    
30. 
    
31. void setup()
    
32. {
    
33.   
    
34. //  SampleTime=10;
    
35. //  //============initial kalman parameter======
    
36. //  A=1.0;
    
37. //  H=1.0;
    
38. //  Q=0.003;
    
39. //  R=0.01;
    
40. //  P00=5.0;
    
41. //  P10=0.0;
    
42. //  X00=0.0;
    
43. //  //===============end initial===========
    
44.   Wire.begin();
    
45.   Serial.begin(9600);
    
46.   Serial.println("start");
    
47.   mpu.initialize();
    
48.   Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
    
49.   //compute the gyroscope offset automatic
    
50.   OffsetSum=0;
    
51.   for(int i=0;i<1024;i++)
    
52.   {
    
53.     //Serial.println(mpu.getRotationY());
    
54. //    OffsetAccSumX+=mpu.getAccelerationX()/16384.00;
    
55. //    OffsetAccSumZ+=mpu.getAccelerationZ()/16384.00;
    
56.     OffsetSum=OffsetSum+mpu.getRotationY();
    
57.     //delay(20);
    
58.    }
    
59.   //OffsetAcc=(-1.0)*atan2(OffsetAccSumX/200.0,OffsetAccSumZ/200.0)*180/PI;
    
60.   OffsetGy=OffsetSum/1024;
    
61.   pid_SP=0.0;
    
62.   Kp=48.0;
    
63.   Ki=12.0;
    
64.   Kd=0.0;
    
65.   AngleMergeLast=0.0;
    
66.   Tg=0.5;
    
67.   MotorDeadZone=80;
    
68. }
    
69. 
    
70. void loop()
    
71. {
    
72.   
    
73.   NowTime=millis();
    
74.   TimeSpan=NowTime-LastTime;
    
75.   LastTime=NowTime;
    
76.   //==============add serial communication=======
    
77.   SerialString="";
    
78.   while(Serial.available()>0)
    
79.   {
    
80.     SerialString += char(Serial.read());
    
81.     delay(2);
    
82.     mark=1;
    
83.   }
    
84.   if (mark)
    
85.   {
    
86.     SerialString.trim();
    
87.     //int len=SerialString.length();
    
88. //    int PIndex=SerialString.indexOf('P');
    
89. //    int IIndex=SerialString.indexOf('I');
    
90. //    int DIndex=SerialString.indexOf('D');
    
91. //    int SIndex=SerialString.indexOf('S');
    
92.     //int PComma=SerialString.indexOf(',');
    
93.     //int IIndex=PIndex+1;
    
94.    
    
95.     //int IComma=SerialString.indexOf(',',IIndex);
    
96.     //==============set only one parameter each time===========
    
97.     String _Para="";
    
98.     switch(SerialString[0])
    
99.     {
    
100.       case 'p':
     
101.       case 'P':
     
102.         _Para=SerialString.substring(1);
     
103.         _Para.trim();
     
104.         comdata=&_Para[0];
     
105.         Kp=atof(comdata);
     
106.         Dangle=atof(comdata);
     
107.         
     
108.         break;
     
109.       case 'i':
     
110.       case 'I':
     
111.         _Para=SerialString.substring(1);
     
112.         _Para.trim();
     
113.         comdata=&_Para[0];
     
114.         Ki=atof(comdata);
     
115.         Pangle=atof(comdata);
     
116.         
     
117.         break;
     
118.         
     
119.       case 'm':
     
120.       case 'M':
     
121.         _Para=SerialString.substring(1);
     
122.         _Para.trim();
     
123.         comdata=&_Para[0];
     
124.         //Ki=atof(comdata);
     
125.         MotorDeadZone=atof(comdata);
     
126.         
     
127.         break;
     
128.         
     
129.       case 'd':
     
130.       case 'D':
     
131.         _Para=SerialString.substring(1);
     
132.         _Para.trim();
     
133.         comdata=&_Para[0];
     
134.         Kd=atof(comdata);
     
135.         
     
136.         break;
     
137.       case 's':
     
138.       case 'S':
     
139.         _Para=SerialString.substring(1);
     
140.         _Para.trim();
     
141.         comdata=&_Para[0];
     
142.         pid_SP=atof(comdata);
     
143.         
     
144.         break;
     
145.       case 't':
     
146.       case 'T':
     
147.         _Para=SerialString.substring(1);
     
148.         _Para.trim();
     
149.         comdata=&_Para[0];
     
150.         Tg=atof(comdata);
     
151.         
     
152.         break;
     
153.       default:
     
154.         break;
     
155.     }
     
156. //    String _mkp=SerialString.substring(PComma+1,IIndex-1);
     
157. //    _mkp.trim();
     
158. //    comdata=&_mkp[0];
     
159. //    Kp=atof(comdata);
     
160. //    String _mki=SerialString.substring(IComma+1);
     
161. //    _mki.trim();
     
162. //    comdata=&_mki[0];
     
163. //    Ki=atof(comdata);
     
164.    
     
165.     mark=0;
     
166.   }
     
167. 
     
168. //  //=================test kalman filter==========================
     
169. //  ax=mpu.getAccelerationX()/16384.00;
     
170. //  az=mpu.getAccelerationZ()/16384.00;
     
171. //  AngleAcc=(-1.0)*atan2(ax,az)*180/PI;
     
172. //  gy=(mpu.getRotationY()-OffsetGy)/131.00;
     
173. //  AngleRotation=AngleRotation+gy*TimeSpan/1000.00;
     
174. //  //=============kalman calculate============
     
175. //  B=TimeSpan/1000.0;
     
176. //  U=gy;
     
177. //  Z=AngleAcc;
     
178. //  X10=A*X00+B*U; //=======formula 1
     
179. //  P10=A*P00*(1/A)+Q;//formula 2
     
180. //  Kg=P10*(1/H)/(H*P10*(1/H)+R);//formula 3
     
181. //  
     
182. //  X00=X10+Kg*(Z-H*X10);//formula 4
     
183. //  
     
184. //  //P00=(1-Kg*H)*P10;//formula 5
     
185. //  P00=(1-Kg*H)*P10*(1-Kg*H)+Kg*R*Kg;
     
186. //  AngleMerge=X00;
     
187. //  
     
188. //  //================end test kalman filter======================
     
189. 
     
190.   //MergeAngle();
     
191.   MergeAngle_Freescale();
     
192.   //PID();
     
193.   if(1.0<AngleMerge)
     
194.   {
     
195.     l298n.MotorAForward(pid_CV);
     
196.     l298n.MotorBForward(pid_CV);
     
197.   }
     
198.   else if(-1.0>AngleMerge)
     
199.   {
     
200.     l298n.MotorAReversal(pid_CV);
     
201.     l298n.MotorBReversal(pid_CV);   
     
202.   }
     
203.   else
     
204.   {
     
205.     l298n.MotorAStop();
     
206.     l298n.MotorBStop();
     
207.   }
     
208.   Serial.print(AngleMerge);
     
209.   Serial.print(",");
     
210.   Serial.print(Dangle);
     
211.   Serial.print(",");
     
212.   Serial.print(Pangle);
     
213.   Serial.print(",");
     
214.   Serial.print(pid_CV);
     
215.   Serial.print(",");
     
216.   Serial.print(MotorDeadZone);
     
217.   Serial.print(",");
     
218.   Serial.println(Tg);
     
219.   delay(10);
     
220. 
     
221. }
     
222. void PID()
     
223. {
     
224.   //===========04-13 add sample time=============
     
225.   
     
226.   if((NowTime-PIDLastTime)>SampleTime)
     
227.   {
     
228.     PIDLastTime=NowTime;
     
229.    
     
230.     ErrAfter=ErrLast;
     
231.     ErrLast=Err;
     
232.     Err=pid_SP-abs(AngleMerge);
     
233.     //Err=pid_SP-AngleMerge;
     
234.     float deltOut=Kp*Err+Ki*ErrLast+Kd*ErrAfter;
     
235.    
     
236.     pid_CV=min(255.0,abs(pid_CV+deltOut)+MotorDeadZone);
     
237.     //pid_CV=max(80,pid_CV);//limit the min PWM output
     
238.   }
     
239. }
     
240. void MergeAngle()
     
241. {
     
242.   //compute AngleAcc
     
243.   ax=mpu.getAccelerationX()/16384.00;
     
244.   az=mpu.getAccelerationZ()/16384.00;
     
245.   AngleAcc=(-1.0)*atan2(ax,az)*180/PI;
     
246.   gy=(mpu.getRotationY()-OffsetGy)/131.00;
     
247.   AngleRotation=AngleRotation+gy*TimeSpan/1000.00;
     
248. 
     
249.   AngleMerge=0.9*AngleRotation+0.1*AngleAcc;
     
250. }
     
251. void MergeAngle_Freescale()
     
252. {
     
253.   ax=mpu.getAccelerationX()/16384.00;
     
254.   az=mpu.getAccelerationZ()/16384.00;
     
255.   AngleAcc=(-1.0)*atan2(ax,az)*180/PI;
     
256.   gy=(mpu.getRotationY()-OffsetGy)/131.00;
     
257.   AngleRotation=AngleRotation+gy*TimeSpan/1000.00;
     
258.   
     
259.   float AngleErr=AngleAcc-AngleMerge;
     
260.   AngleMerge=AngleMerge+(gy+AngleErr/Tg)*TimeSpan/1000.00;
     
261. 
     
262.   // ========this solution is from Freescale, it is effective
     
263. pid_CV=abs((0-gy)*Dangle+(0-AngleMerge)*Pangle)+MotorDeadZone;
     
264. pid_CV=min(255.0,pid_CV);
     
265. }
     

复制代码

xxx

想做个玩玩····

maxiujun

使用普通的玩具直流电机可以实现吗？

maxiujun

传感器使用的什么型号的？

北斗

maxiujun 发表于 2013-4-16 12:49
传感器使用的什么型号的？

看注释里写的是MPU6050

葱拌豆腐

maxiujun 发表于 2013-4-16 12:49
使用普通的玩具直流电机可以实现吗？

只要扭矩和速度足够就可以（也就是功率足够大，O(∩_∩)O~）。我用的是带测速的，淘宝价格22元，搜一下就知道了。

wshp1987515

标记一下，爪机看的麻烦来自: Android客户端

gucci99

没有<L298N.h>文件，不知能否提供。

164335413

程序是有点乱，每个人定义的名字不同，不过我的小车还不能实现蓝牙控制，有点头疼、

西瓜

164335413 发表于 2013-10-10 22:55
程序是有点乱，每个人定义的名字不同，不过我的小车还不能实现蓝牙控制，有点头疼、

同求<L298N.h>文件

浓颜。

在学习平衡小车，，，学习楼主了