备份一下自己小车的程序
正在调试，欢迎围观。O(∩_∩)O~#include <I2Cdev.h>
#include <L298N.h>
#include <MPU6050.h>
#include <Wire.h>
#include <stdlib.h>
float ax,az,gy;//get acceleration_x,acceleration_z,rotation_y
long OffsetGy,OffsetSum;
float OffsetAccSumX,OffsetAccSumZ,OffsetAcc;
float AngleAcc,AngleRotation,AngleMerge;//angel computed by acceleration and rotation
float Kp,Kd,Ki,pid_SP,pid_CV;
float Err,ErrLast,ErrAfter;//PID
long LastTime,NowTime,TimeSpan,SampleTime,PIDLastTime;//
//===========kalman filter parameter define============
float A,B,H,Q,R,P00,P10,X00,X10,Z,U,Kg;
//==========add serial communication=========
String SerialString;
boolean mark=0;
char *comdata;
bool blinkState=false;
L298N l298n;
MPU6050 mpu;
void setup()
{
//============initial kalman parameter======
A=1.0;
H=1.0;
Q=0.003;
R=0.01;
P00=5.0;
P10=0.0;
X00=0.0;
//===============end initial===========
Wire.begin();
Serial.begin(9600);
Serial.println("start");
mpu.initialize();
Serial.println(mpu.getRotationY());
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
//compute the gyroscope offset automatic
for(int i=0;i<200;i++)
{
Serial.println(mpu.getRotationY());
OffsetAccSumX+=mpu.getAccelerationX()/16384.00;
OffsetAccSumZ+=mpu.getAccelerationZ()/16384.00;
OffsetSum=OffsetSum+mpu.getRotationY();
delay(20);
}
OffsetAcc=(-1.0)*atan2(OffsetAccSumX/200.0,OffsetAccSumZ/200.0)*180/PI;
OffsetGy=OffsetSum/200;
pid_SP=0.0;
Kp=50.0;
Ki=10.0;
Kd=0.0;
}
void loop()
{
NowTime=millis();
TimeSpan=NowTime-LastTime;
LastTime=NowTime;
Serial.flush();
//==============add serial communication=======
while(Serial.available()>0)
{
SerialString += char(Serial.read());
delay(2);
mark=1;
}
if (mark)
{
SerialString.trim();
//int len=SerialString.length();
// int PIndex=SerialString.indexOf('P');
// int IIndex=SerialString.indexOf('I');
// int DIndex=SerialString.indexOf('D');
// int SIndex=SerialString.indexOf('S');
//int PComma=SerialString.indexOf(',');
//int IIndex=PIndex+1;
//int IComma=SerialString.indexOf(',',IIndex);
//==============set only one parameter each time===========
String _Para="";
switch(SerialString)
{
case 'p':
case 'P':
_Para=SerialString.substring(1);
_Para.trim();
comdata=&_Para;
Kp=atof(comdata);
break;
case 'i':
case 'I':
_Para=SerialString.substring(1);
_Para.trim();
comdata=&_Para;
Ki=atof(comdata);
break;
case 'd':
case 'D':
_Para=SerialString.substring(1);
_Para.trim();
comdata=&_Para;
Kd=atof(comdata);
break;
case 's':
case 'S':
_Para=SerialString.substring(1);
_Para.trim();
comdata=&_Para;
pid_SP=atof(comdata);
break;
default:
break;
}
// String _mkp=SerialString.substring(PComma+1,IIndex-1);
// _mkp.trim();
// comdata=&_mkp;
// Kp=atof(comdata);
// String _mki=SerialString.substring(IComma+1);
// _mki.trim();
// comdata=&_mki;
// Ki=atof(comdata);
mark=0;
}
//=================test kalman filter==========================
ax=mpu.getAccelerationX()/16384.00;
az=mpu.getAccelerationZ()/16384.00;
AngleAcc=(-1.0)*atan2(ax,az)*180/PI;
gy=(mpu.getRotationY()-OffsetGy)/131.00;
AngleRotation=AngleRotation+gy*TimeSpan/1000.00;
//=============kalman calculate============
B=TimeSpan/1000.0;
U=gy;
Z=AngleAcc;
X10=A*X00+B*U; //=======formula 1
P10=A*P00*(1/A)+Q;//formula 2
Kg=P10*(1/H)/(H*P10*(1/H)+R);//formula 3
X00=X10+Kg*(Z-H*X10);//formula 4
//P00=(1-Kg*H)*P10;//formula 5
P00=(1-Kg*H)*P10*(1-Kg*H)+Kg*R*Kg;
AngleMerge=X00;
//================end test kalman filter======================
//MergeAngle();
PID();
if(1.0<AngleMerge)
{
l298n.MotorAForward(pid_CV);
l298n.MotorBForward(pid_CV);
}
else if(-1.0>AngleMerge)
{
l298n.MotorAReversal(pid_CV);
l298n.MotorBReversal(pid_CV);
}
else
{
l298n.MotorAStop();
l298n.MotorBStop();
}
Serial.print(AngleRotation);
Serial.print(",");
Serial.print(AngleMerge);
Serial.print(",");
Serial.print(AngleAcc);
Serial.print(",");
Serial.print(pid_CV);
Serial.print(",");
Serial.print(Kp);
Serial.print(",");
Serial.println(Ki);
delay(100);
}
void PID()
{
//===========04-13 add sample time=============
if((NowTime-PIDLastTime)>SampleTime)
{
PIDLastTime=NowTime;
ErrAfter=ErrLast;
ErrLast=Err;
//Err=pid_SP-abs(AngleMerge);
Err=pid_SP-AngleMerge;
float deltOut=Kp*Err+Ki*ErrLast+Kd*ErrAfter;
pid_CV=min(255.0,abs(pid_CV+deltOut));
pid_CV=max(80,pid_CV);//limit the min PWM output
}
}
void MergeAngle()
{
//compute AngleAcc
ax=mpu.getAccelerationX()/16384.00;
az=mpu.getAccelerationZ()/16384.00;
AngleAcc=(-1.0)*atan2(ax,az)*180/PI-OffsetAcc;
gy=(mpu.getRotationY()-OffsetGy)/131.00;
AngleRotation=AngleRotation+gy*TimeSpan/1000.00;
AngleMerge=0.9*AngleRotation+0.1*AngleAcc;
}
L298N.h的库文件，哪里有下？ firewise 发表于 2013-4-16 13:52 static/image/common/back.gif
L298N.h的库文件，哪里有下？
自己写的，可以搜一下我以前的帖子。你也可以根据自己习惯写一个。 已找到了，谢楼主。。。
新手，要慢慢消化才行 下载了你的程序试了一下，发现这中间的确有一个死区，就是当处于水平位置的时候，两路驱动全都送出了信号，这个很厉害。。。。。。。 剛剛學習android,感謝無私分享
页:
[1]