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

Gavinlv

mark，回去慢慢看。

iohongwal

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. int Pwm_out = 0;
   
9. int Turn_Need = 0;
   
10. int Speed_Need = 0;
    
11. int speed_output_R , speed_output_L;
    
12. int count_l=13;
    
13. int count_r=8;
    
14. int count_left;
    
15. int count_right;
    
16. float speeds , speeds_filter, positions;
    
17. float diff_speeds,diff_speeds_all;
    
18. float a[3],w[3],Angle[3],T;
    
19. short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
    
20. float Kap = 24.5;       //
    
21. float Kad = 10;         //
    
22. float Ksp = 2.8;        //
    
23. float Ksi = 0.11;       //
    
24. void setup()
    
25. {
    
26.   Serial.begin(115200);
    
27.   pinMode(5,OUTPUT);
    
28.   pinMode(6,OUTPUT);
    
29.   pinMode(9,OUTPUT);
    
30.   pinMode(10,OUTPUT);
    
31.   analogWrite(5,0);
    
32.   analogWrite(6,0);
    
33.   analogWrite(9,0);
    
34.   analogWrite(10,0);
    
35. }
    
36. void Motor(float v1,float v2)
    
37. {
    
38.   if (v1>255){v1=255;analogWrite(M11,0);analogWrite(M12,v1);}
    
39.   else if (v1>0) {analogWrite(M11,0);analogWrite(M12,v1);}
    
40.   else if (v1>-255) {analogWrite(M12,0);analogWrite(M11,-v1);}
    
41.   else  {v1=-255;analogWrite(M12,0);analogWrite(M11,-v1);}
    
42.   
    
43. if (v2>255){v2=255;analogWrite(M21,0);analogWrite(M22,v2);}
    
44.   else if (v2>0) {analogWrite(M21,0);analogWrite(M22,v2);}
    
45.   else if (v2>-255) {analogWrite(M22,0);analogWrite(M21,-v2);}
    
46.   else {v2=-255;analogWrite(M22,0);analogWrite(M21,-v2);}  
    
47. }
    
48. void loop()
    
49. {
    
50. }
    
51. 
    
52. 
    
53. void PID_PWM()
    
54. {
    
55.   speeds=(count_left + count_right)*0.5;
    
56. 
    
57.   diff_speeds = count_left - count_right;
    
58.   diff_speeds_all += diff_speeds;
    
59. 
    
60.   speeds_filter *=0.85;  //一阶互补滤波
    
61.   speeds_filter +=speeds*0.15;
    
62. 
    
63.   positions += speeds_filter;
    
64.   positions += Speed_Need;
    
65.   positions = constrain(positions, -2300, 2300);//抗积分饱和
    
66. 
    
67.   Pwm_out = Kap*Angle[0] + Kad*w[0] + Ksp*speeds_filter + Ksi*positions ;  //PID控制器
    
68.   if(Turn_Need == 0)
    
69.   {
    
70.     speed_output_R = int(Pwm_out - diff_speeds_all);
    
71.     speed_output_L = int(Pwm_out + diff_speeds_all);
    
72.   }
    
73.   speed_output_R = int(Pwm_out + Turn_Need);
    
74.   speed_output_L = int(Pwm_out - Turn_Need);
    
75.   Motor(speed_output_L , speed_output_R);
    
76.   count_left = 0;
    
77.   count_right = 0;
    
78. 
    
79.    Serial.print(Angle[0]);Serial.print(" ");
    
80.   Serial.print(w[0]);Serial.print(" ");
    
81.   Serial.println(Pwm_out);Serial.print(" ");
    
82. }
    
83. void serialEvent() {
    
84.   while (Serial.available()) {
    
85.    
    
86.     //char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code
    
87.   
    
88.     Re_buf[counter]=(unsigned char)Serial.read();
    
89.     if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头              
    
90.     counter++;      
    
91.     if(counter==11)             //接收到11个数据
    
92.     {   
    
93.        counter=0;               //重新赋值，准备下一帧数据的接收
    
94.        switch(Re_buf [1])
    
95.         {
    
96.         case 0x51:
    
97.                 a[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*16;
    
98.                 a[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*16;
    
99.                 a[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*16;               
    
100.                 break;
     
101.         case 0x52:
     
102.                 w[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*250;
     
103.                 w[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*250;
     
104.                 w[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*250;
     
105.                 break;
     
106.         case 0x53:
     
107.                 Angle[0] =  float(short(Re_buf [3]<<8| Re_buf [2]))/32768*180;
     
108.                 Angle[1] =  float(short(Re_buf [5]<<8| Re_buf [4]))/32768*180;
     
109.                 Angle[2] =  float(short(Re_buf [7]<<8| Re_buf [6]))/32768*180;
     
110.                 T =  float(short(Re_buf [9]<<8| Re_buf [8]));///340.0+36.25   
     
111.                 sign=1;
     
112.                 break;
     
113.         }
     
114.     }      
     
115.   }
     
116. }
     
117. 
     
118. 
     

复制代码

這我是用你的PID改 幫我看看有冇甚麼問題
因為呢我寫進arduino 走不動 和讀不到資料

Santiago

楼主可不可以发个买电机的链接啊

shadan0416

LZ 电机是采用步进的摸  有链接木有

SAGA

技术贴！顶！

Lrnxis

想问下如何通过tx和rx实现703N和arduino的通信呢？

春日迟迟n

很好！

McZHA

手机怎么接收路由的串口数据

二十一题选C

不知道为什么站不起来。。

smallfivecn

每个电机测速有A、B相两路信号，直接一路还是接两路呢？接两路可以测正反向，更准确呀！

阿布都

有完整的代码吗？

wininw

为什么我的电机需要三个接口才能用pwm控制速度呢？

dltwcy

学习了，刚开始玩arduino

月夜

跟着楼主买了一样的串口MPU6050,  求楼主放出完整代码啊

月夜

楼主MPU6050里面的代码温度那里有点问题:

1. T =  float(short(Re_buf [9]<<8| Re_buf [8]));///340.0+36.25

复制代码

而那个卖模块给出的代码是:

1. T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;

复制代码