arduino 中断和PWM输出冲突
自己在试着做两轮车，编码器这里遇到了点问题，小车在往一个方向转的时候总是顿顿的，另外一边就没事，去掉码盘就好了。刚才看到论坛上说中断与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)
doubleOutput1,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();
} 请问楼主怎么解决的呢？？
页:
[1]