首页 › Arduino › 循迹小车，PID控制

donglaile 发表于 2017-7-14 23:39:38

循迹小车，PID控制

本帖最后由 donglaile 于 2017-8-2 10:40 编辑

现在x宝上电子模块真心便宜，前段时间心血来潮，买模块拼了个循迹小车玩玩,50块左右就搞定了。




原理图大概这样，没找到L9110s，用L298画了，理解意思就行：

电池使用两节锂电池串联，用了个5A的可调降压模块给电机模块供电。这里主控另外供电，用以前剩下的带移动电源功能项目板子5V升压的。
特别注意一点光电对管的安装间隔最好是黑胶带的宽度，竖着安装（就是模块PCB与小车前进方向平行），用万能的热熔胶粘在废弃的芯片壳上再粘贴到小车前面的。
来张错误安装光电对管的图：

之前这样安装一直没搞好。
控制代码：

//电机电压：6.1V左右的参数，如果不加积分项，可以循线跑，但是无法回到中间传感器位置。
float Kp = 10, Ki = 0.02, Kd = 20;
float error = 0, P = 0, I = 0, D = 0, PID_value = 0;
float previous_error = 0, previous_I = 0;
static int initial_motor_speed = 100; //期望速度
int left_motor_speed = 0;
int right_motor_speed = 0;
uint8_t irs = 0;

//引脚定义
const int IR_PIN[] = {A0, A1, A2, A3, A4}; //红外对管引脚定义
const int IN_A1 = 3;   // 电机A1
const int IN_A2 = 9;   // 电机A2
const int IN_B1 = 10;// 电机B1
const int IN_B2 = 11;// 电机B2

void read_ir_values(void);
void calculate_pid(void);
void motor_control(void);

void setup()
{
// 初始化
pinMode(IN_A1, OUTPUT);
pinMode(IN_A2, OUTPUT);
pinMode(IN_B1, OUTPUT);
pinMode(IN_B2, OUTPUT);
for (int i = 0; i < 5; i++) {
    pinMode(IR_PIN, INPUT);
}
Serial.begin(9600); //调试用
}

void loop()
{
read_ir_values();
calculate_pid();
motor_control();
print_debug();
//    motorsWrite(255,255); // 调试电机
//    delay(3000);
//    motorsWrite(-255,-255);
//    delay(3000);
}

void read_ir_values()
{
for (int i = 4; i < 9; i++) {
    if (digitalRead(IR_PIN)) {
      bitSet(irs, (i - 4));
    } else {
      bitClear(irs, (i - 4));
    }
}

switch (irs) {
    case B00000:
      if (error < 0) {
      error = -9;
      } else {
      error = 9;
      }
      break;
    case B00001: error = -7; break;
    case B00011: error = -5; break;
    case B00010: error = -3; break;
    case B00110: error = -1; break;
    case B00100: error = 0; break;
    case B01100: error = 1; break;
    case B01000: error = 3; break;
    case B11000: error = 5; break;
    case B10000: error = 7; break;
      //对以上值进行处理，其他的就自由发挥
}
}

void calculate_pid()
{
P = error;
I = I + error;
D = error - previous_error;

PID_value = (Kp * P) + (Ki * I) + (Kd * D);
PID_value = constrain(PID_value, -100, 100);

previous_error = error;
}

void motor_control()
{
//计算每个电机的速度
left_motor_speed = initial_motor_speed - PID_value;
right_motor_speed = initial_motor_speed + PID_value;

constrain(left_motor_speed, -255, 255); //速度限定在(-255，255)
constrain(right_motor_speed, -255, 255);
motorsWrite(left_motor_speed, right_motor_speed);
}

//速度设定范围(-255,255)
void motorsWrite(int speedL, int speedR)
{
if (speedR > 0) {
    analogWrite(IN_A1, speedR);
    analogWrite(IN_A2, 0);
} else {
    analogWrite(IN_A1, 0);
    analogWrite(IN_A2, -speedR);
}

if (speedL > 0) {
    analogWrite(IN_B1, speedL);
    analogWrite(IN_B2, 0);
} else {
    analogWrite(IN_B1, 0);
    analogWrite(IN_B2, -speedL);
}
}

void print_debug()
{
// 打印串口调试信息
Serial.print("IRS:");
String irs2bin = String(irs, 2);
int len = irs2bin.length();
if(len < 5){
    for(int i=0;i<5-len;i++){
      irs2bin += "0" + irs2bin;
    }
}
Serial.print(irs2bin);
Serial.print("   ML:");
Serial.print(left_motor_speed, OCT);
Serial.print(" MR:");
Serial.print(right_motor_speed, OCT);
Serial.print(" er:");
Serial.print(error, OCT);
//Serial.print(" P:");
//Serial.print(Kp, OCT);
Serial.print(" PID:");
Serial.print(PID_value, OCT);
Serial.println();
}

运行视频：
1.速度较慢的，忘记初速度设的多少了
http://v.youku.com/v_show/id_XMjg5MjE5MTc3Mg==.html?spm=a2h3j.8428770.3416059.1
http://player.youku.com/player.php/sid/XMjg5MjE5MTc3Mg==/v.swf
2.这个是设置140初速度的，比较前面的快点
http://v.youku.com/v_show/id_XMjg5MjE5Mzk2MA==.html?spm=a2h3j.8428770.3416059.1
http://player.youku.com/player.php/sid/XMjg5MjE5Mzk2MA==/v.swf

Dreamer-Men 发表于 2017-7-15 12:19:52

就这么几个误差值还不如直接手工算存参数呢，省掉大堆的代码呢

neebourne 发表于 2017-8-17 16:58:34

楼主，学习啦，好东西

5free 发表于 2017-8-21 10:31:39

很有借鉴的制作
我也准备搞一个学习学习

橘子茉莉 发表于 2017-8-28 15:35:06

楼主，你这段代码编译不了呀？

破石头 发表于 2018-4-29 15:46:03

你好在吗，有个问题请教你，输入口定义的是0-4模拟口，读取怎么是4-9口呢

JasonChing 发表于 2018-11-7 16:47:10

本帖最后由 JasonChing 于 2018-11-7 19:59 编辑

破石头 发表于 2018-4-29 15:46
你好在吗，有个问题请教你，输入口定义的是0-4模拟口，读取怎么是4-9口呢

这里代表传感器的连接端口，是4，5，6，7，8

查看完整版本: 循迹小车，PID控制