ARDUINO+MPU6050+步进电机的自平衡小车

ANDOON

先亮出我的程序，参看大神们的：

1. /*****************====================================********************/
   
2. //**************** 一阶互补滤波+ PID  + MPU6050********************/
   
3. //************************************************************************//
   
4. #include <Wire.h>
   
5. #include <i2cdev.h>
   
6. #include <MPU6050.h>
   
7. #include <AFMotor.h>
   
8. //*********************************************************/
   
9. //*********************************************************/
   
10. MPU6050 accelgyro;
    
11. int16_t ax, ay, az;
    
12. int16_t gx, gy, gz;
    
13. 
    
14. //********************************************************/
    
15. //*************************L293D**************************/
    
16. // two stepper motors one on each port
    
17. AF_Stepper motor1(200, 1);//LEFT
    
18. AF_Stepper motor2(200, 2);//RIGHT
    
19. 
    
20. #define Gry_offset 296//陀螺仪X轴的静态飘移。
    
21. #define Gyr_Gain -0.00763//由陀螺仪X轴读数转换为角速度值，=1/131 敏感比例因子131、65.5、32.8、16.4 LSB/s，向前方向与X轴为反方向，故乘以-1  
    
22. #define ACC_Gain 0.000061//由读数转换为加速度值，=1/16384  Sensitivity Scale Factor 16384、8192、4096、2048 LSB/g
    
23. #define pi 3.1415926
    
24. #define K 0.715//一阶互补滤波权重取值，不知由来，可更改。
    
25. 
    
26. /********** 互补滤波器参数 *********/
    
27. unsigned long now ;
    
28. unsigned long preTime = 0;
    
29. float angleG = 0;
    
30. 
    
31. /*********** PID控制器参数 *********/
    
32. unsigned long lastTime;           // 前次时间
    
33. float ITerm, lastInput;    // 积分项、前次输入
    
34. float Output = 0.0;        // PID输出值
    
35. float LOutput,ROutput;
    
36. 
    
37. float LSpeed_Need=0.0,RSpeed_Need=0.0;// 转弯设置
    
38. 
    
39. void setup(){
    
40.   Wire.begin();
    
41.   Serial.begin(38400);
    
42.   accelgyro.initialize();//MPU6050初始化。
    
43.   motor1.setSpeed(10);
    
44.   motor2.setSpeed(10);
    
45. }
    
46. void loop(){
    
47.   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
    
48.   float Y_Accelerometer = ay * ACC_Gain;        //转换为向前的加速度(g)，为负值
    
49.   float Z_Accelerometer = az * ACC_Gain;        //转换为向下的加速度(g)
    
50.   float angleA= atan(Y_Accelerometer/ Z_Accelerometer)* (-180)/ pi;//加速度仪，反正切获得弧度值，乘以180度/pi
    
51.                                                                    //获得角度值，乘以-1得正值
    
52. ANGLE_1();                                                //一阶互补滤波后的输出倾斜角度
    
53. if (abs(angleA)<45) {  // 角度小于45度 运行程序
    
54.       PIDD();                                             //PID控制器
    
55.       DISTAN();                                        //左右电机步进距离输出
    
56.   }
    
57.    else {      // 角度大于45度 停止PWM输出
    
58.       motor1.release();
    
59.           motor2.release();
    
60.         }
    
61.    Serial.print(angleG); Serial.print("\t");
    
62.    Serial.println(Output);
    
63. }
    
64. 
    
65.   //*********************************************************************/
    
66.   //*************angleG= 一阶互补滤波后的输出倾斜角度(o)**********************/
    
67. void ANGLE_1()
    
