两轮自平衡小车

百家汇电脑

1. //两轮自平衡小车
   
2. 
   
3. //IDG330 Mannual, 2mv= 1度/s 的角速率,ad读数1024,3.3v,那么每读数对应 3.223mV,所以每读数对应3.223/2/180*PI= 0.028123弧度/秒
   
4. static const double SEMICIRCLE_ARC = 57.29578; /*半圆对应的弧度值*/
   
5. static const double GYRO_OPERATOR = 0.028123; /*AD读取的陀螺仪数值对应的弧度算子*/
   
6. 
   
7. /*kalman*/
   
8. static const double C_0 = 1;
   
9. static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005;//注意：dt的取值为kalman滤波器采样时间
   
10. double P[2][2] = {{ 1, 0 },
    
11.                   { 0, 1 }};
    
12. double Pdot[4] ={ 0,0,0,0};
    
13. double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
    
14. double angle, angle_dot;
    
15. 
    
16. /*sensor*/
    
17. double sensorPort[6] = { 0, 1, 2, 3, 4, 5};/*传感器地址,电路决定.accZ,gyroX, AD1, AD2, AD3, AD4*/
    
18. double sensorValue[6];/*传感器的返回值*/
    
19. 
    
20. /*传感器零点 0:Z轴(平行轴) 1:陀螺仪中点*/
    
21. double sensorZero[2] = {499,505};
    
22. double sensorAdjusted[2];/*传感器的返回值重整*/
    
23. //double provAngle;
    
24. 
    
25. /*moto*/
    
26. int E1 = 6;
    
27. int E2 = 9;
    
28. int M1 = 7;
    
29. int M2 = 8;
    
30. double deadAreaCompensation1 = 45,deadAreaCompensation2 = 35;
    
31. 
    
32. /*balance*/
    
33. double RATE[4] = { 0,0,0,0};/*公式中的4个变量*/
    
34. double K[4] = { 60.45, 1.27, 125, 0.75};/*公式中的4个常量*/
    
35. double K_AD[4] = { 1, 1, 1, 1};/*公式中的4个常量*/
    
36. double wheel_ls[7];/*左轮:0:编码器 1:位移(position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */
    
37. double wheel_rs[7];/*右轮:0:编码器 1:位移(position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */
    
38. 
    
39. double Torque;/*扭矩*/
    
40. unsigned int count,count2;
    
41. boolean OK=false;//这个是误差达到一定程度后的系统关闭开关.
    
42. int bf,X,Y;//从无线端发来的命令
    
43. 
    
44. /*kalman*/
    
45. /*angle_m:经过atan2(ax,ay)方法计算的偏角,弧度值
    
46. gyro_m:经过初步减去零点的陀螺仪角速度值,弧度值
    
47. */
    
48. void Kalman_Filter(double angle_m,double gyro_m)
    
49. {
    
50. angle+=(gyro_m-q_bias) * dt;
    
51. angle_err = angle_m - angle;
    
52. Pdot[0]=Q_angle - P[0][1] - P[1][0];
    
53. Pdot[1]=- P[1][1];
    
54. Pdot[2]=- P[1][1];
    
55. Pdot[3]=Q_gyro;
    
56. P[0][0] += Pdot[0] * dt;
    
57. P[0][1] += Pdot[1] * dt;
    
58. P[1][0] += Pdot[2] * dt;
    
59. P[1][1] += Pdot[3] * dt;
    
60. PCt_0 = C_0 * P[0][0];
    
61. PCt_1 = C_0 * P[1][0];
    
62. E = R_angle + C_0 * PCt_0;
    
63. K_0 = PCt_0 / E;
    
64. K_1 = PCt_1 / E;
    
65. t_0 = PCt_0;
    
66. t_1 = C_0 * P[0][1];
    
67. P[0][0] -= K_0 * t_0;
    
68. P[0][1] -= K_0 * t_1;
    
69. P[1][0] -= K_1 * t_0;
    
70. P[1][1] -= K_1 * t_1;
    
71. angle        += K_0 * angle_err;
    
72. q_bias += K_1 * angle_err;
    
73. angle_dot = gyro_m-q_bias;//也许应该用last_angle-angle
    
74. }
    
75. 
    
