极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

楼主: It's_me

基于Arduino+MPU6050+Tp-link 703n平衡车完美站立(部分代码上传)

  [复制链接]
发表于 2014-4-14 16:13:14 | 显示全部楼层
It's_me 发表于 2014-3-31 20:22
你具体说说,看我能帮你什么忙

请问怎样区别mpu6050是否已卡尔曼滤波?我的mpu6050和楼主你发的以一个图片类似。
回复 支持 反对

使用道具 举报

发表于 2014-4-14 19:16:08 | 显示全部楼层
对于MPU6050很迷茫。
回复 支持 反对

使用道具 举报

发表于 2014-4-14 20:47:03 | 显示全部楼层
It's_me 发表于 2014-3-31 20:22
你具体说说,看我能帮你什么忙

我的车电子扭力不够,所以可调角度很小,最近一直在弄卡尔曼滤波,希望能把波形弄平滑些。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-4-15 11:08:56 | 显示全部楼层
浅墨飞鱼 发表于 2014-4-14 20:47
我的车电子扭力不够,所以可调角度很小,最近一直在弄卡尔曼滤波,希望能把波形弄平滑些。

你只用了角度环吗,只用一个角度环是不行的
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-4-15 11:10:07 | 显示全部楼层
RedFISH 发表于 2014-4-14 19:16
对于MPU6050很迷茫。

MPU6050很简单啊,你只需要得到很好的数据就行,其他的你不需要管的
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-4-15 11:19:03 | 显示全部楼层
蓝调葬礼 发表于 2014-4-6 09:05
楼主  用arduino  UNO 的话    I/O脚够用吗    谢谢

uno怕是不行,中断就不够,串口也不够
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-4-15 11:21:38 | 显示全部楼层
iohongwal 发表于 2014-4-13 21:16
我自己了一個平衡車 我是用芯片厰家提拱的代碼來做的 車子可以根劇angle來向前走和向後走 但是不平衡 可以幫 ...

看一下你的电机是怎么安装的,还有你的算法是怎么写的
回复 支持 反对

使用道具 举报

发表于 2014-4-15 11:30:45 | 显示全部楼层
It's_me 发表于 2014-4-15 11:21
看一下你的电机是怎么安装的,还有你的算法是怎么写的

是要我拍照給你看?
回复 支持 反对

使用道具 举报

发表于 2014-4-15 12:45:36 | 显示全部楼层
電機安裝
10156889_594062574022199_765126926_n.jpg

