中科因仑“3+1”工程特种兵精英论坛

标题: arduino 追踪小车 [打印本页]
作者: 刘登辉    时间: 2015-5-24 18:48
标题: arduino 追踪小车
最近一直在尝试用舵机带动超声波传感器寻找目标，但由于扫描的角度和小车的转向精度不够，结果总是不尽人意。

来看段视频： http://v.youku.com/v_show/id_XMjUzNDcxNzky.html

于是修改了项目，将小车重构，放弃四个轮子改用两个（小车灵活性大大提高），以红外线为主，超声波为辅，从目前来看效果很不错。

思路是：

硬件列表：

来看小车的图片：
[attach]3504[/attach]

[attach]3505[/attach]

再来看视频： http://v.youku.com/v_show/id_XMjU0NDkzNDM2.html

最后是源代码：

  1. #include <MazeSensor.h>
  2. #include <BaseCar.h>
  3. #include <Ultrasonic.h>
  4. #include <Servo.h>



  8. // for Ultrasonic
  9. const int TRIGGER_PIN = 1;
  10. const int RESPONSE_PIN = 2;

  12. // for Maze Sensor
  13. const int LEFT_PIN = 7;
  14. const int MIDDLE_PIN = 4;
  15. const int RIGHT_PIN = 8;

  17. // for detect Servo
  18. const int DETECT_SERVO_PIN = 9;

  20. // for car
  21. const int I11_PIN = A1;  // 15
  22. const int I12_PIN = A2;  // 16
  23. const int I21_PIN = A3;  // 17
  24. const int I22_PIN = A4;  // 18
  25. const int MOTO1_PIN = 3;
  26. const int MOTO2_PIN = 11;





  32. // log
  33. const boolean VERBOSE = true;
  34. const int SERIAL_SPEED = 9600;

  36. // for Ultrasonic
  37. const int MAX_DISTANCE = 300;
  38. const double C = 0.96;

  40. // for detect Servo
  41. const int DETECT_SERVO_CENTER_POS = 80;

  43. // speed
  44. const int SPEED_VAL = 150;
  45. const int SPEED_VAL_SLOW = 100;

  47. // direction adjustment
  48. const int ADJ = 0;





  54. // sensor
  55. MazeSensor sensor(LEFT_PIN, MIDDLE_PIN, RIGHT_PIN);

  57. // car control
  58. BaseCar car(MOTO1_PIN, MOTO2_PIN, I11_PIN, I12_PIN, I21_PIN, I22_PIN);

  60. // ultrasonic
  61. Ultrasonic us(TRIGGER_PIN, RESPONSE_PIN, C);

  63. // servo
  64. Servo servo;

  66. boolean targetFound = false;




  71. void setup() {
  72.   // init servo
  73.   servo.attach(DETECT_SERVO_PIN);
  74.   changeDetectServoPos(DETECT_SERVO_CENTER_POS);
  75.   // log
  76.   if (VERBOSE) {
  77.     Serial.begin(SERIAL_SPEED);
  78.   }
  79. }


  82. void loop() {
  83.   int distance = us.getDistance();
  84.   int pos = detectBlockPos();
  85.   if (VERBOSE) {
  86.     Serial.print("pos = ");
  87.     Serial.print(pos);
  88.     Serial.print(", distance = ");
  89.     Serial.println(distance);
  90.   }
  91.   if (pos == 0) {
  92.     if (distance > MAX_DISTANCE) {
  93.       Serial.println("target lost, searching...");
  94.       car.rotateRight(SPEED_VAL_SLOW, ADJ);
  95.     } else {
  96.       Serial.println("target found but leaving now, following...");
  97.       car.forward(SPEED_VAL_SLOW, ADJ);
  98.     }
  99.   } else {
  100.     switch(pos) {
  101.       case 1: // left
  102.         Serial.println("target on left, turn left...");
  103.         car.rotateLeft(SPEED_VAL, ADJ);
  104.         break;
  105.       case 3: // right
  106.         Serial.println("target on right, turn left...");
  107.         car.rotateRight(SPEED_VAL, ADJ);
  108.         break;
  109.       case 2: // middle
  110.         // goal
  111.         Serial.println("target caught, haha!");
  112.         car.standBy();
  113.         break;
  114.     }
  115.   }
  116. }


  119. int detectBlockPos() {
  120.   // get sensor status
  121.   SensorStatus s = sensor.getStatus();
  122.   if ((!s.left_flag) && (!s.middle_flag) && (!s.right_flag)) {
  123.     // 0:no
  124.     return 0;
  125.   } else if ((s.left_flag) && (!s.middle_flag) && (!s.right_flag)) {
  126.     // 1:left
  127.     return 1;
  128.   } else if ((!s.left_flag) && (s.middle_flag) && (!s.right_flag)) {
  129.     // 2:middle
  130.     return 2;
  131.   } else if ((!s.left_flag) && (!s.middle_flag) && (s.right_flag)) {
  132.     // 3:right
  133.     return 3;
  134.   } else if ((s.left_flag) && (s.middle_flag) && (s.right_flag)) {
  135.     // 4:all
  136.     return 4;
  137.   } else if ((s.left_flag) && (s.middle_flag) && (!s.right_flag)) {
  138.     // 5:left & middle
  139.     return 5;
  140.   } else if ((!s.left_flag) && (s.middle_flag) && (s.right_flag)) {
  141.     // 6:right & middle
  142.     return 6;
  143.   }
  144. }


  147. void changeDetectServoPos(int pos) {
  148.   servo.write(pos);
  149. }
复制代码







欢迎光临 中科因仑“3+1”工程特种兵精英论坛 (http://bbs.enlern.com/) Powered by Discuz! X3.4