76. void do_balance() {
    
77. 
    
78. sensorValue[0] = analogRead(sensorPort[0]);//Z轴加速度
    
79. sensorValue[1] = analogRead(sensorPort[1]);//X轴旋转(陀螺)
    
80. sensorValue[2] = analogRead(sensorPort[2]);//旋钮1
    
81. sensorValue[3] = analogRead(sensorPort[3]);//旋钮2
    
82. sensorValue[4] = analogRead(sensorPort[4]);//旋钮3
    
83. sensorValue[5] = analogRead(sensorPort[5]);//旋钮4
    
84. 
    
85. //算出整理过的零点值
    
86. sensorAdjusted[0] = sensorValue[0] - sensorZero[0];
    
87. if(abs(sensorAdjusted[0]) > 70)//如果倾斜到一定程度,就停机
    
88. {
    
89.     OK = false;
    
90. sensorAdjusted[0] =sensorAdjusted[0]>0, 70,-70;
    
91. }
    
92. sensorAdjusted[1] = sensorZero[1] - sensorValue[1];/*减去初始零点*/
    
93. 
    
94. 
    
95. /*根据可变电阻改变K的倍数*/
    
96. //deadAreaCompensation = (sensorValue[4]+1)/10.24;
    
97. //K_AD[0] = (sensorValue[2]+1)/256;//102.4;/*值在0.001~10之间变化.在AD=100的时候接近1 */
    
98. //K_AD[1] = (sensorValue[3]+1)/256;//102.4;
    
99. //K_AD[2] = (sensorValue[4]+1)/256;//102.4;
    
100. //K_AD[3] = (sensorValue[5]+1)/256;//102.4;
     
101. 
     
102. /*根据上一周期Torqu的值统计轮子转动积分*/
     
103. wheel_ls[2] = Torque > 0, wheel_ls[0],-wheel_ls[0];
     
104. wheel_rs[2] = Torque > 0, wheel_rs[0],-wheel_rs[0];
     
105. wheel_ls[0] = wheel_rs[0] = 0;
     
106. /*************** balance *********************************************/
     
107. 
     
108. /*小车初始状态*/
     
109. {
     
110.     if(!OK)
     
111.     {
     
112.       if(abs(sensorAdjusted[0]) <= 3)//小车被推倒平衡位置后才启动电机
     
113.       {
     
114.         count=0;
     
115.         OK = true;
     
116.       }
     
117.       for(int i = 0; i <7;i++)
     
118.       {
     
119.         wheel_ls[i] = 0;
     
120.         wheel_rs[i] = 0;
     
121.       }
     
122.     }
     
123. }
     
124. 
     
125. /*balance*/
     
126. {
     
127.     /*GYRO_OPERATOR = 0.028123 AD读取的陀螺仪数值对应的弧度算子*/
     
128.     Kalman_Filter(atan2(sensorAdjusted[0], sqrt(10000-sensorAdjusted[0]*sensorAdjusted[0])), sensorAdjusted[1] * GYRO_OPERATOR);//10000:因为只有一轴加速度计,所以虚拟一个斜边   
     
129. 
     
130. RATE[0] = angle * SEMICIRCLE_ARC;//SEMICIRCLE_ARC=57.29578; /*半圆对应的弧度值*/
     
131.     RATE[1] = angle_dot * SEMICIRCLE_ARC;
     
132. }
     
133. 
     
134. /*计算速度 double wheel_ls[8]; 0:编码器累加 1:位移(position) 2:position_dot 3:速度(position_dot_filter) 4:speedNeed 5:turnNeed 6:speedOutPut
     
135. */
     
136. {
     
137.     wheel_ls[3] *= 0.95;              /*车轮速度滤波,wheel_ls[3] : position_dot_filter*/
     
138.     wheel_ls[3] += wheel_ls[2]*0.05; /*wheel_ls[2] : position_dot*/
     
139.     wheel_ls[1] += wheel_ls[3];       /*wheel_ls[1] : position*/
     
140.     wheel_ls[1] += wheel_ls[4];       /*wheel_ls[4] : speedNeed*/
     
141.     wheel_ls[1] = max(-50, wheel_ls[1]);
     
142.     wheel_ls[1] = min(50, wheel_ls[1]);
     
143. 
     
144.     RATE[2] = wheel_ls[3];//速度--滤波过了
     
145.     RATE[3] = wheel_ls[1];//位置
     
146. }
     
147. 
     
148. /*Torque 综合所有参数算出扭矩*/
     
149. {
     
150.     Torque = RATE[0] * K[0] * K_AD[0] + RATE[1] * K[1] * K_AD[1] + RATE[2] * K[2] * K_AD[2] + RATE[3] * K[3]* K_AD[3];
     
151.    
     
152.     //根据扭矩算轮子的命令值
     
153.    wheel_ls[6] = abs(Torque)+ deadAreaCompensation1;//wheel_ls[6]:扭矩输出 deadAreaCompensation1:左轮的死区补偿
     
154. wheel_ls[6] = min(255, wheel_ls[6]);//限制最大扭矩255
     
155.     wheel_ls[6] = OK, wheel_ls[6],0;
     
156. 
     
157. wheel_rs[6] = abs(Torque)+ deadAreaCompensation2;
     
158.     wheel_rs[6] = min(255, wheel_rs[6]);
     
159.     wheel_rs[6] = OK, wheel_rs[6],0;
     
160. }
     
161. 
     
162. if(Torque > 0)
     
163. {
     
164.     digitalWrite(M2, HIGH);
     
165.     digitalWrite(M1, LOW);
     
166. }
     
167. else
     
168. {
     
169.     digitalWrite(M2, LOW);
     
170.     digitalWrite(M1, HIGH);
     
171. }
     
172. analogWrite(E1, wheel_ls[6]); //PWM调速a==0-255
     
173. analogWrite(E2, wheel_rs[6]);
     
174. }
     
