平衡車求助
我用了這段代碼 當車車向后傾時要向前走 但向前傾時不會向后走求大神們幫幫助 我是用arduino uno v3 加6050
unsigned char Re_buf,counter=0;
unsigned char sign=0;
int M11=5;
int M12=6;
int M21=9;
int M22=10;
float a,w,Angle,T;
short sAccelerat,sAngleVelocity,sAngle,sT;
void setup() {
// initialize serial:
Serial.begin(115200);
pinMode(M11,OUTPUT);analogWrite(M11,0);
pinMode(M12,OUTPUT);analogWrite(M12,0);
pinMode(M21,OUTPUT);analogWrite(M21,0);
pinMode(M22,OUTPUT);analogWrite(M22,0);
}
void SetMotor(float v1,float v2)
{
if (v1>255){v1=255;analogWrite(M11,0);analogWrite(M12,v1);}
else if (v1>0) {analogWrite(M11,0);analogWrite(M12,v1);}
else if (v1>-255) {analogWrite(M12,0);analogWrite(M11,-v1);}
else{v1=-255;analogWrite(M12,0);analogWrite(M11,-v1);}
if (v2>255){v2=255;analogWrite(M21,0);analogWrite(M22,v2);}
else if (v2>0) {analogWrite(M21,0);analogWrite(M22,v2);}
else if (v2>-255) {analogWrite(M22,0);analogWrite(M21,-v2);}
else {v2=-255;analogWrite(M22,0);analogWrite(M21,-v2);}
}
float PID1(float e,float kp,float ki,float kd)
{
static float es=0,sum=0;
float r;
sum+=e;
r = kp*e+ki*sum+kd*(e-es);
es=e;
return r;
}
float PID2(float e,float kp,float ki,float kd)
{
static float es=0,sum=0;
float r;
sum+=e;
r = kp*e+ki*sum+kd*(e-es);
es=e;
return r;
}
void loop() {
float kp=30,ki=0.0,kd=10,r1,r2;
if (sign==0) return;//sign为数据更新标志，每隔10ms更新一次，也就是说以下代码每隔10ms控制一次。
sign=0;
kd = (float)analogRead(0)/1024*200;
r1=PID1(Angle,kp,ki,kd);//PID1、PID2函数就是第四节的PID函数，为了区分左右轮，所以分成两个。
r2=PID2(Angle,kp,ki,kd);
SetMotor(r1,r2);//设置电机转速。
Serial.print("angle:");
Serial.print(Angle);Serial.print(" ");
Serial.print(r1);Serial.print(" ");
Serial.println(kd);Serial.print(" ");
}
void serialEvent() {
while (Serial.available()) {
//char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code
Re_buf=(unsigned char)Serial.read();
if(counter==0&&Re_buf!=0x55) return; //第0号数据不是帧头
counter++;
if(counter==11) //接收到11个数据
{
counter=0; //重新赋值，准备下一帧数据的接收
switch(Re_buf )
{
case 0x51:
a = float(short(Re_buf <<8| Re_buf ))/32768*16;
a =float(short(Re_buf <<8| Re_buf ))/32768*16;
a =float(short(Re_buf <<8| Re_buf ))/32768*16;
break;
case 0x52:
w =float(short(Re_buf <<8| Re_buf ))/32768*250;
w =float(short(Re_buf <<8| Re_buf ))/32768*250;
w =float(short(Re_buf <<8| Re_buf ))/32768*250;
break;
case 0x53:
Angle =float(short(Re_buf <<8| Re_buf ))/32768*180;
Angle =float(short(Re_buf <<8| Re_buf ))/32768*180;
Angle =float(short(Re_buf <<8| Re_buf ))/32768*180;
T =float(short(Re_buf <<8| Re_buf ));///340.0+36.25
sign=1;
break;
}
}
}
}
页:
[1]