68. {
    
69.   now = millis();//当前时间(ms)
    
70.   float dt = (now - preTime) / 1000.0;//微分时间(s)
    
71.   preTime = now;//上一次采样时间(ms)                                                            
    
72.   float gx_revised = gx + Gry_offset;//陀螺仪x轴静态时修正后的角速度读数，向前为负值
    
73.   float omega= Gyr_Gain* gx_revised;//陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
    
74.   float angle_dt = omega * dt;//角度的单次积分值
    
75.   //angleG+= angle_dt;//陀螺仪，积分获得角度值
    
76.   angleG+= (Gyr_Gain * (gx + Gry_offset)) * dt;
    
77.   float A= K/ (K+ dt);//陀螺仪的权值
    
78.   angleG= A* (angleG+ omega* dt)+ (1-A)* angleA;//一阶互补滤波后的输出角度(o)
    
79. }
    
80.   //************************PID控制器*************************************/
    
81. //*************步进电机根据倾斜程度调整输出距离***************************/
    
82. //************左电机输出距离LOutput**************************************/
    
83. //************右电机输出距离ROutput**************************************/
    
84.   //********************************************************************/
    
85. void PIDD(){
    
86.    
    
87. 
    
88.   }
    
89. //**********************左右电机步进距离输出******************************/
    
90. //***************************************************************/
    
91. void DISTAN(){
    
92.     stepper1.setMaxSpeed(300.0);
    
93.     stepper1.setAcceleration(100.0);
    
94.     stepper1.moveTo(LOutput);
    
95.    
    
96.     stepper2.setMaxSpeed(300.0);
    
97.     stepper2.setAcceleration(100.0);
    
98.     stepper2.moveTo(ROutput);
    
99.    
    
100.     stepper1.run();
     
101.     stepper2.run();
     
102. }
     

复制代码

望大神们帮忙修改PID控制和步进电机控制程序，小弟谢谢了。

ANDOON

电机驱动是L293D模块。

学慧放弃

那平衡怎么和前进结合的？？？

FoieDEEEE_仲敬

很复杂啊，看不懂==。
把这些东西弄成一个库该多好

ANDOON

学慧放弃 发表于 2013-6-9 19:29
那平衡怎么和前进结合的？？？

//************************PID控制器*************************************/
//*************步进电机根据倾斜程度调整输出步进距离***************************/
//************左电机输出距离LOutput**************************************/
//************右电机输出距离ROutput**************************************/
   //********************************************************************/
void PIDD(){
     

  }
//**********************左右电机步进距离输出******************************/
//***************************************************************/
void DISTAN(){
     stepper1.setMaxSpeed(300.0);
     stepper1.setAcceleration(100.0);
     stepper1.moveTo(LOutput);
     
    stepper2.setMaxSpeed(300.0);
     stepper2.setAcceleration(100.0);
     stepper2.moveTo(ROutput);
     
    stepper1.run();
     stepper2.run();
}

学慧放弃

ANDOON 发表于 2013-6-9 19:39
//************************PID控制器*************************************/
//*************步进电机 ...

喔，你那小车平衡性能好不？？？不是说陀螺仪算法还是有很大误差吗？？

ANDOON

学慧放弃 发表于 2013-6-9 19:47
喔，你那小车平衡性能好不？？？不是说陀螺仪算法还是有很大误差吗？？

误差不大。。不会控制它平衡，不会算应该让小车步进多少，才能平衡。

学慧放弃

ANDOON 发表于 2013-6-9 19:58
误差不大。。不会控制它平衡，不会算应该让小车步进多少，才能平衡。

你可以参考德国四轴的算法看看

L╃→煌/xin

步进电机做不了平衡车吧？

ANDOON

L╃→煌/xin 发表于 2013-8-17 16:24
步进电机做不了平衡车吧？

基于Cortex-M4与步进电机下两轮自平衡小车的移动控制
http://www.amobbs.com/thread-5531409-1-1.html

dalywan

同感，我也觉得步进电机跑起来真的很慢啊。

tangmao48

第一次见到有人用步进电机做平衡车

ANDOON

1. /*****************====================================********************/
   
2. //**************** 一阶互补滤波+ PID  + MPU6050********************/
   
3. //************************************************************************//
   
