平衡車求助!!

iohongwal

我自己了一個平衡車 我是用芯片厰家提拱的代碼來做的 車子可以根劇angle來向前走和向後走 但是不平衡 各位大神幫我看看是車子的結構問題 還是甚麼問題 圖片不夠的可以叫我上傳  電機我是用田宮的70093 齒輪組是用高速的
這是我用的芯片


車子

1. 
   
2. unsigned char Re_buf[11],counter=0;
   
3. unsigned char sign=0;
   
4. int M11=5;
   
5. int M12=6;
   
6. int M21=9;
   
7. int M22=10;
   
8. 
   
9. float a[3],w[3],Angle[3],T;
   
10. short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
    
11. void setup() {
    
12.   // initialize serial:
    
13.   Serial.begin(115200);
    
14.   pinMode(M11,OUTPUT);analogWrite(M11,0);
    
15.   pinMode(M12,OUTPUT);analogWrite(M12,0);
    
16.   pinMode(M21,OUTPUT);analogWrite(M21,0);
    
17.   pinMode(M22,OUTPUT);analogWrite(M22,0);
    
18. }
    
19. void SetMotor(float v1,float v2)
    
20. {
    
21.   if (v1>255){v1=255;analogWrite(M11,0);analogWrite(M12,v1);}
    
22.   else if (v1>0) {analogWrite(M11,0);analogWrite(M12,v1);}
    
23.   else if (v1>-255) {analogWrite(M12,0);analogWrite(M11,-v1);}
    
24.   else  {v1=-255;analogWrite(M12,0);analogWrite(M11,-v1);}
    
25.   
    
26. if (v2>255){v2=255;analogWrite(M21,0);analogWrite(M22,v2);}
    
27.   else if (v2>0) {analogWrite(M21,0);analogWrite(M22,v2);}
    
28.   else if (v2>-255) {analogWrite(M22,0);analogWrite(M21,-v2);}
    
29.   else {v2=-255;analogWrite(M22,0);analogWrite(M21,-v2);}  
    
30. }
    
31. 
    
32. float PID1(float e,float kp,float ki,float kd)
    
33. {
    
34.   static float es=0,sum=0;
    
35.   float r;
    
36.   sum+=e;
    
37.   r = kp*e+ki*sum+kd*(e-es);
    
38.   es=e;
    
39.   return r;  
    
40. }
    
41. float PID2(float e,float kp,float ki,float kd)
    
42. {
    
43.   static float es=0,sum=0;
    
44.   float r;
    
45.   sum+=e;
    
46.   r = kp*e+ki*sum+kd*(e-es);
    
47.   es=e;
    
48.   return r;  
    
49. }
    
50. void loop() {
    
51.   float kp=30,ki=0.0,kd=10,r1,r2;
    
52.   if (sign==0) return;//sign为数据更新标志，每隔10ms更新一次，也就是说以下代码每隔10ms控制一次。
    
53.   sign=0;
    
54.   kd = (float)analogRead(0)/1024*200;
    
55.   r1=PID1(Angle[0],kp,ki,kd);//PID1、PID2函数就是第四节的PID函数，为了区分左右轮，所以分成两个。
    
56.   r2=PID2(Angle[0],kp,ki,kd);
    
57.   SetMotor(r1,r2);//设置电机转速。
    
58.   Serial.print("angle:");
    
59.   Serial.print(Angle[0]);Serial.print(" ");
    
60.   Serial.print(r1);Serial.print(" ");
    
61.   Serial.println(kd);Serial.print(" ");
    
62. }
    
63. 
    
64. void serialEvent() {
    
65.   while (Serial.available()) {
    
66.    
    
67.     //char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code
    
68.   
    
69.     Re_buf[counter]=(unsigned char)Serial.read();
    
70.     if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头              
    
71.     counter++;      
    
72.     if(counter==11)             //接收到11个数据
    
73.     {   
    
74.        counter=0;               //重新赋值，准备下一帧数据的接收
    
75.        switch(Re_buf [1])
    
76.         {
    
77.         case 0x51:
    
78.                 a[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*16;
    
79.                 a[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*16;
    
80.                 a[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*16;               
    
81.                 break;
    
82.         case 0x52:
    
83.                 w[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*250;
    
84.                 w[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*250;
    
85.                 w[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*250;
    
86.                 break;
    
87.         case 0x53:
    
88.                 Angle[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*180;
    
89.                 Angle[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*180;
    
90.                 Angle[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*180;
    
91.                 T =  float(short(Re_buf [9]<<8| Re_buf [8]));///340.0+36.25   
    
92.                 sign=1;
    
93.                 break;
    
94.         }
    
95.     }      
    
96.   }
    
97. }
    

复制代码

杨帅

代码问题，代码呢，看下？

iohongwal

杨帅 发表于 2014-4-13 14:57
代码问题，代码呢，看下？

已添加代碼

杨帅

iohongwal 发表于 2014-4-13 15:10
已添加代碼

朋友，你这个程序调的是PID，我没弄过，我只简单的弄过陀螺仪和加速度计。你帖子的第一张图片那个传感器叫什么名字是IMU吗？

iohongwal

杨帅 发表于 2014-4-13 18:21
朋友，你这个程序调的是PID，我没弄过，我只简单的弄过陀螺仪和加速度计。你帖子的第一张图片那个传感器叫 ...

6軸陀螺仪MPU6050

ranqingfa

你这车体， 有点奇葩

iohongwal

ranqingfa 发表于 2014-4-14 09:13
你这车体， 有点奇葩

其實我知道 但我不知怎樣設計好

捡贝壳的孩子

我现在也在用mpu6050做呢，只不过用的是stm32控制的，能互相交流学习下么

iohongwal

捡贝壳的孩子 发表于 2014-4-14 13:48
我现在也在用mpu6050做呢，只不过用的是stm32控制的，能互相交流学习下么

我不很懂PID算法那方面 我用了arduino pid library 但輸出只有0~255

ranqingfa

iohongwal 发表于 2014-4-14 12:41
其實我知道 但我不知怎樣設計好

你起码看看别人怎么设计的啊  成功的案例

iohongwal

ranqingfa 发表于 2014-4-14 22:25
你起码看看别人怎么设计的啊  成功的案例

我有看迥過 其實那輛車銅柱我還未裝完 其實高點是不是會更好

ranqingfa

iohongwal 发表于 2014-4-14 23:03
我有看迥過 其實那輛車銅柱我還未裝完 其實高點是不是會更好

最好是搞点，你的车轮距太宽

iohongwal

ranqingfa 发表于 2014-4-15 09:18
最好是搞点，你的车轮距太宽

重物放最上面嗎

ranqingfa

不是啊  车体结构改变

ranqingfa

iohongwal 发表于 2014-4-15 10:52
重物放最上面嗎

你看过我的车子么