极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 155324|回复: 115

Arduino 自平衡小车,制作经验记录与分享,愿有兴趣人士,一起学习探讨。

  [复制链接]
发表于 2014-5-23 14:16:28 | 显示全部楼层 |阅读模式
本帖最后由 smxyk123 于 2014-5-24 09:20 编辑

论坛潜行1个月,终于做出我的平衡小车了。以下为我的制作经验。希望大家共同学习进步。

一、以下为物料清单:
NO        名稱                        數量        功能               
1        電機JGA25-371                        2Pcs        提供動力               
2        電機驅動模塊L298n                        1Pcs        驅動馬達               
3        Arduino nano V3.0                        1Pcs        主控制晶片               
4        HC-06藍牙通訊模塊                        1Pcs        與藍牙設備通訊               
5        JY521-MP6050 陀螺儀加速計                        1Pcs        提供平衡依據               
6        機構件                        若干        拼接               
二、图片

三、算法
1.有参考过卡尔曼滤波,加Arduino 内置PID库并使用Processing 图形工具在线调试PID参数方法,小车运动趋势有,但是无法站立,并且参数始终没调好。
2.参考高通滤波方法,效果出奇的好。调试了下参数,就站起来了。

四、code分享。
1.请大家重点看MP6050 initial 及公式。如有疑问,愿意解答。
  1. //copyright by kaiser 20140423 V1.0
  2. void loop()
  3. {
  4.   //////////////////////////////////////MP6050//////////////////////////////////////////////////////////////////
  5.      accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  6.     Angle_Calcu();
  7.     Serial.print("Angle");Serial.println(Angle);
  8.     if (abs(Angle)<45) /////////////////up-right judge
  9.     {
  10.       Input=Angle;
  11.       //myPID.SetTunings(Kp, Ki, Kd);
  12.       //myPID.Compute();
  13.       Output=Kp*Angle+Kd*Gyro_y;
  14.       Serial.print("Output");Serial.println(Output);
  15.       SetMotorVoltage(Output,Output);
  16.   }
  17.   else{SetMotorVoltage(0,0);//角度大于45度 停止PWM输出}
  18.         if(millis()>serialTime)
  19.     {
  20.       SerialReceive();
  21.       SerialSend();
  22.       serialTime+=500;
  23.     }
  24.   }
  25. }
