电机驱动采用的是DRV8833直流驱动模块,可以直接替换TB6612驱动模块,管脚基本完全兼容。
我把4路红外循线模块的高低电平分别对应1和0,然后有轻度偏离(0010,0100)、中度偏离(1100,1110,0111,0011)、重度偏离(1000和0001)这几种情况,然后根据偏离程度不同分别采取不同的误差err,左右偏离分别取正负,因为PWM范围在0~255,因此为了防止err过大或过小导致PWM大于255或者小于0,故需要添加if语句比如:当PWM大于130或者小于10时直接令PWM为你设定的阈值。
当然也可以不用PID算法,但是用了PID小车更加稳定不需要频繁的去更改速度来应对不同的路线,反之用了PID算法可以应对许多情况当你把KP,KI,KD设置好后可以应对许多不同的路线不需要再频繁的去调节速度。我这里只用了PD,所以强烈推荐学习PID。
代码如下:
int Left_motor_back=2; //左电机后退
int Left_motor_go=3; //左电机前进
int Right_motor_back=4; // 右电机后退
int Right_motor_go=5; // 右电机前进
const int SensorRight1=6;//红外循线模块
const int SensorRight2=7;
const int SensorLeft1=8;
const int SensorLeft2=9;
int S1;
int S2;
int S3;
int S4;
float err,last_err=0;
float P,D;
int v=70;//基础速度可以自己调节
int kp=20,kd=10;//系数需要自己调节
void setup()
{
//初始化电机驱动IO为输出方式
pinMode(Left_motor_go,OUTPUT);
pinMode(Left_motor_back,OUTPUT);
pinMode(Right_motor_go,OUTPUT);
pinMode(Right_motor_back,OUTPUT);
pinMode(SensorRight1, INPUT);
pinMode(SensorRight2, INPUT);
pinMode(SensorLeft1, INPUT);
pinMode(SensorLeft2, INPUT);
}
//=======================智能小车的基本动作=========================
void run(int x)//小车驱动
{
int v1,v2;
v1=v-x;
v2=v+x;
if(v1>=130)
v1==130;
if(v1<=10)
v1==10;
if(v2>=130)
v2==130;
if(v2<=10)
v2==10;
digitalWrite(Right_motor_back,LOW);//控制右电机
digitalWrite(Right_motor_go,HIGH);
analogWrite(Right_motor_back,0);
analogWrite(Right_motor_go,v1);
digitalWrite(Left_motor_back,LOW);//控制左电机
digitalWrite(Left_motor_go,HIGH);
analogWrite(Left_motor_back,0);
analogWrite(Left_motor_go,v2);
delay(5); //执行时间,可以调整
}
void brake()//电机停止,刹车
{
digitalWrite(Right_motor_go,LOW);
digitalWrite(Right_motor_back,LOW);
digitalWrite(Left_motor_go,LOW);
digitalWrite(Left_motor_back,LOW);
}
int pid()//PD算法
{
int output;
P=err;
D=err-last_err;
output=kp*P+kd*D;
last_err=err;
return output;
}
void loop()
{
S1 = digitalRead(SensorRight1);
S2 = digitalRead(SensorRight2);
S3 = digitalRead(SensorLeft1);
S4 = digitalRead(SensorLeft2);
//一共8种情况
if(S1==LOW&&S2==LOW&&S3==LOW&&S4==LOW)//0000此时小车未在线上电机停止
brake();
else
{
if(S1==HIGH&&S2==LOW&&S3==LOW&&S4==LOW)//1000小车重度偏右,需要向左大幅度偏转
err=3;
else if((S1==HIGH&&S2==HIGH&&S3==LOW&&S4==LOW)||(S1==HIGH&&S2==HIGH&&S3==HIGH&&S4==LOW))//1100 1110小车中度偏右
err=2;
else if(S1==LOW&&S2==HIGH&&S3==LOW&&S4==LOW)//0100小车轻度偏右
err=1;
else if((S1==LOW&&S2==HIGH&&S3==HIGH&&S4==LOW)||(S1==HIGH&&S2==HIGH&&S3==HIGH&&S4==HIGH))//0110 1111小车居中直行
err=0;
else if(S1==LOW&&S2==LOW&&S3==HIGH&&S4==LOW)//0010小车轻度偏左
err=-1;
else if((S1==LOW&&S2==LOW&&S3==HIGH&&S4==HIGH)||(S1==LOW&&S2==HIGH&&S3==HIGH&&S4==HIGH))//0011 0111小车中度偏左
err=-2;
else if(S1==LOW&&S2==LOW&&S3==LOW&&S4==HIGH)//0001小车重度偏左,需要向右大幅度偏转
err=-3;
run(pid());
}
}