首页 › Arduino › 让你的自衡小车更快的站起来

葱拌豆腐 发表于 2013-4-16 10:08:58

让你的自衡小车更快的站起来

简单说就是先测出小车的死区，逐渐调高PWM的输出，电机开始转动时的PWM值就是小车的死区，然后在PID输出加上这个死区，这样就能更好的保持小车的稳定性。
原理分析（个人分析）：以前做伺服控制时，伺服电机有一个“使能”的状态，当伺服电机上使能后，电机轴就处在一种“抱死”的状态，在自衡小车的控制信号中增加死区值，也是相当于将小车始终保持在一个“抱死”的状态，小车的稳定性也就会更高。小车还没站立起来的可以试一下。
本人的小车基本站起来的，算法就是按照官方手册里来的，纯角度控制，没用速度控制，改天上视频。

一下代码是临时版的代码，注释很烂，程序也很烂，如果觉得有参考可以看一下，否则可以直接无视，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
//============define variable for freescale refrence manual=========
float AngleMergeLast,Tg,Pangle,Dangle,MotorDeadZone;
//==================

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()
{

//SampleTime=10;
////============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.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
//compute the gyroscope offset automatic
OffsetSum=0;
for(int i=0;i<1024;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/1024;
pid_SP=0.0;
Kp=48.0;
Ki=12.0;
Kd=0.0;
AngleMergeLast=0.0;
Tg=0.5;
MotorDeadZone=80;
}

void loop()
{

NowTime=millis();
TimeSpan=NowTime-LastTime;
LastTime=NowTime;
//==============add serial communication=======
SerialString="";
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);
      Dangle=atof(comdata);
      
      break;
      case 'i':
      case 'I':
      _Para=SerialString.substring(1);
      _Para.trim();
      comdata=&_Para;
      Ki=atof(comdata);
      Pangle=atof(comdata);
      
      break;
      
      case 'm':
      case 'M':
      _Para=SerialString.substring(1);
      _Para.trim();
      comdata=&_Para;
      //Ki=atof(comdata);
      MotorDeadZone=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;
      case 't':
      case 'T':
      _Para=SerialString.substring(1);
      _Para.trim();
      comdata=&_Para;
      Tg=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();
MergeAngle_Freescale();
//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(AngleMerge);
Serial.print(",");
Serial.print(Dangle);
Serial.print(",");
Serial.print(Pangle);
Serial.print(",");
Serial.print(pid_CV);
Serial.print(",");
Serial.print(MotorDeadZone);
Serial.print(",");
Serial.println(Tg);
delay(10);

}
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)+MotorDeadZone);
    //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;
gy=(mpu.getRotationY()-OffsetGy)/131.00;
AngleRotation=AngleRotation+gy*TimeSpan/1000.00;

AngleMerge=0.9*AngleRotation+0.1*AngleAcc;
}
void MergeAngle_Freescale()
{
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;

float AngleErr=AngleAcc-AngleMerge;
AngleMerge=AngleMerge+(gy+AngleErr/Tg)*TimeSpan/1000.00;

// ========this solution is from Freescale, it is effective
pid_CV=abs((0-gy)*Dangle+(0-AngleMerge)*Pangle)+MotorDeadZone;
pid_CV=min(255.0,pid_CV);
}

xxx 发表于 2013-4-16 12:10:29

想做个玩玩····

maxiujun 发表于 2013-4-16 12:49:34

使用普通的玩具直流电机可以实现吗？

maxiujun 发表于 2013-4-16 12:49:53

传感器使用的什么型号的？

北斗 发表于 2013-4-16 13:43:45

maxiujun 发表于 2013-4-16 12:49 static/image/common/back.gif
传感器使用的什么型号的？

看注释里写的是MPU6050

葱拌豆腐 发表于 2013-4-16 14:28:41

maxiujun 发表于 2013-4-16 12:49 static/image/common/back.gif
使用普通的玩具直流电机可以实现吗？

只要扭矩和速度足够就可以（也就是功率足够大，O(∩_∩)O~）。我用的是带测速的，淘宝价格22元，搜一下就知道了。

wshp1987515 发表于 2013-4-22 20:57:41

标记一下，爪机看的麻烦

gucci99 发表于 2013-6-29 17:50:37

没有<L298N.h>文件，不知能否提供。

164335413 发表于 2013-10-10 22:55:07

程序是有点乱，每个人定义的名字不同，不过我的小车还不能实现蓝牙控制，有点头疼、

西瓜 发表于 2013-11-26 12:47:00

164335413 发表于 2013-10-10 22:55 static/image/common/back.gif
程序是有点乱，每个人定义的名字不同，不过我的小车还不能实现蓝牙控制，有点头疼、

同求<L298N.h>文件

浓颜。 发表于 2013-12-29 17:48:28

在学习平衡小车，，，学习楼主了

查看完整版本: 让你的自衡小车更快的站起来