这一期为创客们带来基于PID算法的循迹小车制作
1.标准赛道示意图：
（该赛道包含了：左直角、右直角、十字路口等路况）
2.灰度传感器安装示意图：
（可根据实际情况调节各传感器之间的间距）
3.硬件原理图：
4.控制逻辑：
5.程序如下：
#define leftA_PIN 7
#define leftB_PIN 6
#define left_Pwm_PIN 5
#define STBY 8
#define rightA_PIN 9
#define rightB_PIN 10
#define right_Pwm_PIN 11
#define leftA_track_PIN 2
#define leftB_track_PIN 3
#define middle_track_PIN 4
#define rightA_track_PIN 12
#define rightB_track_PIN 13
float Kp = 10, Ki = 0.5, Kd = 0; //pid弯道参数参数
float error = 0, P = 0, I = 0, D = 0, PID_value = 0; //pid直道参数
float decide = 0; //元素判断
float previous_error = 0, previous_I = 0; //误差值
int sensor[5] = {
0, 0, 0, 0, 0}; //5个传感器数值的数组
static int initial_motor_speed = 60; //初始速度
void read_sensor_values(void); //读取初值
void calc_pid(void); //计算pid
void motor_control(void); //电机控制
void motor_pinint( ); //引脚初始化
void _stop(); //停车
void setup()
{
Serial.begin(9600); //串口波特率115200（PC端使用）
track_pinint(); //循迹引脚初始化
motor_pinint(); //电机引脚初始化
}
void loop()
{
read_sensor_values(); //循迹小车
calc_pid();
motor_control();
}
/*循迹模块引脚初始化*/
void track_pinint()
{
pinMode (leftA_track_PIN, INPUT); //设置引脚为输入引脚
pinMode (leftB_track_PIN, INPUT); //设置引脚为输入引脚
pinMode (middle_track_PIN, INPUT);//设置引脚为输入引脚
pinMode (rightA_track_PIN, INPUT); //设置引脚为输入引脚
pinMode (rightB_track_PIN, INPUT); //设置引脚为输入引脚
}
/*电机引脚初始化*/
void motor_pinint()
{
pinMode (leftA_PIN, OUTPUT); //设置引脚为输出引脚
pinMode (leftB_PIN, OUTPUT); //设置引脚为输出引脚
pinMode (left_Pwm_PIN, OUTPUT); //设置引脚为输出引脚
pinMode (STBY, OUTPUT); //设置引脚为输出引脚
pinMode (rightA_PIN, OUTPUT); //设置引脚为输出引脚
pinMode (rightB_PIN, OUTPUT); //设置引脚为输出引脚
pinMode (right_Pwm_PIN, OUTPUT); //设置引脚为输出引脚
}
//停车函数
void _stop()
{
digitalWrite(leftA_PIN, LOW);
digitalWrite(leftB_PIN, LOW);
analogWrite(left_Pwm_PIN,255); //左轮静止不动
digitalWrite(rightA_PIN, LOW);
digitalWrite(rightB_PIN, LOW);
analogWrite(right_Pwm_PIN,255); //右轮静止不动
}
//速度设定范围(-255,255)
void motorsWrite(int speedL, int speedR)
{
digitalWrite(STBY, HIGH);
if (speedR > 0)
{
digitalWrite(rightA_PIN, HIGH);
digitalWrite(rightB_PIN, LOW);
analogWrite(right_Pwm_PIN,speedR);
}
else
{
digitalWrite(rightA_PIN, LOW);
digitalWrite(rightB_PIN, HIGH);
analogWrite(right_Pwm_PIN,-speedR);
}
if (speedL > 0)
{
digitalWrite(leftA_PIN, HIGH);
digitalWrite(leftB_PIN, LOW);
analogWrite(left_Pwm_PIN,speedL);
}
else
{
digitalWrite(leftA_PIN, LOW);
digitalWrite(leftB_PIN, HIGH);
analogWrite(left_Pwm_PIN,-speedL);
}
}
//速度设定范围(-100，100)
void motorsWritePct(int speedLpct, int speedRpct)
{
//speedLpct, speedRpct ranges from -100 to 100
motorsWrite(speedLpct * 2.55, speedRpct * 2.55);
}
void motorsStop()
{
digitalWrite(leftA_PIN, LOW);
digitalWrite(leftB_PIN, LOW);
analogWrite(left_Pwm_PIN,255); //左轮静止不动
digitalWrite(rightA_PIN, LOW);
digitalWrite(rightB_PIN, LOW);
analogWrite(right_Pwm_PIN,255); //右轮静止不动
}
void read_sensor_values()
{
sensor[0] = digitalRead(leftA_track_PIN);
sensor[1] = digitalRead(leftB_track_PIN);
sensor[2] = digitalRead(middle_track_PIN);
sensor[3] = digitalRead(rightA_track_PIN);
sensor[4] = digitalRead(rightB_track_PIN);
if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1))
{
decide = 1;//十字路口 1 1 1 1 1 直行
}
else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0))
{
decide = 1;//十字路口 0 1 1 1 0 直行
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1))
{
decide = 2;//90度路口 0 0 1 1 1 右转90度
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0))
{
decide = 2;//90度路口 0 0 1 1 0 右转90度
}
else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
{
decide = 3;//90度路口 1 1 1 0 0 左转90度
}
else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
{
decide = 3;//90度路口 0 1 1 0 0 左转90度
}
else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
{
decide = 3;//向上锐角 0 1 1 0 0 向上锐角
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1))
{
error = 2;// 0 0 0 0 1
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0))
{
error = 1;// 0 0 0 1 0
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
{
error = 0;// 0 0 1 0 0
}
else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
{
error = -1;// 0 1 0 0 0
}
else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
{
error = -2;// 1 0 0 0 0
}
else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
{
if (error == -2)
{
// 0 0 0 0 0
error = -3;
}
else
{
error = 3;
}
}
}
void calc_pid()
{
P = error;
I = I + error;
D = error - previous_error;
PID_value = (Kp * P) + (Ki * I) + (Kd * D);
previous_error = error;
}
void motor_control()
{
int left_motor_speed = initial_motor_speed + PID_value;
int right_motor_speed = initial_motor_speed + PID_value;
if(left_motor_speed < -255)
{
left_motor_speed = -255;
}
if(left_motor_speed > 255)
{
left_motor_speed = 255;
}
motorsWrite(left_motor_speed,right_motor_speed);
Serial.print("move_A: ");
Serial.print(left_motor_speed, OCT);
Serial.print(" move_B: ");
Serial.print(right_motor_speed, OCT);
Serial.print(" error: ");
Serial.print(error, OCT);
Serial.print(" P: ");
Serial.print(Kp, OCT);
Serial.print(" PID_value: ");
Serial.print(PID_value, OCT);
Serial.println();
}
传送门：https://blog.csdn.net/qq_38351824/article/details/81275262
详情请关注亿航创客官方抖音账号：EhangGroup
今天的文章基于PID算法的循迹小车分享到此就结束了，感谢您的阅读，如果确实帮到您，您可以动动手指转发给其他人。
版权声明：本文内容由互联网用户自发贡献，该文观点仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 举报，一经查实，本站将立刻删除。
如需转载请保留出处：https://bianchenghao.cn/27897.html
