平衡小车轮子只会往一个方向转

Nro

各位大神，我用arduino nano+mpu6050+l298n做一个平衡小车，结果小车的轮子只会往一个方向转，这是什么原因？

1. #include "PID_v1.h"
   
2. #include "Wire.h"
   
3. #include "I2Cdev.h"
   
4. #include "MPU6050_6Axis_MotionApps20.h"
   
5. MPU6050 mpu(0x68);
   
6. 
   
7. #define center  0x7F
   
8. 
   
9. char flag=0;
   
10. char num=0;
    
11. double time;
    
12. signed int speeds = 0;
    
13. signed int oldspeed =0;
    
14. byte inByte ;
    
15. // MPU control/status vars
    
16. bool dmpReady = false;   
    
17. uint8_t mpuIntStatus;
    
18. uint8_t devStatus;   
    
19. uint16_t packetSize;   
    
20. uint16_t fifoCount;   
    
21. uint8_t fifoBuffer[64];  
    
22. 
    
23. signed int speedcount=0;
    
24.   
    
25. // orientation/motion vars
    
26. Quaternion q;           // [w, x, y, z]         quaternion container
    
27. VectorFloat gravity;    // [x, y, z]            gravity vector
    
28. float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
    
29. float angle;
    
30. double Setpoint, Input, Output;
    
31. double kp = 21,ki = 450,kd = 0.8;//需要你修改的参数
    
32. 
    
33. double Setpoints, Inputs, Outputs;
    
34. double sp = 0.8,si = 0.8,sd = 0.55;//需要你修改的参数
    
35. 
    
36. unsigned char dl=17,count;
    
37. 
    
38. union{
    
39.         signed int all;
    
40.         unsigned char s[2];
    
41. }data;
    
42.    
    
43. 
    
44. volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
    
45. void dmpDataReady() {
    
46.   mpuInterrupt = true;
    
47. }
    
48. PID myPID(&Input, &Output, &Setpoint,kp,ki,kd, DIRECT);
    
49. 
    
50. PID sPID(&Inputs, &Outputs, &Setpoints,sp,si,sd, DIRECT);
    
51. 
    
52. void motor(int v)
    
53. {
    
54.   if(v>0)
    
55.   {
    
56.     v+=dl;
    
57.     digitalWrite(6,0);
    
58.     digitalWrite(7,1);
    
59.     digitalWrite(8,1);
    
60.     digitalWrite(9,0);   
    
61.     analogWrite(10,v);
    
62.     analogWrite(11,v);
    
63.   }
    
64.   else if(v<0)
    
65.   {
    
66.     v=-v;
    
67.     v+=dl;
    
68.     digitalWrite(6,1);
    
69.     digitalWrite(7,0);
    
70.     digitalWrite(8,0);
    
71.     digitalWrite(9,1);   
    
72.     analogWrite(10,v);
    
73.     analogWrite(11,v);
    
74.   }
    
75.   else
    
76.   {
    
77.     analogWrite(10,0);
    
78.     analogWrite(11,0);  
    
79.   }
    
80. }
    
81. 
    
82. void motort(int v)
    
83. {
    
84.   if(v>0)
    
85.   {
    
86.     v+=dl;
    
87.     digitalWrite(8,1);
    
88.     digitalWrite(9,0);
    
89.     analogWrite(10,v);
    
90.   }
    
91.   else if(v<0)
    
92.   {
    
93.     v=-v;
    
94.     v+=dl;
    
95.     digitalWrite(8,0);
    
96.     digitalWrite(9,1);  
    
97.     analogWrite(10,v);
    
98.   }
    
99.   else
    
100.   {
     
101.     analogWrite(10,0);
     
102.   }
     
103. }
     
104. 
     
105. void setup()
     
106. {
     
107.   pinMode(6,OUTPUT);
     
108.   pinMode(7,OUTPUT);
     
109.   pinMode(8,OUTPUT);
     
110.   pinMode(9,OUTPUT);
     
111.   pinMode(10,OUTPUT);
     
112.   pinMode(11,OUTPUT);
     
113.   digitalWrite(6,0);
     
114.   digitalWrite(7,1);
     
115.   digitalWrite(8,1);
     
116.   digitalWrite(9,0);
     
117.   analogWrite(10,0);
     
118.   analogWrite(11,0);
     
119.   Serial.begin(115200);
     
120.   Wire.begin();
     
121. 
     
122.   delay(100);
     
123.    
     
124.   Serial.println("Initializing I2C devices...");
     
125.   mpu.initialize();
     
126.   Serial.println("Testing device connections...");
     
127.   Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
     
128.   delay(2);
     
129.   Serial.println("Initializing DMP...");
     
130.   devStatus = mpu.dmpInitialize();
     
131.   if (devStatus == 0)
     
132.   {
     
133.     Serial.println("Enabling DMP...");
     
134.     mpu.setDMPEnabled(true);
     
135.     Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");
     
136.     attachInterrupt(0, dmpDataReady, RISING);
     
137.     mpuIntStatus = mpu.getIntStatus();
     
138.     Serial.println("DMP ready! Waiting for first interrupt...");
     
139.     dmpReady = true;
     
140.     packetSize = mpu.dmpGetFIFOPacketSize();
     
141.   }
     
142.   else
     
143.   {
     
144.     Serial.print("DMP Initialization failed (code ");
     
145.     Serial.print(devStatus);
     
146.     Serial.println(")");
     
147.   }
     
148.    
     
149.   Setpoint = -10.2;
     
150.   myPID.SetTunings(kp,ki,kd);
     
151.   myPID.SetOutputLimits(-255+dl,255-dl);
     
152.   myPID.SetSampleTime(5);
     
153.   myPID.SetMode(AUTOMATIC);
     
154. 
     
155.   sPID.SetTunings(sp,si,sd);
     
156.   sPID.SetOutputLimits(-10,70);
     
157.   sPID.SetSampleTime(200);
     
158.   sPID.SetMode(AUTOMATIC);
     
159. 
     
160. 
     
161.   attachInterrupt(1,speed,RISING);
     
162. }
     