复制代码
2.我把程序在更新下,最终版上传。

  1. //copyright by kaiser 20140521 V1.0
  2. /////////////////////////////////////////////////////////////////////////////////////////////////////
  3. #include "Wire.h"            //serial
  4. #include "I2Cdev.h"          //IIC
  5. #include "MPU6050.h"         //acc&gyro Sensor
  6. //Define Variables we'll be connecting to
  7. char val='s';int Speed_need=0;int Turn_need=0;
  8. float speeds,speeds_filter, positions;
  9. float diff_speeds,diff_speeds_all;
  10. ////////////////////PID parameter///////////////////////////////////////
  11. double Output=0;
  12. float Kp=10,Kd=0.06,Ksp = 2.8,Ksi = 0.11;        //

  13. ////////////////////////////////////////////////
  14. MPU6050 accelgyro;
  15. int16_t ax, ay, az;
  16. int16_t gx, gy, gz;
  17. /********************角度参数**************/
  18. float Gyro_y;            //Y轴陀螺仪数据暂存
  19. float Angle_ax;          //由加速度计算的倾斜角度
  20. float Angle;             //小车最终倾斜角度
  21. //int Setpoint=0;
  22. ////////////////////////////////////////Pin assignment///////////////////////////////////////
  23. int PinA_right= 2; //interrupt 0
  24. int PinA_left= 3; //interrupt 1
  25. int E_left =6;//ENA
  26. int M_right =7;
  27. int M_right2=8;
  28. int E_right =5; //ENB
  29. int M_left =9;  
  30. int M_left2 =10;
  31. //////////////////////////////////////////////////////////////////////////////
  32. int PWM_right=0; int PWM_left=0;
  33. int PWM_left_least=87; int PWM_right_least=88;//left:77,right:78
  34. ///////////////////////////////interrupt for Speed/////////////////////////////////
  35. int count_right =0;
  36. int count_left  =0;
  37. int old_time=0;
  38. int flag;
  39. void Code_right(){  if(Output>=0){count_right += 1;}else{count_right -= 1;} }//if only +,can't stand up 编码器码盘计数加一
  40. void Code_left(){  if(Output>=0){count_left += 1;} else{count_left -= 1;}}// 编码器码盘计数加一   
  41. /////////////////////////Right&Left&Stop///////////////////////////////////////////////
  42. void SetMotorVoltage(int nLeftVol, int nRightVol) {
  43.     if(nLeftVol >=0)
  44.   {  digitalWrite(M_left,LOW);
  45.      digitalWrite(M_left2,HIGH);
  46.    } else {      
  47.      digitalWrite(M_left,HIGH);
  48.      digitalWrite(M_left2,LOW);
  49.      nLeftVol=-nLeftVol;
  50.    }
  51.     if(nRightVol >= 0) {
  52.      digitalWrite(M_right,LOW);
  53.      digitalWrite(M_right2,HIGH);
  54.     } else {
  55.      digitalWrite(M_right,HIGH);
  56.      digitalWrite(M_right2,LOW);
  57.     nRightVol=-nRightVol;
  58.   }
  59.     if(nLeftVol>255) { nLeftVol = 255 ; }   //防止PWM值超过255
  60.     if(nRightVol>255) { nRightVol = 255 ; }//防止PWM值超过255
  61.     analogWrite(E_left,nLeftVol);
  62.     analogWrite(E_right,nRightVol);
  63. }

  64. /////////////////////////////////////////////////////////////////////////////////////////////
  65. void Angle_Calcu(void)         {
  66.         Angle_ax = (ax+1942)/238.2 ;   //去除零点偏移1942,//16384*3.14/1.2*180//弧度转换为度,
  67.               Gyro_y = -(gy-119.58)/16.4;         //去除零点偏移119,2000deg/s 16.4 LSB/(deg/s)250---131 ,负号为方向处理
  68.         //Serial.print("Angle_ax,Angle_gy");Serial.print(Angle_ax);Serial.println(Angle_gy);
  69.         Angle=0.97*(Angle+Gyro_y*0.0005)+0.03*Angle_ax;
  70.         //Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角                                                                                                          
  71. }   

  72. void setup()
  73. {
  74.     Wire.begin();
  75.     Serial.begin(9600);  
  76.    ///////////////////////////////////////////////////////////////////
  77.     accelgyro.reset();//reset
  78.     delay(1);
  79.     accelgyro.setClockSource(MPU6050_CLOCK_PLL_YGYRO);//PLL with Y Gyro reference*/
  80.     accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//0x1B Value 0x18 2000°/s
  81.     accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);//0x1C Value 0x18 16g
  82.     accelgyro.setDLPFMode(MPU6050_DLPF_BW_5);//0x06 means acc 5Hz delay19.0ms Gyro 5Hz delay 18.6ms
  83.     accelgyro.setTempSensorEnabled(true);//disable temsensor
  84.     accelgyro.setSleepEnabled(false);
  85.     Serial.println("Testing device connections...");
  86.     Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  87.    ////////////////////////pin mode////////////////////////////////////////
  88.    pinMode(E_right, OUTPUT); pinMode(M_right, OUTPUT);pinMode(M_right2, OUTPUT);  //right
  89.    pinMode(E_left, OUTPUT);  pinMode(M_left, OUTPUT);pinMode(M_left2, OUTPUT);  //left
  90.    pinMode(PinA_right,INPUT);pinMode(PinA_left,INPUT);// in 0 in 1
  91.    Serial.println("Pin mode ...");
  92.    /////////////////////////////interrupt/////////////////////////////////////////////
  93.    attachInterrupt(0, Code_right, FALLING);attachInterrupt(1, Code_left, FALLING);
  94.    
  95. }

  96. void loop()
  97. {
  98.    if (Serial.available() > 0){val = Serial.read();Serial.println(val);}
  99.      switch(val){
  100.      case 'a':Speed_need=30;Turn_need=0;positions=80;break;//Go
  101.      case 'b':Speed_need=10;Turn_need=-10;positions=10;break;//right
  102.      case 'c':Speed_need=10;Turn_need=10;positions=10;break;//left
  103.      case 'd':Speed_need=0;Turn_need=0;positions=0;break;
  104.      default:Speed_need=0;Turn_need=0;positions=0;break;}//stop
  105.    
  106.      //Speed_need=30;Turn_need=0;positions=80;  
  107.      //SetMotorVoltage(255,255);
  108.     //Kp=15,Kd=0.09,Ksp = 2.8,Ksi = 0.11;
  109.          //Serial.print("count_left");Serial.println(count_left);
  110.     accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  111.     Angle_Calcu();
  112.     //Serial.print("Angle");Serial.println(Angle);
  113.     PWM_Calcu();
  114.   
  115.    //if(millis()-old_time>=500){ Serial.print("count_right");Serial.print(count_right);Serial.print("count_left");Serial.println(count_left);old_time=millis();count_right=0;count_left=0;}     
  116. }
  117. void PWM_Calcu(void)
  118. {
  119.   if (abs(Angle)>40)
  120.     {SetMotorVoltage(0,0);}//角度大于45度 停止PWM输出
  121.     else
  122.     { //Speed_need=30;Turn_need=0;positions=80;
  123.       //////////////////////
  124.       speeds=(count_left + count_right)*0.5;
  125.       diff_speeds = count_left - count_right;
  126.       diff_speeds_all += diff_speeds;
  127.       speeds_filter *=0.85;  //一阶互补滤波
  128.       speeds_filter +=speeds*0.15;
  129.       positions += speeds_filter;
  130.       positions += Speed_need;
  131.       positions = constrain(positions, -2300, 2300);//抗积分饱和
  132.       ////////////////////
  133.       Output=Kp*Angle+Kd*Gyro_y+ Ksp*speeds_filter + Ksi*positions ;
  134.       //Serial.print("Output");Serial.println(Output);
  135.       if(Turn_need==0){PWM_right=Output-diff_speeds_all;
  136.       PWM_left=Output+diff_speeds_all;}
  137.       
  138.       PWM_right=Output+Turn_need;
  139.       PWM_left=Output-Turn_need;
  140.       
  141.       if(PWM_right>=0){PWM_right+=PWM_right_least;}else{PWM_right-=PWM_right_least;}
  142.       if(PWM_left>=0){PWM_left+=PWM_left_least;}else{PWM_left-=PWM_left_least;}

  143.       SetMotorVoltage(PWM_left,PWM_right);}
  144.        count_left = 0;
  145.   count_right = 0;
  146. }

