平衡小车站起来

tanbocandy

在这里感谢 弘毅 ，pww999，Randy，arduino官网，基本是看他们的帖子学习的，经过大半个月的理论学习，实验，加上一个星期的制作过程（主要是做架子，调试），我的小车算是能站立，但是有个问题：我用蓝牙控制小车前后（还不带原地旋转）移动，小车老是要倒，程序是参照PWW999的，在接受到命令后，改变平衡的角度，不知道是程序原因还是硬件问题（电机是12V，8KG，100转），程序在后面发出来，全部是用的库，所以看起来还是比较简单的，如果看了我前面两个帖子（MPU6050卡尔曼，和电机PID）的内容都可以看懂这个程序，我的小车平衡角度是180。
还是哪个问题，小车不能前后移动，一动就倒。

1. #include <PID_v1.h>
   
2. #include "Wire.h"
   
3. #include"Kalman.h"
   
4. #include "I2Cdev.h"
   
5. #include "MPU6050.h"
   
6. #define Left1 10
   
7. #define Left2 11
   
8. #define Right1 5
   
9. #define Right2 6
   
10. #define drift 20
    
11. 
    
12. double Setpoint, Input, Output;
    
13. double temp_kp,temp_ki,temp_kd;
    
14. //Kp=0.1,Ki=0.05,Kd=0,change depend on your system
    
15. PID myPID(&Input,&Output,&Setpoint,1,0,0,DIRECT);
    
16. MPU6050 accelgyro;
    
17. Kalman kalmanX; // Create the Kalman instances
    
18. //Kalman kalmanY;
    
19. //Kalman kalmanZ;
    
20. 
    
21. int16_t ax, ay, az;
    
22. int16_t gx, gy, gz;
    
23. uint32_t timer;
    
24. int bluetooth_read;
    
25. int Lspeed_need,Rspeed_need;
    
26. 
    
27. double accXangle, accYangle,accZangle; // Angle calculate using the accelerometer
    
28. double gyroXangle, gyroYangle,gyroZangle; // Angle calculate using the gyro
    
29. double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
    
30. 
    
31. void setup()
    
32. {
    
33.   
    
34.     pinMode(Left1,OUTPUT);
    
35.     pinMode(Left2,OUTPUT);
    
36.     pinMode(Right1,OUTPUT);
    
37.     pinMode(Right2,OUTPUT);
    
38.   
    
39.     car_stop();  
    
40.     // join I2C bus (I2Cdev library doesn't do this automatically)
    
41.     Wire.begin();
    
42. 
    
43.     // initialize serial communication
    
44.     // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
    
45.     // it's really up to you depending on your project)
    
46.     Serial.begin(9600);
    
47.     // initialize device
    
48.     accelgyro.initialize();
    
49.     // verify connection
    
50.     accelgyro.testConnection();
    
51.     timer=micros();
    
52.     Setpoint=180.00;
    
53.     myPID.SetSampleTime(10);
    
54.     myPID.SetMode(AUTOMATIC);
    
55. }
    
56. 
    
57. void loop()
    
58. {
    
59.     // read raw accel/gyro measurements from device
    
60.     data_recv();
    
61.     temp_kp=analogRead(A0)*0.15;
    
62.     temp_ki=analogRead(A1)*0.0002;
    
63.     temp_kd=analogRead(A2)*0.08;
    
64.     myPID.SetTunings(temp_kp,temp_ki,temp_kd);
    
65.    
    
66.     accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
67.     accXangle = (atan2(ay,az)+PI)*RAD_TO_DEG;
    
68.    // accYangle = (atan2(ax,az)+PI)*RAD_TO_DEG;
    
69.    
    
70.     double gyroXrate = (double)gx/131.0;
    
71.     //double gyroYrate =-((double)gy/131.0);
    
72. 
    
73.    
    
74.     gyroXangle+= gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter  
    
75.    // gyroYangle+= gyroYrate*((double)(micros()-timer)/1000000);
    
76. 
    
77.    
    
78.     kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
    
79.    // kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
    
80.     timer = micros();
    
81.     if((kalAngleX>(Setpoint-drift))&&(kalAngleX<(Setpoint+drift)))
    
82.     {
    
83.             if((kalAngleX>Setpoint)&&(kalAngleX<(Setpoint+drift)))
    
84.         {   
    
85.             myPID.SetControllerDirection(REVERSE);
    
86.             Input=kalAngleX;   
    
87.             myPID.Compute();
    
88.             analogWrite(Left2,Output);       //PWM out put
    
89.             analogWrite(Right2,Output);       //PWM out put
    
90.             analogWrite(Left1,0);       //PWM out put
    
91.             analogWrite(Right1,0);   //PWM out put
    
92.         }
    
93.         if((kalAngleX<Setpoint)&&(kalAngleX>(Setpoint-drift)))
    
94.         {
    
95.             myPID.SetControllerDirection(DIRECT);
    
96.             Input=kalAngleX;
    
97.             myPID.Compute();
    
98.             analogWrite(Left1,min(Output+Lspeed_need,255));       //PWM out put
    
99.             analogWrite(Right1,min(Output+Lspeed_need,255));       //PWM out put
    
100.             analogWrite(Left2,0);       //PWM out put
     
101.             analogWrite(Right2,0);       //PWM out put
     
102.         }
     
103.     }
     
104.     else
     
105.     {
     
106.       car_stop();  
     
107.     }
     
108.    // Serial.print(accXangle);Serial.print(",");
     
109.    // Serial.print(gyroXangle);Serial.print(",");
     
110.     Serial.println(kalAngleX);
     
111.     Serial.println(temp_kp);
     
112.     Serial.println(temp_ki);
     
113.     Serial.println(temp_kd);
     
114. 
     
115.     //  Serial.print("\t");
     
116.     //Serial.print(accYangle);Serial.print(",");
     
117.     //Serial.print(gyroYangle);Serial.print(",");
     
118.     //Serial.print(kalAngleY);Serial.print("\r\n");   
     
119. 
     
120. }
     