163.   
     
164. void loop()
     
165. {
     
166.   if (!dmpReady)
     
167.     return;
     
168.   // wait for MPU interrupt or extra packet(s) available
     
169.   if (!mpuInterrupt && fifoCount < packetSize)
     
170.     return;
     
171.   mpuInterrupt = false;
     
172.   mpuIntStatus = mpu.getIntStatus();
     
173.   fifoCount = mpu.getFIFOCount();
     
174.   if ((mpuIntStatus & 0x10) || fifoCount == 1024)
     
175.   {
     
176.    mpu.resetFIFO();
     
177.   }
     
178.   else if (mpuIntStatus & 0x02)
     
179.   {
     
180.      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
     
181.      mpu.getFIFOBytes(fifoBuffer, packetSize);
     
182.      fifoCount -= packetSize;
     
183.      mpu.dmpGetQuaternion(&q, fifoBuffer);
     
184.      mpu.dmpGetGravity(&gravity, &q);
     
185.      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);  //从DMP中取出Yaw、Pitch、Roll三个轴的角度，放入数组ypr。单位：弧度
     
186.      angle=-ypr[1] * 180/M_PI;
     
187.   }
     
188. 
     
189. 
     
190.   Inputs = speedcount;
     
191.    sPID.Compute();
     
192. 
     
193.     Setpoint = -10.2+Outputs;
     
194. 
     
195.   Input = angle;
     
196.   myPID.Compute();
     
197.    
     
198.   if(angle>60||angle<0)
     
199.   Output=0;
     
200.   if(flag)
     
201.   {
     
202.   motort(Output);
     
203.   flag=0;
     
204.   }
     
205.   else {
     
206.         motor(Output);
     
207.          
     
208.   }
     
209.    
     
210.   if (Serial.available() > 0) {
     
211.     inByte = Serial.read();
     
212.   }
     
213.   if(inByte == 'w'){
     
214.   kp+=0.5;}
     
215.   else if(inByte == 'q'){
     
216.   kp-=0.5;}
     
217.   else if(inByte == 'r'){
     
218.   ki+=10;}
     
219.   else if(inByte == 'e'){
     
220.   ki-=10;}
     
221.   else if(inByte == 'y'){
     
222.   kd+=0.01;}
     
223.   else if(inByte == 't'){
     
224.   kd-=0.01;}
     
225.   else if(inByte == 'i'){
     
226.   dl+=1;}
     
227.   else if(inByte == 'u'){
     
228.   dl-=1;}
     
229.   else if(inByte == 's'){
     
230.   sp+=0.1;}
     
231.   else if(inByte == 'a'){
     
232.   sp-=0.1;}
     
233.    else if(inByte == 'f'){
     
234.   si+=1;}
     
235.   else if(inByte == 'd'){
     
236.   si-=1;}
     
237.    else if(inByte == 'h'){
     
238.   sd+=0.01;}
     
239.   else if(inByte == 'g'){
     
240.   sd-=0.01;}
     
241.   else if(inByte == 'v'){
     
242.   Setpoints+=0.7;}
     
243.   else if(inByte == 'b'){
     
244.   Setpoints-=0.7;}
     
245.   else if(inByte == '1'){
     
246.   Setpoints+=0.2;}
     
247.   else if(inByte == '2'){
     
248.   Setpoints-=0.2;}
     
249.   else if(inByte == 'n'){
     
250.   Setpoints+=2;
     
251.   flag=1;}
     
252.   else if(inByte == 'm'){
     
253.   Setpoints-=2;
     
254.   flag=1;}
     
255.   
     
256.   inByte='x';
     
257.   
     
258.   sPID.SetTunings(sp,si,sd);  
     
259.   myPID.SetTunings(kp,ki,kd);  
     
260. 
     
261.   num++;
     
262.   if(num==20)
     
263.   {num=0;
     
264.   Serial.print(kp);
     
265.   Serial.print(',');
     
266.   Serial.print(ki);
     
267.   Serial.print(',');
     
268.   Serial.print(kd);
     
269.   Serial.print(',');
     
270.   Serial.print(dl);
     
271.   Serial.print("  ");
     
272.   Serial.print(sp);
     
273.   Serial.print(',');
     
274.   Serial.print(si);
     
275.   Serial.print(',');
     
276.   Serial.print(sd);
     
277.    Serial.print(',');
     
278.   Serial.println(angle);
     
279.   }
     
280. 
     
281. }
     
282. 
     
283. 
     
284. 
     
285. void speed()
     
286. {
     
287.         if(digitalRead(6)){
     
288.         speedcount+=1;
     
289.         }
     
290.         else
     
291.         speedcount-=1;
     
292. }

复制代码

suoma

控制两个车轮速度，它就会左转右转

Nro

suoma 发表于 2015-10-3 08:35
控制两个车轮速度，它就会左转右转

怎么控制？麻烦提示一下，我是超级新手

suoma

参考http://bbs.ickey.cn/group-topic-id-53675.html