复制代码
//////////////////////////////////////////////////////////
五、吃水不忘挖井人,以下为我的参考文章,前辈们还是要非常尊敬的。
1.Processing 图形工具在线调试PID参数教程。http://blog.sina.com.cn/s/blog_69bcf45201019bp8.html
2.PVCBOT【9号】忐忑者·自平衡双轮小车。http://blog.163.com/pvc_robot/blog/static/17527643220113334818803/
3.基于Arduino+MPU6050+Tp-link 703n平衡小车。http://www.geek-workshop.com/thread-8991-1-1.html
4.虚拟NXT的NXTway-GS自行平衡两轮机器人教程 http://bbs.cmnxt.com/thread-7697-1-1.html
5.第二个Arduino小车 两轮自平衡(Arduino PID方式)。http://www.geek-workshop.com/thread-467-1-1.html
6.碉堡的双轮平衡小车(萧大哥) http://www.geek-workshop.com/thread-9005-1-1.html
7.总算实现了 PID 调速(重点推荐)http://www.embedream.com/ndxs/2011-04-23/121.html

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复

使用道具 举报

发表于 2014-5-23 14:40:26 | 显示全部楼层
真羨慕....我買回來已經幾個星期, 最忙沒時間攪, 還未解決 電機開動時 mpu6050 不穩定的問題, 單是站立也不行.

