各位大神程序如下。问题就是角度小于零的时候轮子转，角度大于零轮子就不动了，输出的角度还有PWM的值都没问题，哪里错了啊？？？
#include <Wire.h>
#include <ADXL345.h>
#include <L3G4200DJ.h>
#include <PID_v1.h>
ADXL345 acceleration;
L3G4200DJ gyro;
#define Gry_offset 0 // 陀螺仪偏移量
#define Gyr_Gain 0.07 // 满量程2000dps时灵敏度(dps/digital)
#define pi 3.14159
#define drift 50
double Setpoint, Input, Output;
double Output1,Output2,Output_x;
double temp_kp,temp_ki,temp_kd;
PID myPID(&Input,&Output,&Setpoint,10,0,0,DIRECT);
double Klm_angle;
unsigned long preTime = 0; // 采样时间
double f_angle = 0.0; // 滤波处理后的角度值
uint32_t timer;
double count1 = 0; //左轮编码器码盘脉冲计数值
double count2= 0; //右轮编码器码盘脉冲计数值
double rpm1 = 0; //左轮电机每分钟(min)转速(r/min)
double rpm2 = 0; //右轮电机每分钟(min)转速(r/min)
#define Q_angle 0.001 // 角度数据置信度
#define Q_omega 0.003 // 角速度数据置信度
#define R_angle 0.03 // 方差噪声
double bias = 0;
double P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
unsigned long lastTime; // 前次时间
double ITerm, lastInput; // 积分项、前次输入
unsigned long time = 0, old_time = 0; // 时间标记
unsigned long time1 = 0, time2 = 0; // 时间标记
void setup() {
pinMode(10,OUTPUT);//1号电机转向
pinMode(11,OUTPUT);//1号电机转速
pinMode(5,OUTPUT);//2号电机转向
pinMode(6,OUTPUT);//2号电机转速
pinMode(2,INPUT); //伺服电机编码器的OUTA和OUTB信号端设置为输入模式
pinMode(8,INPUT);
pinMode(3,INPUT);
pinMode(9,INPUT);
car_stop();
Serial.begin(9600);
Wire.begin();
acceleration.Init_ADXL345();
gyro.Init_L3G4200DJ();
timer=micros();
Setpoint=0.00;
myPID.SetSampleTime(10);
myPID.SetMode(AUTOMATIC);
attachInterrupt(0, Code1, FALLING);//小车左车轮电机的编码器脉冲中断函数
attachInterrupt(1, Code2, FALLING);//小车右车轮电机的编码器脉冲中断函数
}
void loop() {
unsigned long now = millis(); // 当前时间(ms)
double dt = (now - preTime) / 1000.0; // 微分时间(s)
preTime = now;
acceleration.Out_ADXL345();
gyro.Out_L3G4200DJ();
double a_a=acceleration.a.x;
double a_c=acceleration.a.z;
double g_b=gyro.g.y;
double angleA = atan(a_a / a_c) * 180 / pi; // 根据加速度分量得到的角度(degree)
double omega = Gyr_Gain * (g_b + Gry_offset); // 当前角速度(degree/s)
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; // 先验估计误差协方差
double K_0 = P_00 / (P_00 + R_angle);
double 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;
/************ PID控制器 ***********/
timer = micros();
if((Klm_angle>(Setpoint-drift))&&(Klm_angle<(Setpoint+drift)))
{
if((Klm_angle<Setpoint)&&(Klm_angle>(Setpoint-drift)))
{
myPID.SetControllerDirection(DIRECT);
Input=Klm_angle;
myPID.Compute();
Output1=Output;
Output2=Output+Output_x;
analogWrite(11,Output1); //PWM out put
analogWrite(6,Output2); //PWM out put
analogWrite(10,0); //PWM out put
analogWrite(5,0);
}
if((Klm_angle>Setpoint)&&(Klm_angle<(Setpoint+drift)))
{
myPID.SetControllerDirection(REVERSE);
Input=Klm_angle;
myPID.Compute();
Output1=Output;
Output2=Output+Output_x;
analogWrite(10,Output1); //PWM out put
analogWrite(5,Output2); //PWM out put
analogWrite(11,0); //PWM out put
analogWrite(6,0); //PWM out put
}
else
{
car_stop();
}
}
else
{
car_stop();
}
time = millis();
if(abs(time - old_time) >= 50) // 如果计时时间已达1秒
{
detachInterrupt(0); // 关闭外部中断0
detachInterrupt(1); // 关闭外部中断1
//把每一秒钟编码器码盘计得的脉冲数，换算为当前转速值
//转速单位是每分钟多少转，即r/min。这个编码器码盘为12个齿。
rpm1 =(float)count1*60/334;//小车左车轮电机转速
rpm2 =(float)count2*60/334; //小车右车轮电机转速
Output_x=(rpm1-rpm2)*(Output/255);
count1 = 0; //把脉冲计数值清零，以便计算下一秒的脉冲计数
count2 = 0;
old_time= millis(); // 记录每秒测速时的时间节点
attachInterrupt(0, Code1,FALLING); // 重新开放外部中断0
attachInterrupt(1, Code2,FALLING); // 重新开放外部中断1
}
Serial.print(Klm_angle);
Serial.print(", ");
Serial.print(Output);
Serial.print(",");
Serial.println(" ");
delay(50);
}
void Code1()
{
//为了不计入噪音干扰脉冲，
//当2次中断之间的时间大于5ms时，计一次有效计数
if((millis()-time1)>5)
//当编码器码盘的OUTA脉冲信号下跳沿每中断一次，
count1 += 1; // 编码器码盘计数加一
time1==millis();
}
// 右侧车轮电机的编码器码盘计数中断子程序
void Code2()
{
if((millis()-time2)>5)
//当编码器码盘的OUTA脉冲信号下跳沿每中断一次，
count2 += 1; // 编码器码盘计数加一
time2==millis();
}
void car_stop()
{
analogWrite(11,0); //PWM out put
analogWrite(6,0); //PWM out put
analogWrite(10,0); //PWM out put
analogWrite(5,0);
}
|
发表于 2014-3-3 19:29:48
楼主