我的两轮小车用的是网上的代码
PID控制是用了网上的库
只调了一个参数K
车轮的速度会改变
但是车轮的方向就是不会改变，始终朝着一个方向。
求论坛大神帮忙分析一下，小弟感激不尽
代码如下（大部分抄的网上的再改动的）
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "ADXL345.h"
#include "PID_v1.h"
ADXL345 accel;
//#define Gry_offset 1.2 // 陀螺仪偏移量
//#define Gyr_Gain 0.04 // 满量程2000dps时灵敏度(dps/digital)
//#define Gyr_Gain 0.39
#define pi 3.14159
const int xpin = 1;
int16_t ax, ay, az;
//int16_t gx, gy, gz;
/********** 互补滤波器参数 *********/
//unsigned long preTime = 0; // 采样时间
//float f_angle = 0.0; // 滤波处理后的角度值
/*********** PID控制器参数 *********/
//unsigned long lastTime;
//;, Setpoint,Input;
//double errSum, lastErr;
float kp, ki, kd,kpp;
//int SampleTime = 0.1; //1 sec
//float Outputa = 0.0;
float angleA,omega;
//double Kp, Ki, Kd;
/*float P_00,P_01,P_10,P_11;
float y1,S,x_angle,x_bias;*/
//float P[2][2] = {{ 1, 0 },{ 0, 1 }};
//float Pdot[4] ={ 0,0,0,0};
//static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dtt=0.007,C_0 = 1;
//float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
#define Q_angle 0.01 // 角度数据置信度
#define Q_omega 0.0003 // 角速度数据置信度
#define R_angle 0.01 // 方差噪声
float Klm_angle;
float bias = 0,dt=0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
//float angle, angle_dot; // aaxdot,aax;
float position_dot,position_dot_filter,positiono;
//double Speed_Need=0;
//float K_angle=2;
//float K_angle_dot=0.5; //换算系数：256/10 =25.6；
//float K_position=0.1; //换算系数：(256/10) * (2*pi/(64*12))=0.20944；//256/10:电压换算至PWM，256对应10V；
//float K_position_dot=1; //换算系数：(256/10) * (25*2*pi/(64*12))=20.944；
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint,2,0,0, DIRECT);
//--------------------------------------
unsigned preTime;
//int oommm;
//int ooommm;
//int oooommmm;
//double OLH= 10,ORH = 10;
int TN1=10;
int TN2=11;
int TN3=12;
int TN4=13;
int ENA=3;
int ENB=5;
//-------------------------------------
void setup() {
Wire.begin();
Setpoint = -11;
myPID.SetOutputLimits(-255,255);
myPID.SetMode(AUTOMATIC);
//delay(1000);
accel.initialize();
//accelgyro.initialize();
myPID.SetMode(AUTOMATIC);
delay(500);
pinMode(10,OUTPUT);
pinMode(11,OUTPUT);
pinMode(12,OUTPUT);
pinMode(13,OUTPUT);
pinMode(3,OUTPUT);
pinMode(5,OUTPUT);
delay(100);
Serial.begin(9600);
}
void loop() {
unsigned long now = millis();
dt=(now-preTime)/1000.0;
preTime=now;
accel.getAcceleration(&ax, &ay, &az);
angleA= atan2(az , ax) * 180 / pi;
//Serial.println((float)angleA);
int x = analogRead(xpin);
// 根据加速度分量得到的角度(degree)
//180度至0至-180（360度）取0度为坚直时中立点 因为坚直时为有偏差，所以减去0.2度....
omega=((float)x-512)*0.39;
//omega=((float)x-512)*150/512;
//Serial.print((float)angleA);
//Serial.print((float)omega);
//Serial.print("\n");
if (abs(angleA)<50)
{ //这个是误差达到一定程度后的系统关闭开关.
Kalman_Filter(angleA,omega);
Input = Klm_angle;
myPID.Compute();
Serial.print("output:");
Serial.println(Output);
PWMB();
}
else
{
analogWrite(ENA, 0); //PWM调速a==0-255
analogWrite(ENB, 0);
}
delay(50);
}
//=---------------------------------------------------------------
void Kalman_Filter(double angleA,double omega)
{
Klm_angle += (omega - bias) * dt; // 先验估计
P_00 += -(P_10 + P_01) * dt + Q_angle *dt;
P_01 += -P_11 * dt;
P_10 += -P_11 * dt;
P_11 += +Q_omega * dt; // 先验估计误差协方差
float K_0 = P_00 / (P_00 + R_angle);
float K_1 = P_10 / (P_00 + R_angle);
bias += K_1 * (angleA - Klm_angle);
Klm_angle += K_0 * (angleA - Klm_angle); // 后验估计
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
Serial.print("Klm_angle:");
Serial.println((float)Klm_angle);
}
//-----------------------
/*void PIDD(){
kp=3.2;
ki=10;
kd=10;
kpp=10*/;
/*kp=analogRead(8)*0.01;
ki=analogRead(9)*0.007;
kd=analogRead(10)*0.007;
kpp=analogRead(11)*0.007;*/
//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+=Speed_Need;
positiono=constrain(positiono,-500,500);
Output= 2*angle *kp + 0.5*angle_dot*ki +0.02*positiono *kd + 1*position_dot_filter *kpp;
//Output= K_angle*angle *kp + K_angle_dot*angle_dot *ki +K_position*positiono *kd + K_position_dot*position_dot_filter *kpp;*/
//Serial.print((float)Output);
//Serial.print("\n");
//Serial.println("output");
//Serial.println((float)Output);
//}
//-------------电机正反转 PWM输出-----------
void PWMB(){
//if(Output<-0.3)
if(Output<0)
{
digitalWrite(TN1, HIGH);
digitalWrite(TN2, LOW);
digitalWrite(TN3, HIGH);
digitalWrite(TN4, LOW);
}
// if(Output>0.3)
if(Output>0);
{
digitalWrite(TN1, LOW);
digitalWrite(TN2, HIGH);
digitalWrite(TN3, LOW);
digitalWrite(TN4, HIGH);
analogWrite(ENA,abs(Output)); //PWM调速a==0-255
analogWrite(ENB,abs(Output));
// Serial.println(Output);
}
/*oommm=min(220,abs(Output));
analogWrite(ENA, oommm+35); //PWM调速a==0-255
analogWrite(ENB, oommm+30); */
} |
发表于 2013-7-18 23:20:44
