备份一下自己小车的程序

葱拌豆腐

正在调试，欢迎围观。O(∩_∩)O~

1. 
   
2. #include <I2Cdev.h>
   
3. #include <L298N.h>
   
4. #include <MPU6050.h>
   
5. #include <Wire.h>
   
6. #include <stdlib.h>
   
7. 
   
8. float ax,az,gy;//get acceleration_x,acceleration_z,rotation_y
   
9. long OffsetGy,OffsetSum;
   
10. float OffsetAccSumX,OffsetAccSumZ,OffsetAcc;
    
11. float AngleAcc,AngleRotation,AngleMerge;//angel computed by acceleration and rotation
    
12. float Kp,Kd,Ki,pid_SP,pid_CV;
    
13. float Err,ErrLast,ErrAfter;//PID
    
14. long LastTime,NowTime,TimeSpan,SampleTime,PIDLastTime;//
    
15. //===========kalman filter parameter define============
    
16. float A,B,H,Q,R,P00,P10,X00,X10,Z,U,Kg;
    
17. //==========add serial communication=========
    
18. String SerialString;
    
19. boolean mark=0;
    
20. char *comdata;
    
21. 
    
22. bool blinkState=false;
    
23. L298N l298n;
    
24. MPU6050 mpu;
    
25. 
    
26. 
    
27. void setup()
    
28. {
    
29.   //============initial kalman parameter======
    
30.   A=1.0;
    
31.   H=1.0;
    
32.   Q=0.003;
    
33.   R=0.01;
    
34.   P00=5.0;
    
35.   P10=0.0;
    
36.   X00=0.0;
    
37.   //===============end initial===========
    
38.   Wire.begin();
    
39.   Serial.begin(9600);
    
40.   Serial.println("start");
    
41.   mpu.initialize();
    
42.   Serial.println(mpu.getRotationY());
    
43.   Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
    
44.   //compute the gyroscope offset automatic
    
45.   for(int i=0;i<200;i++)
    
46.   {
    
47.     Serial.println(mpu.getRotationY());
    
48.     OffsetAccSumX+=mpu.getAccelerationX()/16384.00;
    
49.     OffsetAccSumZ+=mpu.getAccelerationZ()/16384.00;
    
50.     OffsetSum=OffsetSum+mpu.getRotationY();
    
51.     delay(20);
    
52.    }
    
53.   OffsetAcc=(-1.0)*atan2(OffsetAccSumX/200.0,OffsetAccSumZ/200.0)*180/PI;
    
54.   OffsetGy=OffsetSum/200;
    
55.   pid_SP=0.0;
    
56.   Kp=50.0;
    
57.   Ki=10.0;
    
58.   Kd=0.0;
    
59. }
    
60. 
    
61. void loop()
    
