在本论坛吸取别人的程序经验，然后自己稍加修改，发现我自己做的PID控制怎么调也不能实现两个轮子的速度一致，虽然速度传感器一直在进行检测，但是就是说算法不好使
现贴上程序请大家帮忙参谋下，感谢！！！
#include <PID_v1.h>
double Setpoint=0, Input, Output;
double Kp=20, Ki=3, Kd=0.08;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
int motorIA1 = 9;左轮输入
int motorIB1 = 8;
int motorIA2 = 11;右轮输入
int motorIB2 = 10;
int scode_L = 0; //左轮编码器码盘脉冲计数值
int scode_R =0; //右轮编码器码盘脉冲计数值
unsigned long old_time=0,new_time=0; // 总时间标记
unsigned long time1 = 0, time2 = 0; //左1，右2 轮 时间标记
double val_right=0,val_left=0,pwm = 180;
float spdLft, spdRgt;
boolean flag= true;
char code; //定义字符变量
void setup()
{
Serial.begin(9600);
pinMode(motorIA1,OUTPUT);
pinMode(motorIB1,OUTPUT);
pinMode(motorIA2,OUTPUT);
pinMode(motorIB2,OUTPUT);
attachInterrupt(0, Code1, FALLING);//中断设置
attachInterrupt(1, Code2, FALLING);
//PID设置参数
Setpoint=0; //左右差为0 为目标
myPID.SetOutputLimits(-40,40); //输出限制
myPID.SetTunings(Kp,Ki,Kd);
myPID.SetSampleTime(1000);
myPID.SetMode(AUTOMATIC);
}
// 左侧车轮电机的编码器码盘计数中断子程序
void Code1()
{
if((millis()-time1)>5) scode_L += 1;
time1=millis();
}
void Code2()
{
if((millis()-time2)>5) scode_R += 1;
time2=millis();
}
//前进函数
void advance()
{
analogWrite(motorIA1, val_left);//使直流电机（左）顺时针转,可调速
digitalWrite(motorIB1,LOW);
analogWrite(motorIA2,255- val_right);//使直流电机（右）逆时针转,可调速
digitalWrite(motorIB2,HIGH);
flag= true;
}
//后退函数
void backup()
{
analogWrite(motorIA1,255- val_left);//使直流电机（左）逆时针转,可调速
digitalWrite(motorIB1,HIGH);
analogWrite(motorIA2, val_right);//使直流电机（右）顺时针转,可调速
digitalWrite(motorIB2,LOW);
flag= false;
}
//停止函数
void stopa()
{
digitalWrite(motorIA1,HIGH);//使直流电机（左）制动
digitalWrite(motorIB1,HIGH);
digitalWrite(motorIA2,HIGH);//使直流电机（右）制动
digitalWrite(motorIB2,HIGH);
}
void loop()
{
if(Serial.available() > 0) // 判断缓冲区中有无数据
{
code = Serial.read(); // 读取数据
switch(code)
{
case '1': advance(); break; //前进
case '2': backup(); break; //后退
case '5': stopa(); break; //停止
default:break;
}
}
new_time=millis();
if(abs(new_time - old_time) >= 1000) // 如果计时时间已达1秒
{
detachInterrupt(0); // 关闭外部中断0
detachInterrupt(1); // 关闭外部中断1
spdLft =(float)scode_L*60/20;//小车左车轮电机转速
spdRgt =(float)scode_R*60/20; //小车右车轮电机转速
Input=(spdLft-spdRgt);
myPID.Compute();
val_right=pwm-Output;
val_left=pwm+Output;
scode_L = 0; //恢复到编码器测速的初始状态
scode_R = 0;
old_time= millis();
attachInterrupt(0, Code1,FALLING); // 重新开放外部中断0，左
attachInterrupt(1, Code2,FALLING); // 重新开放外部中断1，右
}
}
|
发表于 2017-4-30 22:49:51
楼主