自己在试着做两轮车,编码器这里遇到了点问题,小车在往一个方向转的时候总是顿顿的,另外一边就没事,去掉码盘就好了。刚才看到论坛上说中断与pwm冲突,所以求教一下程序应该怎么改呀。- #include <Wire.h>
- #include <ADXL345.h>
- #include <L3G4200DJ.h>
- ADXL345 acceleration;
- L3G4200DJ gyro;
- #define Gry_offset -20 // 陀螺仪偏移量
- #define Gyr_Gain -0.07 // 满量程2000dps时灵敏度(dps/digital)
- #define pi 3.14159
- #define drift 50
- double Setpoint, Input, Output;
- double Klm_angle;
- double kp = 5.3;
- unsigned long preTime = 0; // 采样时间
- double f_angle = 0.0; // 滤波处理后的角度值
- int x;
- #define Q_angle 0.01 // 角度数据置信度
- #define Q_omega 0.0003 // 角速度数据置信度
- #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; // 时间标记
- double count1 = 0; //左轮编码器码盘脉冲计数值
- double count2= 0; //右轮编码器码盘脉冲计数值
- double rpm1 = 0; //左轮电机每分钟(min)转速(r/min)
- double rpm2 = 0; //右轮电机每分钟(min)转速(r/min)
- double Output1,Output2,Output_x;
- 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);
- Serial.begin(9600);
- Wire.begin();
- acceleration.Init_ADXL345();
- gyro.Init_L3G4200DJ();
- Setpoint=0.00;
- 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;
-
- if(Klm_angle>1.36){
- if(36>Klm_angle>1.36)
- { Output=kp*(Klm_angle-1.36);
- 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{
- Output=10*(Klm_angle-1.36);
- Output1=Output;
- Output2=Output+Output_x;
- analogWrite(10,min(Output1,255)); //PWM out put
- analogWrite(5,min(Output2,255)); //PWM out put
- analogWrite(11,0); //PWM out put
- analogWrite(6,0); //PWM out put
- }
- }
- if(Klm_angle<1.36)
- {if(-38<Klm_angle<1.36)
- { Output=kp*(Klm_angle-1.36);
- Output = -Output;
- 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);
- }
- else{
- Output=10*(Klm_angle-1.36);
- Output = -Output;
- Output1=Output;
- Output2=Output+Output_x;
- analogWrite(11,min(Output1,255)); //PWM out put
- analogWrite(6,min(Output2,255)); //PWM out put
- analogWrite(10,0); //PWM out put
- analogWrite(5,0);
- }
- }
- time = millis();
- if(abs(time - old_time) >= 10) // 如果计时时间已达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(rpm2);
- Serial.print(", ");
- Serial.println(Output1);
- delay(10);
- }
- void Code1()
- {
- //为了不计入噪音干扰脉冲,
- //当2次中断之间的时间大于5ms时,计一次有效计数
- if((millis()-time1)>5)
- //当编码器码盘的OUTA脉冲信号下跳沿每中断一次,
- count1 += 1; // 编码器码盘计数加一
- time1==millis();
- }
- // 右侧车轮电机的编码器码盘计数中断子程序
- void Code2()
- {
- if((millis()-time2)>5)
- //当编码器码盘的OUTA脉冲信号下跳沿每中断一次,
- count2 += 1; // 编码器码盘计数加一
- time2==millis();
- }
复制代码 |