121.     void car_stop()
     
122. {
     
123.       analogWrite(Left1,0);       //PWM out put
     
124.       analogWrite(Right1,0);       //PWM out put
     
125.       analogWrite(Left2,0);       //PWM out put
     
126.       analogWrite(Right2,0);
     
127. }
     
128. 
     
129. void data_recv()
     
130. {
     
131.     if(Serial.available()>0)
     
132.     {
     
133.         bluetooth_read=Serial.read();
     
134.         switch (bluetooth_read)
     
135.         {
     
136.             case 0x11:
     
137.                  Setpoint=183.00;
     
138.                 break;
     
139.             case 0x22:
     
140.                   Setpoint=177.00;
     
141.                 break;
     
142.         /*     case 33:
     
143.                   Lspeed_need=10;
     
144.                   Rspeed_need=-10;
     
145.                   break;
     
146.               case 44:
     
147.                   Lspeed_need=-10;
     
148.                   Rspeed_need=10;
     
149.                  break;        */
     
150.         }
     
151.     }
     
152.     else
     
153.     {
     
154.         Setpoint=180.00;
     
155.     }
     
156. }
     

复制代码

pww999

效果不错嘛，改变前后角度数值大小，太大会一下就倒，太小又只会点下头！(你电池放在上面，头重脚轻了吧)
如果不知道大概前后数值，可以增加一电位器，从小调节(或者*0.1,,,0.01缩小调节范围)

iRobot

做的挺好，重心很低。学习了。

tanbocandy

pww999 发表于 2013-8-5 02:06
效果不错嘛，改变前后角度数值大小，太大会一下就倒，太小又只会点下头！(你电池放在上面，头重脚轻了吧)
...

我看一个国外的网站上说重量在高处会更容易保持平衡，就像人把扫帚倒立起来一样，所以我把电池放在上面的。。。难道被国外的坑了。。。

pww999

觉得重力在下面好吧，象不倒翕？

学慧放弃

是用的6050么？？？感觉比之前平稳许多，不知道跑起来效果会怎样？？？

tanbocandy

学慧放弃 发表于 2013-8-5 14:16
是用的6050么？？？感觉比之前平稳许多，不知道跑起来效果会怎样？？？

就是用的6050  我用的是PI控制，但是我看其他好多人都是用的PD，就是跑不起来，跑了就到，PWW999说是前进和后退的角度没调对，我还没试出来。

tanbocandy

pww999 发表于 2013-8-5 13:44
觉得重力在下面好吧，象不倒翕？

这个也是我郁闷的地方啊，到底是不倒翁还是像扫帚那样。。。。

学慧放弃

tanbocandy 发表于 2013-8-5 17:07
就是用的6050  我用的是PI控制，但是我看其他好多人都是用的PD，就是跑不起来，跑了就到，PWW999说是前进 ...

恩恩，加油，但是国外两轮平衡车都可以站人呢，貌似也是PI控制的

sfan

很不错,我什么时候能做一个呢...

小天狼星

不错额，我在用STM32做

一号zhong

你用的是什么样的arduino 主板

wwggmm01

楼主的kalman.h哪里来的

lmb312

分享下调试过程吧。

浓颜。

很好啊，，，坐等楼主让它跑起来