175. void do_msg(){
     
176. if(count%100==0)
     
177. {
     
178.     Serial.print("$CLEAR\r\n");
     
179.     Serial.print("$GO 1 1\r\n");
     
180.     Serial.print("$PRINT ");
     
181. 
     
182.     Serial.print(wheel_ls[1]);
     
183.     Serial.print(" ");
     
184.     Serial.print(wheel_ls[3]);
     
185.     //Serial.print(" ");
     
186.     //Serial.print(RATE[2]);
     
187.     //Serial.print(K[0]*K_AD[0]);
     
188.     //Serial.print(" ");
     
189.     //Serial.print(K[1]*K_AD[1]);
     
190.     //Serial.print(" ");
     
191.     Serial.print("\r\n");
     
192. 
     
193.     Serial.print("$GO 2 1\r\n");
     
194.     Serial.print("$PRINT ");
     
195.     //Serial.print(K[0]*K_AD[0]);
     
196.     //Serial.print(" ");
     
197.     //Serial.print(K[1]*K_AD[1]);
     
198.     //Serial.print(" ");
     
199.     //Serial.print(K[2]*K_AD[2]);
     
200.     //Serial.print(" ");
     
201.     //Serial.print(K[3]*K_AD[3]);
     
202.     //Serial.print(count);
     
203.     Serial.print(wheel_ls[5]);
     
204.     Serial.print(" ");
     
205.     Serial.print(wheel_rs[5]);
     
206.     //Serial.print(count);
     
207.     //Serial.print(Torque);
     
208.     Serial.print("\r\n");
     
209. }
     
210. }
     
211. /***************** setup-loop *************************************************/
     
212. void setup() {
     
213. count=0;
     
214. X=Y=8;
     
215. bf=-1;
     
216. //init motos
     
217. for (int i = 6; i <= 9; i++) {
     
218.     pinMode(i, OUTPUT);
     
219. }
     
220. Serial.begin(9600);//115200);
     
221. analogReference(EXTERNAL); //设置模拟输入为外部参考3.3V
     
222. 
     
223. attachInterrupt(0, blinkone, CHANGE); //设置为0号中断，中断函数blink，触发方式为RISING
     
224. attachInterrupt(1, blinktwo, CHANGE); //设置为0号中断，中断函数blink，触发方式为RISING
     
225. }
     
226. void blinkone()//中断函数
     
227. {
     
228. wheel_ls[0] ++;
     
229. }
     
230. void blinktwo()//中断函数
     
231. {
     
232. wheel_rs[0] ++;
     
233. }
     
234. 
     
235. void loop() {
     
236. do_balance();//计算平衡
     
237. do_msg();
     
238. delay(3);
     
239. count++;
     
240. if(count>=60000)
     
241.     count=0;
     
242. }
     

复制代码

xm8888

你自己弄的呀！

远远

好多地方看不懂啊……高人……………………{:soso_e179:}

davidce

又见卡尔曼滤波

donarduino

期待实验视频

zixo

继续学习！

荒野无涯

学习中……………………

zuzuhe

这个是全部代码吗