62. {
    
63.   
    
64.   NowTime=millis();
    
65.   TimeSpan=NowTime-LastTime;
    
66.   LastTime=NowTime;
    
67.   Serial.flush();
    
68.   //==============add serial communication=======
    
69.   while(Serial.available()>0)
    
70.   {
    
71.     SerialString += char(Serial.read());
    
72.     delay(2);
    
73.     mark=1;
    
74.   }
    
75.   if (mark)
    
76.   {
    
77.     SerialString.trim();
    
78.     //int len=SerialString.length();
    
79. //    int PIndex=SerialString.indexOf('P');
    
80. //    int IIndex=SerialString.indexOf('I');
    
81. //    int DIndex=SerialString.indexOf('D');
    
82. //    int SIndex=SerialString.indexOf('S');
    
83.     //int PComma=SerialString.indexOf(',');
    
84.     //int IIndex=PIndex+1;
    
85.    
    
86.     //int IComma=SerialString.indexOf(',',IIndex);
    
87.     //==============set only one parameter each time===========
    
88.     String _Para="";
    
89.     switch(SerialString[0])
    
90.     {
    
91.       case 'p':
    
92.       case 'P':
    
93.         _Para=SerialString.substring(1);
    
94.         _Para.trim();
    
95.         comdata=&_Para[0];
    
96.         Kp=atof(comdata);
    
97.         
    
98.         break;
    
99.       case 'i':
    
100.       case 'I':
     
101.         _Para=SerialString.substring(1);
     
102.         _Para.trim();
     
103.         comdata=&_Para[0];
     
104.         Ki=atof(comdata);
     
105.         
     
106.         break;
     
107.       case 'd':
     
108.       case 'D':
     
109.         _Para=SerialString.substring(1);
     
110.         _Para.trim();
     
111.         comdata=&_Para[0];
     
112.         Kd=atof(comdata);
     
113.         
     
114.         break;
     
115.       case 's':
     
116.       case 'S':
     
117.         _Para=SerialString.substring(1);
     
118.         _Para.trim();
     
119.         comdata=&_Para[0];
     
120.         pid_SP=atof(comdata);
     
121.         
     
122.         break;
     
123.       default:
     
124.         break;
     
125.     }
     
126. //    String _mkp=SerialString.substring(PComma+1,IIndex-1);
     
127. //    _mkp.trim();
     
128. //    comdata=&_mkp[0];
     
129. //    Kp=atof(comdata);
     
130. //    String _mki=SerialString.substring(IComma+1);
     
131. //    _mki.trim();
     
132. //    comdata=&_mki[0];
     
133. //    Ki=atof(comdata);
     
134.    
     
135.     mark=0;
     
136.   }
     
137.   //=================test kalman filter==========================
     
138.   ax=mpu.getAccelerationX()/16384.00;
     
139.   az=mpu.getAccelerationZ()/16384.00;
     
140.   AngleAcc=(-1.0)*atan2(ax,az)*180/PI;
     
141.   gy=(mpu.getRotationY()-OffsetGy)/131.00;
     
142.   AngleRotation=AngleRotation+gy*TimeSpan/1000.00;
     
143.   //=============kalman calculate============
     
144.   B=TimeSpan/1000.0;
     
145.   U=gy;
     
146.   Z=AngleAcc;
     
147.   X10=A*X00+B*U; //=======formula 1
     
148.   P10=A*P00*(1/A)+Q;//formula 2
     
149.   Kg=P10*(1/H)/(H*P10*(1/H)+R);//formula 3
     
150.   
     
151.   X00=X10+Kg*(Z-H*X10);//formula 4
     
152.   
     
153.   //P00=(1-Kg*H)*P10;//formula 5
     
154.   P00=(1-Kg*H)*P10*(1-Kg*H)+Kg*R*Kg;
     
155.   AngleMerge=X00;
     
156.   //================end test kalman filter======================
     
157.   //MergeAngle();
     
158. 
     
159.   PID();
     
160.   if(1.0<AngleMerge)
     
161.   {
     
162.     l298n.MotorAForward(pid_CV);
     
163.     l298n.MotorBForward(pid_CV);
     
164.   }
     
165.   else if(-1.0>AngleMerge)
     
166.   {
     
167.     l298n.MotorAReversal(pid_CV);
     
168.     l298n.MotorBReversal(pid_CV);   
     
169.   }
     
170.   else
     
171.   {
     
172.     l298n.MotorAStop();
     
173.     l298n.MotorBStop();
     
174.   }
     
175.   Serial.print(AngleRotation);
     
176.   Serial.print(",");
     
177.   Serial.print(AngleMerge);
     
178.   Serial.print(",");
     
179.   Serial.print(AngleAcc);
     
180.   Serial.print(",");
     
181.   Serial.print(pid_CV);
     
182.   Serial.print(",");
     
183.   Serial.print(Kp);
     
184.   Serial.print(",");
     
185.   Serial.println(Ki);
     
186.   delay(100);
     
187. 
     
188. }
     
189. void PID()
     
190. {
     
191.   //===========04-13 add sample time=============
     
192.   
     
193.   if((NowTime-PIDLastTime)>SampleTime)
     
194.   {
     
195.     PIDLastTime=NowTime;
     
196.    
     
197.     ErrAfter=ErrLast;
     
198.     ErrLast=Err;
     
199.     //Err=pid_SP-abs(AngleMerge);
     
200.     Err=pid_SP-AngleMerge;
     
201.     float deltOut=Kp*Err+Ki*ErrLast+Kd*ErrAfter;
     
202.    
     
203.     pid_CV=min(255.0,abs(pid_CV+deltOut));
     
204.     pid_CV=max(80,pid_CV);//limit the min PWM output
     
205.   }
     
206. }
     
207. void MergeAngle()
     
208. {
     
209.   //compute AngleAcc
     
210.   ax=mpu.getAccelerationX()/16384.00;
     
211.   az=mpu.getAccelerationZ()/16384.00;
     
212.   AngleAcc=(-1.0)*atan2(ax,az)*180/PI-OffsetAcc;
     
213.   gy=(mpu.getRotationY()-OffsetGy)/131.00;
     
214.   AngleRotation=AngleRotation+gy*TimeSpan/1000.00;
     
215. 
     
216.   AngleMerge=0.9*AngleRotation+0.1*AngleAcc;
     
217. }
     

复制代码

firewise

L298N.h的库文件，哪里有下？

葱拌豆腐

firewise 发表于 2013-4-16 13:52
L298N.h的库文件，哪里有下？

自己写的，可以搜一下我以前的帖子。你也可以根据自己习惯写一个。

firewise

已找到了，谢楼主。。。
新手，要慢慢消化才行

邵林寺

下载了你的程序试了一下，发现这中间的确有一个死区，就是当处于水平位置的时候，两路驱动全都送出了信号，这个很厉害。。。。。。。

sj61ha94

剛剛學習android,感謝無私分享