想請教樓主, 你的電路是如何連接的?  是否全部共地的?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-23 14:51:42 | 显示全部楼层
Super169 发表于 2014-5-23 14:40
真羨慕....我買回來已經幾個星期, 最忙沒時間攪, 還未解決 電機開動時 mpu6050 不穩定的問題, 單是站立也不 ...

是全部公地,
回复 支持 反对

使用道具 举报

发表于 2014-5-23 16:07:29 | 显示全部楼层
很棒的资源,收藏!很棒的资源,收藏!
回复 支持 反对

使用道具 举报

发表于 2014-5-24 01:42:31 | 显示全部楼层
我想单独看你的6050模块!
回复 支持 反对

使用道具 举报

发表于 2014-5-24 01:45:06 | 显示全部楼层
还有还有,这个PID_v1.h库求共享!
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-24 08:11:08 | 显示全部楼层
wetnt 发表于 2014-5-23 16:07
很棒的资源,收藏!很棒的资源,收藏!


谢谢!
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-24 08:14:16 | 显示全部楼层
tgyfish 发表于 2014-5-24 01:45
还有还有,这个PID_v1.h库求共享!



PS:这个也可以在官网Arduino.cc上找到

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-24 08:15:34 | 显示全部楼层
若天寒 发表于 2014-5-23 21:25
楼主上面是完整程序吗??之前做过一个,不是很好

这个是我第一版能站起来的程式。可以发现,里面很多被Mark掉了。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-24 08:57:17 | 显示全部楼层
tgyfish 发表于 2014-5-24 01:42
我想单独看你的6050模块!


附件为MP6050标准库,example中有两种方式:
1.普通方式
2.DMP方式,此方式需要占用中断资源

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

发表于 2014-5-24 09:10:36 | 显示全部楼层
感谢楼主,收藏待用
回复 支持 反对

使用道具 举报

发表于 2014-5-24 15:49:08 | 显示全部楼层
smxyk123 发表于 2014-5-24 08:57
附件为MP6050标准库,example中有两种方式:
1.普通方式
2.DMP方式,此方式需要占用中断资源

我就这么没节操的收下了,感谢楼主!
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-24 16:34:19 | 显示全部楼层
tgyfish 发表于 2014-5-24 15:49
我就这么没节操的收下了,感谢楼主!

No thanks!
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-24 16:40:08 | 显示全部楼层
若天寒 发表于 2014-5-24 11:05
楼主:Processing 图形工具在线调试PID参数教程我看了之后又疑问,请教一下,我在第十步的时候出现了问题, ...

com 口通讯一定是一个口,如下图黄色部分是设置的地方,你可以调试调试。

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-24 16:44:54 | 显示全部楼层
若天寒 发表于 2014-5-24 10:08
我做过第一个不成功,想请教楼主角度是如何得出的


请看上图,我的接线方式为,Y轴与双轮电机轴平行,X轴与之垂直。Z轴为重力方向。
角度很简单得出:
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Angle_ax = (ax+1942)/238.2 ;   //去除零点偏移1942,//16384*3.14/1.2*180//弧度转换为度,
Gyro_y = -(gy-119.58)/16.4;         //去除零点偏移119,2000deg/s 16.4 LSB/(deg/s)250---131 ,负号为方向处理
//Serial.print("Angle_ax,Angle_gy");Serial.print(Angle_ax);Serial.println(Angle_gy);
Angle=0.97*(Angle+Gyro_y*0.0005)+0.03*Angle_ax;

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-19 17:57 , Processed in 0.056443 second(s), 22 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表