平衡車求助

iohongwal

我用了這段代碼 當車車向后傾時要向前走 但向前傾時不會向后走

求大神們幫幫助 我是用arduino uno v3 加6050

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. }
    

复制代码