厰家給的代碼

  1. unsigned char Re_buf[11],counter=0;
  2. unsigned char sign=0;
  3. int M11=5;
  4. int M12=6;
  5. int M21=9;
  6. int M22=10;

  7. float a[3],w[3],Angle[3],T;
  8. short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
  9. void setup() {
  10.   // initialize serial:
  11.   Serial.begin(115200);
  12.   pinMode(M11,OUTPUT);analogWrite(M11,0);
  13.   pinMode(M12,OUTPUT);analogWrite(M12,0);
  14.   pinMode(M21,OUTPUT);analogWrite(M21,0);
  15.   pinMode(M22,OUTPUT);analogWrite(M22,0);
  16. }
  17. void SetMotor(float v1,float v2)
  18. {
  19.   if (v1>255){v1=255;analogWrite(M11,0);analogWrite(M12,v1);}
  20.   else if (v1>0) {analogWrite(M11,0);analogWrite(M12,v1);}
  21.   else if (v1>-255) {analogWrite(M12,0);analogWrite(M11,-v1);}
  22.   else  {v1=-255;analogWrite(M12,0);analogWrite(M11,-v1);}
  23.   
  24. if (v2>255){v2=255;analogWrite(M21,0);analogWrite(M22,v2);}
  25.   else if (v2>0) {analogWrite(M21,0);analogWrite(M22,v2);}
  26.   else if (v2>-255) {analogWrite(M22,0);analogWrite(M21,-v2);}
  27.   else {v2=-255;analogWrite(M22,0);analogWrite(M21,-v2);}  
  28. }

  29. float PID1(float e,float kp,float ki,float kd)
  30. {
  31.   static float es=0,sum=0;
  32.   float r;
  33.   sum+=e;
  34.   r = kp*e+ki*sum+kd*(e-es);
  35.   es=e;
  36.   return r;  
  37. }
  38. float PID2(float e,float kp,float ki,float kd)
  39. {
  40.   static float es=0,sum=0;
  41.   float r;
  42.   sum+=e;
  43.   r = kp*e+ki*sum+kd*(e-es);
  44.   es=e;
  45.   return r;  
  46. }
  47. void loop() {
  48.   float kp=30,ki=0.0,kd=10,r1,r2;
  49.   if (sign==0) return;//sign为数据更新标志,每隔10ms更新一次,也就是说以下代码每隔10ms控制一次。
  50.   sign=0;
  51.   kd = (float)analogRead(0)/1024*200;
  52.   r1=PID1(Angle[0],kp,ki,kd);//PID1、PID2函数就是第四节的PID函数,为了区分左右轮,所以分成两个。
  53.   r2=PID2(Angle[0],kp,ki,kd);
  54.   SetMotor(r1,r2);//设置电机转速。
  55.   Serial.print("angle:");
  56.   Serial.print(Angle[0]);Serial.print(" ");
  57.   Serial.print(r1);Serial.print(" ");
  58.   Serial.println(kd);Serial.print(" ");
  59. }

  60. void serialEvent() {
  61.   while (Serial.available()) {
  62.    
  63.     //char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code
  64.   
  65.     Re_buf[counter]=(unsigned char)Serial.read();
  66.     if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头              
  67.     counter++;      
  68.     if(counter==11)             //接收到11个数据
  69.     {   
  70.        counter=0;               //重新赋值,准备下一帧数据的接收
  71.        switch(Re_buf [1])
  72.         {
  73.         case 0x51:
  74.                 a[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*16;
  75.                 a[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*16;
  76.                 a[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*16;               
  77.                 break;
  78.         case 0x52:
  79.                 w[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*250;
  80.                 w[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*250;
  81.                 w[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*250;
  82.                 break;
  83.         case 0x53:
  84.                 Angle[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*180;
  85.                 Angle[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*180;
  86.                 Angle[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*180;
  87.                 T =  float(short(Re_buf [9]<<8| Re_buf [8]));///340.0+36.25   
  88.                 sign=1;
  89.                 break;
  90.         }
  91.     }      
  92.   }
  93. }
复制代码


以下是我自己用PID library 寫出來的

  1. #include <PID_v1.h>

  2. //Define Variables we'll be connecting to
  3. double Setpoint, Input, Output;

  4. //Specify the links and initial tuning parameters
  5. PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
  6. unsigned char Re_buf[11],counter=0;
  7. unsigned char sign=0;
  8. int M11=5;
  9. int M12=6;
  10. int M21=9;
  11. int M22=10;

  12. float a[3],w[3],Angle[3],T;
  13. short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
  14. void setup() {
  15.   // initialize serial:
  16.   Serial.begin(115200);
  17.   pinMode(M11,OUTPUT);
  18.   pinMode(M12,OUTPUT);
  19.   pinMode(M21,OUTPUT);
  20.   pinMode(M22,OUTPUT);
  21.    Input = Angle[0];
  22.   Setpoint = 0;
  23.   myPID.SetMode(AUTOMATIC);
  24.   myPID.SetOutputLimits(-255,255);


  25. }
  26. void loop(){
  27.   Input = Angle[0];
  28.   myPID.Compute();
  29.   if(Angle[0]>0){analogWrite(M11,Output);analogWrite(M12,0);analogWrite(M21,Output);analogWrite(M22,0);}
  30.   else if(Angle[0]<0){analogWrite(M11,0);analogWrite(M12,-Output);analogWrite(M21,0);analogWrite(M22,-Output);}



  31.   Serial.print("angle:");
  32.   Serial.print(Angle[0]);Serial.print(" "); Serial.print(Output);Serial.println(" ");
  33. }
  34.   void serialEvent() {
  35.   while (Serial.available()) {
  36.    
  37.     //char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code
  38.   
  39.     Re_buf[counter]=(unsigned char)Serial.read();
  40.     if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头              
  41.     counter++;      
  42.     if(counter==11)             //接收到11个数据
  43.     {   
  44.        counter=0;               //重新赋值,准备下一帧数据的接收
  45.        switch(Re_buf [1])
  46.         {
  47.         case 0x51:
  48.                 a[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*16;
  49.                 a[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*16;
  50.                 a[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*16;               
  51.                 break;
  52.         case 0x52:
  53.                 w[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*250;
  54.                 w[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*250;
  55.                 w[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*250;
  56.                 break;
  57.         case 0x53:
  58.                 Angle[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*180;
  59.                 Angle[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*180;
  60.                 Angle[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*180;
  61.                 T =  float(short(Re_buf [9]<<8| Re_buf [8]));///340.0+36.25   
  62.                 sign=1;
  63.                 break;
  64.         }
  65.     }      
  66.   }
  67. }



复制代码
回复 支持 反对

使用道具 举报

发表于 2014-4-15 15:11:23 | 显示全部楼层
It's_me 发表于 2014-4-15 11:08
你只用了角度环吗,只用一个角度环是不行的

要加入速度环对吧?类似于这个网页说的http://www.geology.smu.edu/~dpa-www/robo/nbot/bal2.txt 但是具体比例大小还不晓得如何下手。
回复 支持 反对

使用道具 举报

发表于 2014-4-15 16:34:42 | 显示全部楼层
positions表示位置,是速度对时间的积分。这句话在程序中体现不出来啊
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-4-15 21:50:59 | 显示全部楼层
iohongwal 发表于 2014-4-15 12:45
電機安裝

pid还是自己写吧,pid库看不大懂,我不建议你用,pid一定要双闭环,一个角度环不可能调好的。你的电机减速比是多少,扭矩够吗?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-4-15 21:54:23 | 显示全部楼层
浅墨飞鱼 发表于 2014-4-15 16:34
positions表示位置,是速度对时间的积分。这句话在程序中体现不出来啊

怎么体现不出来啊,很明显是速度滤波后,积分就可以得到位置的啊,这个就是我现在用的pid程序,你可以借鉴一下。
回复 支持 反对

使用道具 举报

发表于 2014-4-15 23:27:38 | 显示全部楼层
本帖最后由 iohongwal 于 2014-4-15 23:52 编辑
It's_me 发表于 2014-4-15 21:50
pid还是自己写吧,pid库看不大懂,我不建议你用,pid一定要双闭环,一个角度环不可能调好的。你的电机减速 ...


電機的問題我不太清楚(不過我覺得不夠) 但我不是會自己寫PID  電機一定要有編碼器?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-4-16 16:27:40 | 显示全部楼层
iohongwal 发表于 2014-4-15 23:27
電機的問題我不太清楚(不過我覺得不夠) 但我不是會自己寫PID  電機一定要有編碼器?

我发的这一段就是pid程序,你回去改改就行
回复 支持 反对

使用道具 举报

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

本版积分规则

Archiver|联系我们|极客工坊 ( 浙ICP备09023225号 )

GMT+8, 2020-7-4 01:46 , Processed in 0.055594 second(s), 25 queries .

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

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