求大神們 幫助解釋下 程碼的意思 我不是很懂

iohongwal

這是串口MPU6050 的PID 示範程式 我不是看得很懂

1. /*
   
2. This code is used for connecting arduino to serial mpu6050 module, and test in arduino uno R3 board.
   
3. connect map:
   
4. arduino   mpu6050 module
   
5. VCC    5v/3.3v
   
6. RX     TX->1
   
7. GND    GND
   
8. note:
   
9. because arduino download and mpu6050 are using the same serial port, you need to un-connect 6050 module when you want to download program to arduino.
   
10. Created 14 Nov 2013
    
11. by Zhaowen
    
12. 
    
13. serial mpu6050 module can be found in the link below:
    
14. http://item.taobao.com/item.htm?id=19785706431
    
15. */
    
16. 
    
17. unsigned char Re_buf[11],counter=0;
    
18. unsigned char sign=0;
    
19. int M11=5;
    
20. int M12=6;
    
21. int M21=9;
    
22. int M22=10;
    
23. 
    
24. float a[3],w[3],Angle[3],T;
    
25. short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
    
26. void setup() {
    
27.   // initialize serial:
    
28.   Serial.begin(115200);
    
29.   pinMode(M11,OUTPUT);analogWrite(M11,0);
    
30.   pinMode(M12,OUTPUT);analogWrite(M12,0);
    
31.   pinMode(M21,OUTPUT);analogWrite(M21,0);
    
32.   pinMode(M22,OUTPUT);analogWrite(M22,0);
    
33. }
    
34. void SetMotor(float v1,float v2)
    
35. {
    
36.   if (v1>255){v1=255;analogWrite(M11,0);analogWrite(M12,v1);}
    
37.   else if (v1>0) {analogWrite(M11,0);analogWrite(M12,v1);}
    
38.   else if (v1>-255) {analogWrite(M12,0);analogWrite(M11,-v1);}
    
39.   else  {v1=-255;analogWrite(M12,0);analogWrite(M11,-v1);}
    
40.   
    
41. if (v2>255){v2=255;analogWrite(M21,0);analogWrite(M22,v2);}
    
42.   else if (v2>0) {analogWrite(M21,0);analogWrite(M22,v2);}
    
43.   else if (v2>-255) {analogWrite(M22,0);analogWrite(M21,-v2);}
    
44.   else {v2=-255;analogWrite(M22,0);analogWrite(M21,-v2);}  
    
45. }
    
46. 
    
47. float PID1(float e,float kp,float ki,float kd)
    
48. {
    
49.   static float es=0,sum=0;
    
50.   float r;
    
51.   sum+=e;
    
52.   r = kp*e+ki*sum+kd*(e-es);
    
53.   es=e;
    
54.   return r;  
    
55. }
    
56. float PID2(float e,float kp,float ki,float kd)
    
57. {
    
58.   static float es=0,sum=0;
    
59.   float r;
    
60.   sum+=e;
    
61.   r = kp*e+ki*sum+kd*(e-es);
    
62.   es=e;
    
63.   return r;  
    
64. }
    
65. void loop() {
    
66.   float kp=30,ki=0.0,kd=10,r1,r2;
    
67.   if (sign==0) return;//sign为数据更新标志，每隔10ms更新一次，也就是说以下代码每隔10ms控制一次。
    
68.   sign=0;
    
69.   kd = (float)analogRead(0)/1024*200;
    
70.   r1=PID1(Angle[0],kp,ki,kd);//PID1、PID2函数就是第四节的PID函数，为了区分左右轮，所以分成两个。
    
71.   r2=PID2(Angle[0],kp,ki,kd);
    
72.   SetMotor(r1,r2);//设置电机转速。
    
73.   Serial.print("angle:");
    
74.   Serial.print(Angle[0]);Serial.print(" ");
    
75.   Serial.print(r1);Serial.print(" ");
    
76.   Serial.println(kd);Serial.print(" ");
    
77. }
    
78. 
    
79. void serialEvent() {
    
80.   while (Serial.available()) {
    
81.    
    
82.     //char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code
    
83.   
    
84.     Re_buf[counter]=(unsigned char)Serial.read();
    
85.     if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头              
    
86.     counter++;      
    
87.     if(counter==11)             //接收到11个数据
    
88.     {   
    
89.        counter=0;               //重新赋值，准备下一帧数据的接收
    
90.        switch(Re_buf [1])
    
91.         {
    
92.         case 0x51:
    
93.                 a[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*16;
    
94.                 a[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*16;
    
95.                 a[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*16;               
    
96.                 break;
    
97.         case 0x52:
    
98.                 w[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*250;
    
99.                 w[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*250;
    
100.                 w[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*250;
     
101.                 break;
     
102.         case 0x53:
     
103.                 Angle[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*180;
     
104.                 Angle[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*180;
     
105.                 Angle[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*180;
     
106.                 T =  float(short(Re_buf [9]<<8| Re_buf [8]));///340.0+36.25   
     
107.                 sign=1;
     
108.                 break;
     
109.         }
     
110.     }      
     
111.   }
     
112. }
     

复制代码