4. #include <Wire.h>
   
5. #include <i2cdev.h>
   
6. #include <MPU6050.h>
   
7. //*********************************************************/
   
8. //*********************************************************/
   
9. MPU6050 accelgyro;
   
10. int16_t ax, ay, az;
    
11. int16_t gx, gy, gz;
    
12. 
    
13. //********************************************************/
    
14. //*************************MOTOR**************************/
    
15. // Define two steppers and the pins they will use
    
16. int dirPin_1 = 8;
    
17. int stepperPin_1 = 9;
    
18. int dirPin_2 = 6;
    
19. int stepperPin_2 = 7;
    
20. 
    
21. 
    
22. #define Gry_offset 296//陀螺仪X轴的静态飘移。
    
23. #define Gyr_Gain -0.00763//由陀螺仪X轴读数转换为角速度值，=1/131 敏感比例因子131、65.5、32.8、16.4 LSB/s，向前方向与X轴为反方向，故乘以-1  
    
24. #define ACC_Gain 0.000061//由读数转换为加速度值，=1/16384  Sensitivity Scale Factor 16384、8192、4096、2048 LSB/g
    
25. #define pi 3.1415926
    
26. #define K 0.715//一阶互补滤波权重取值，不知由来，可更改。
    
27. 
    
28. /********** 互补滤波器参数 *********/
    
29. unsigned long now ;
    
30. unsigned long preTime = 0;
    
31. float angleG = 0;
    
32. 
    
33. /*********** PID控制器参数 *********/
    
34. [color=Red]unsigned long lastTime = 0;           // 前次时间
    
35. float ITerm = 0.0, lastInput = 0.0;    // 积分项、前次输入
    
36. float Output = 0.0;        // PID输出值
    
37. [/color]float LOutput,ROutput;
    
38. 
    
39. float LSpeed_Need=0.0,RSpeed_Need=0.0;// 转弯设置
    
40. 
    
41. void setup(){
    
42.   Wire.begin();
    
43.   Serial.begin(38400);
    
44.   accelgyro.initialize();//MPU6050初始化。
    
45.   pinMode(dirPin_1, OUTPUT);
    
46.   pinMode(stepperPin_1, OUTPUT);
    
47.   pinMode(dirPin_2, OUTPUT);
    
48.   pinMode(stepperPin_2, OUTPUT);
    
49. }
    
50. void loop(){
    
51.   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
    
52.   float Y_Accelerometer = ay * ACC_Gain;//转换为向前的加速度(g)，为负值
    
53.   float Z_Accelerometer = az * ACC_Gain;//转换为向下的加速度(g)
    
54.   float angleA= atan(Y_Accelerometer/ Z_Accelerometer)* (-180)/ pi;//加速度仪，反正切获得弧度值，乘以180度/pi
    
55.                                                                    //获得角度值，乘以-1得正值
    
56.   now = millis();//当前时间(ms)
    
57.   float dt = (now - preTime) / 1000.0;//微分时间(s)
    
58.   preTime = now;//上一次采样时间(ms)                                                            
    
59.   float gx_revised = gx + Gry_offset;//陀螺仪x轴静态时修正后的角速度读数，向前为负值
    
60.   float omega= Gyr_Gain* gx_revised;//陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
    
61.   float angle_dt = omega * dt;//角度的单次积分值
    
62.   //angleG+= angle_dt;//陀螺仪，积分获得角度值
    
63.   angleG+= (Gyr_Gain * (gx + Gry_offset)) * dt;
    
64.   float A= K/ (K+ dt);//陀螺仪的权值
    
65.   angleG= A* (angleG+ omega* dt)+ (1-A)* angleA;//一阶互补滤波后的输出角度(o)
    
66. //********************************************************************
    
67. //*********************************************************************
    
68.   if (abs(angleG)<45) {    // 角度小于45度 运行程序
    
69.       
    
70.       PIDD();
    
71.       PWMB();
    
72.    }
    
73.    Serial.print(angleG); Serial.print("\t");
    
74.    Serial.println(Output);
    
75. }
    
