赐教：我的平衡车能站稳，但不能解决前进后退的问题
求教各位大侠。附上源代码
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
#define Gry_offset 674
#define Gyr_Gain 65.5
#define pi 3.14159
int16_t ax, ay, az;
int16_t gx, gy, gz;
/*********** PID控制器参数 *********/
float Output;
float kp, ki, kd,kpp;
float angleA,omega;
float P = {{ 1, 0 },{ 0, 1 }};
float Pdot ={ 0,0,0,0};
static const double Q_angle=0.01, Q_gyro=0.03, R_angle=0.5,dtt=0.005,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
float angle, angle_dot,aaxdot,aax;
float position_dot,position_dot_filter,positiono;
//--------------------------------------
//-----------control arguments-----------
int oommm;
int ooommm;
int TN1=2;
int TN2=3;
int TN3=4;
int TN4=7;
int ENA=5;
int ENB=6;
int A=A2;
int C=A3;
int B=A0;
int D=A1;
float LOutput;
float ROutput;
float LLOutput;
float RROutput;
//-------------------------------------
void setup() {
Wire.begin();
delay(1000);
accelgyro.initialize();
delay(100);
pinMode(2,OUTPUT);
pinMode(3,OUTPUT);
pinMode(4,OUTPUT);
pinMode(7,OUTPUT);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
delay(1);
//Serial.begin(9600);
}
void loop() {
//---------control-----------
llD();
//------control----------
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//-----------------------------------------------------------------------------------------------------------------
angleA= atan2(ay , az) * 180 / pi;
omega=(gx +Gry_offset)/Gyr_Gain;
if (abs(angleA)<30) {
Kalman_Filter(angleA,omega);
PIDD();
PWMB();
}
else {
analogWrite(ENA, 0); //PWM调速a==0-255
analogWrite(ENB, 0);
}
delay(20);
Serial.print("\t");
Serial.print(LOutput);
Serial.print("\t");
Serial.print(ROutput);
Serial.print("\t");
}
//=---------------------------------------------------------------
void llD(){
int up=digitalRead(A);
int down=digitalRead(C);
int leftB=digitalRead(B);
int rightD=digitalRead(D);
if(up==HIGH)
{
former();
}
if(down==HIGH)
{
back();
}
if(leftB==HIGH)
{
left();
}
if(rightD==HIGH)
{
right();
}
}
void left(){
LLOutput=430;
RROutput=-430;
}
void right(){
LLOutput=-80;
RROutput=80;
}
void back(){
LLOutput=40;
RROutput=40;
}
voidformer(){
LLOutput=-40;
RROutput=-40;
}
//-----------------------------
void Kalman_Filter(double angle_m,double gyro_m)
{
angle+=(gyro_m-q_bias) * dtt;
Pdot=Q_angle - P - P;
Pdot=- P;
Pdot=- P;
Pdot=Q_gyro;
P += Pdot * dtt;
P += Pdot * dtt;
P += Pdot * dtt;
P += Pdot * dtt;
angle_err = angle_m - angle;
PCt_0 = C_0 * P;
PCt_1 = C_0 * P;
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P;
P -= K_0 * t_0;
P -= K_0 * t_1;
P -= K_1 * t_0;
P -= K_1 * t_1;
angle+= K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;//也许应该用last_angle-angle
}
//-----------------------
voidPIDD(){
kp=1;ki=1;kd=0.5;kpp=1;
aaxdot=0.5*aaxdot+0.5*angle_dot;
aax=0.5*aax+0.5*angle;
position_dot=Output*0.04; //利用PWM值代替轮速传感器的信号
position_dot_filter*=0.9; //车轮速度滤波
position_dot_filter+=position_dot*0.1;
positiono+=position_dot_filter;
positiono=constrain(positiono,-500,500);
Output= 80*aax *kp + 1*aaxdot *ki +0.1*positiono *kd + 1*position_dot_filter *kpp;
//if(LLOutput==0)
//{LOutput=Output-abs(Output);
// ROutput=Output-abs(out;
// }else
LOutput=Output+LLOutput;
ROutput=Output-RROutput;
//Output= K_angle*angle *kp + K_angle_dot*angle_dot *ki +K_position*positiono *kd + K_position_dot*position_dot_filter *kpp;
}
//-------------电机正反转 PWM输出-----------
void PWMB(){
if(LOutput<0)
{
digitalWrite(TN1, LOW);//left back
digitalWrite(TN2, HIGH);//left former
//digitalWrite(TN3, LOW);//right back
//digitalWrite(TN4, HIGH); //right former
}
else if(LOutput>0)
{
digitalWrite(TN1, HIGH);//left back
digitalWrite(TN2, LOW);//left former
// digitalWrite(TN3, HIGH);//right back
// digitalWrite(TN4, HIGH); //right former
}
else
{
digitalWrite(TN1, HIGH);//left back
digitalWrite(TN2, HIGH);//left former
//digitalWrite(TN3, HIGH);//right back
// digitalWrite(TN4, HIGH); //right former
}
//-----------------------------------------
if(ROutput<0)
{
digitalWrite(TN3, LOW);//right back
digitalWrite(TN4, HIGH); //right former
}
else if(ROutput>0)
{
digitalWrite(TN3, HIGH);//right back
digitalWrite(TN4, LOW); //right former
}
else
{
digitalWrite(TN3, HIGH);//right back
digitalWrite(TN4, HIGH); //right former
}
analogWrite(ENA,min(255,abs(LOutput)+35)); //PWM调速a==0-255
analogWrite(ENB,min(255,abs(ROutput)+35));
} 楼主，想请教一下你那个Gry_offset和Gyr_Gain是怎么定的？ jacen梁 发表于 2013-10-21 19:54 static/image/common/back.gif
楼主，想请教一下你那个Gry_offset和Gyr_Gain是怎么定的？
这个是偏置和增益吧、、、 #include "MPU6050.h"
请问楼主这个头文件怎么写，怎么找到呀？？编译总是提示不能打开 li448475953 发表于 2013-10-24 16:35 static/image/common/back.gif
这个是偏置和增益吧、、、
我是初学者，多问两句，那个偏置和增益是什么的偏置和增益啊？怎么去理解或者是怎么确定他们的参数呢？ 现在是和你同样的进度。。。前进后退要把速度闭环进去吧 jacen梁 发表于 2013-10-25 13:35 static/image/common/back.gif
我是初学者，多问两句，那个偏置和增益是什么的偏置和增益啊？怎么去理解或者是怎么确定他们的参数呢？ ...
陀螺仪在静态时输出的直流偏置电压 增益这个参数是由积分的时间 和陀螺仪信号放大的倍数来确定、、 flyxiaopei 发表于 2013-10-26 20:05
现在是和你同样的进度。。。前进后退要把速度闭环进去吧
请教下，速度环和角度环，什么形式嵌套在一起比较合理。
页:
[1]