极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 1779|回复: 4

循迹小车,PID控制

  [复制链接]
发表于 2017-7-14 23:39:38 | 显示全部楼层 |阅读模式
本帖最后由 donglaile 于 2017-8-2 10:40 编辑

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

nano控制板

nano控制板

l9110s电机驱动

l9110s电机驱动

底盘

底盘

光电对管

光电对管

原理图大概这样,没找到L9110s,用L298画了,理解意思就行:
UC截图20170714230743.png
电池使用两节锂电池串联,用了个5A的可调降压模块给电机模块供电。这里主控另外供电,用以前剩下的带移动电源功能项目板子5V升压的。
特别注意一点光电对管的安装间隔最好是黑胶带的宽度,竖着安装(就是模块PCB与小车前进方向平行),用万能的热熔胶粘在废弃的芯片壳上再粘贴到小车前面的。
来张错误安装光电对管的图:
0_IMG_20170112_200701.jpg
之前这样安装一直没搞好。
控制代码:

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

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

  15. void read_ir_values(void);
  16. void calculate_pid(void);
  17. void motor_control(void);

  18. void setup()
  19. {
  20.   // 初始化
  21.   pinMode(IN_A1, OUTPUT);
  22.   pinMode(IN_A2, OUTPUT);
  23.   pinMode(IN_B1, OUTPUT);
  24.   pinMode(IN_B2, OUTPUT);
  25.   for (int i = 0; i < 5; i++) {
  26.     pinMode(IR_PIN[i], INPUT);
  27.   }
  28.   Serial.begin(9600); //调试用
  29. }

  30. void loop()
  31. {
  32.   read_ir_values();
  33.   calculate_pid();
  34.   motor_control();
  35.   print_debug();
  36.   //    motorsWrite(255,255); // 调试电机
  37.   //    delay(3000);
  38.   //    motorsWrite(-255,-255);
  39.   //    delay(3000);
  40. }

  41. void read_ir_values()
  42. {
  43.   for (int i = 4; i < 9; i++) {
  44.     if (digitalRead(IR_PIN[i])) {
  45.       bitSet(irs, (i - 4));
  46.     } else {
  47.       bitClear(irs, (i - 4));
  48.     }
  49.   }

  50.   switch (irs) {
  51.     case B00000:
  52.       if (error < 0) {
  53.         error = -9;
  54.       } else {
  55.         error = 9;
  56.       }
  57.       break;
  58.     case B00001: error = -7; break;
  59.     case B00011: error = -5; break;
  60.     case B00010: error = -3; break;
  61.     case B00110: error = -1; break;
  62.     case B00100: error = 0; break;
  63.     case B01100: error = 1; break;
  64.     case B01000: error = 3; break;
  65.     case B11000: error = 5; break;
  66.     case B10000: error = 7; break;
  67.       //对以上值进行处理,其他的就自由发挥
  68.   }
  69. }

  70. void calculate_pid()
  71. {
  72.   P = error;
  73.   I = I + error;
  74.   D = error - previous_error;

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

  77.   previous_error = error;
  78. }

  79. void motor_control()
  80. {
  81.   //计算每个电机的速度
  82.   left_motor_speed = initial_motor_speed - PID_value;
  83.   right_motor_speed = initial_motor_speed + PID_value;

  84.   constrain(left_motor_speed, -255, 255); //速度限定在(-255,255)
  85.   constrain(right_motor_speed, -255, 255);
  86.   motorsWrite(left_motor_speed, right_motor_speed);
  87. }

  88. //速度设定范围(-255,255)
  89. void motorsWrite(int speedL, int speedR)
  90. {
  91.   if (speedR > 0) {
  92.     analogWrite(IN_A1, speedR);
  93.     analogWrite(IN_A2, 0);
  94.   } else {
  95.     analogWrite(IN_A1, 0);
  96.     analogWrite(IN_A2, -speedR);
  97.   }

  98.   if (speedL > 0) {
  99.     analogWrite(IN_B1, speedL);
  100.     analogWrite(IN_B2, 0);
  101.   } else {
  102.     analogWrite(IN_B1, 0);
  103.     analogWrite(IN_B2, -speedL);
  104.   }
  105. }

  106. void print_debug()
  107. {
  108.   // 打印串口调试信息
  109.   Serial.print("IRS:");
  110.   String irs2bin = String(irs, 2);
  111.   int len = irs2bin.length();
  112.   if(len < 5){
  113.     for(int i=0;i<5-len;i++){
  114.       irs2bin += "0" + irs2bin;
  115.     }
  116.   }
  117.   Serial.print(irs2bin);
  118.   Serial.print("   ML:");
  119.   Serial.print(left_motor_speed, OCT);
  120.   Serial.print(" MR:");
  121.   Serial.print(right_motor_speed, OCT);
  122.   Serial.print(" er:");
  123.   Serial.print(error, OCT);
  124. //  Serial.print(" P:");
  125. //  Serial.print(Kp, OCT);
  126.   Serial.print(" PID:");
  127.   Serial.print(PID_value, OCT);
  128.   Serial.println();
  129. }
复制代码

运行视频:
1.速度较慢的,忘记初速度设的多少了
http://v.youku.com/v_show/id_XMjg5MjE5MTc3Mg==.html?spm=a2h3j.8428770.3416059.1

2.这个是设置140初速度的,比较前面的快点
http://v.youku.com/v_show/id_XMjg5MjE5Mzk2MA==.html?spm=a2h3j.8428770.3416059.1
回复

使用道具 举报

发表于 2017-7-15 12:19:52 | 显示全部楼层
就这么几个误差值还不如直接手工算存参数呢,省掉大堆的代码呢
回复 支持 0 反对 1

使用道具 举报

发表于 2017-8-17 16:58:34 | 显示全部楼层
楼主,学习啦,好东西
回复 支持 反对

使用道具 举报

发表于 2017-8-21 10:31:39 | 显示全部楼层
很有借鉴的制作
我也准备搞一个学习学习
回复 支持 反对

使用道具 举报

发表于 2017-8-28 15:35:06 | 显示全部楼层
楼主,你这段代码编译不了呀?
回复 支持 反对

使用道具 举报

高级模式  
您需要登录后才可以回帖 登录 | 注册

本版积分规则

Archiver|联系我们|极客工坊 ( 浙ICP备09023225号 )

GMT+8, 2017-11-23 21:06 , Processed in 0.044948 second(s), 8 queries , File On.

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

快速回复 返回顶部 返回列表