76.   //*********************************************************************/
    
77.   //********************************************************************/
    
78.   /**********=================PID控制器====================*************/
    
79.   //********************************************************************/
    
80. void PIDD(){
    
81.     now = millis();                                         // 当前时间(ms)
    
82.     float TimeChange = (now - lastTime) / 1000.0;               // 采样时间(s)
    
83.     float Kp = 10.0, Ki = 0.23, Kd = 0.0;                    // 比例系数、积分系数、微分系数
    
84.     float SampleTime = 0.1;                                 // 采样时间(s)
    
85.     float Setpoint =-2.0;                                  // 设定目标值(degree)
    
86.     //float outMin = -400.0, outMax = +400.0;                   // 输出上限、输出下限
    
87.     if(TimeChange >= SampleTime) {                              // 到达预定采样时间时
    
88.         float Input = angleG;                              // 输入赋值
    
89.         float error = Setpoint - Input;                     // 偏差值
    
90.         ITerm+= (Ki * error * TimeChange);                      // 计算积分项
    
91.        // ITerm = constrain(ITerm, outMin, outMax);           // 限定值域
    
92.         float DTerm = Kd * (Input - lastInput) / TimeChange;    // 计算微分项
    
93.         Output = Kp * error + ITerm + DTerm;                // 计算输出值
    
94.        // Output = constrain(Output, outMin, outMax);         // 限定值域
    
95.         LOutput=Output+RSpeed_Need;                         //左电机
    
96.         ROutput=Output-RSpeed_Need;                         //右电机
    
97.         lastInput = Input;                                  // 记录输入值
    
98.         lastTime = now;                                     // 记录本次时间
    
99.     }
    
100.   }
     
101. //**********************PWM调速输出******************************/
     
102. //***************************************************************/
     
103. void PWMB(){
     
104.   if(Output > 0){
     
105.     step_forwards(Output);
     
106.   }
     
107.   if (Output < 0){
     
108.     step_backwards(abs(Output));
     
109.   }
     
110. }
     
111. //**********************电机运转控制******************************/
     
112. //***************************************************************/
     
113. void step_forwards(int speeds){
     
114.     digitalWrite(dirPin_1,0);
     
115.     digitalWrite(dirPin_2,1);
     
116.    
     
117.     digitalWrite(stepperPin_1, HIGH);
     
118.     digitalWrite(stepperPin_2, HIGH);
     
119.     delayMicroseconds(speeds);
     
120.     digitalWrite(stepperPin_1, LOW);
     
121.     digitalWrite(stepperPin_2, LOW);
     
122.     delayMicroseconds(speeds);
     
123.   }
     
124. [color=Red]  void step_backwards(int speeds){
     
125.     digitalWrite(dirPin_1,1);
     
126.     digitalWrite(dirPin_2,0);
     
127.    
     
128.     digitalWrite(stepperPin_1, HIGH);
     
129.     digitalWrite(stepperPin_2, HIGH);
     
130.     delayMicroseconds(speeds);
     
131.     digitalWrite(stepperPin_1, LOW);
     
132.     digitalWrite(stepperPin_2, LOW);
     
133.     delayMicroseconds(speeds);[/color]
     
134.   }

复制代码

最终能够稳定站立的程序，使用LV8731驱动步进电机，L293输出电流太小，容易热。

ANDOON

dalywan 发表于 2013-9-1 22:40
同感，我也觉得步进电机跑起来真的很慢啊。

基于Cortex-M4与步进电机下两轮自平衡小车的移动控制
http://www.amobbs.com/thread-5531409-1-1.html

ANDOON

tangmao48 发表于 2013-9-1 23:16
第一次见到有人用步进电机做平衡车

基于Cortex-M4与步进电机下两轮自平衡小车的移动控制
http://www.amobbs.com/thread-5531409-1-1.html