Maze path search robot : mainFunction.ino

Please copy and paste it into Arduino IDE.
  1. #include "BasicStepperDriver.h"
  2. #include "SyncDriver.h"
  3. # define STOPPED 0
  4. # define FOLLOWING_LINE 1
  5. # define NO_LINE 2
  6. # define CONT_LINE 3
  7. # define POS_LINE 4
  8. # define RIGHT_TURN 5
  9. # define LEFT_TURN 6
  10. #define MOTOR_STEPS 200
  11. #define L_Motor_RPM 40 //speed
  12. #define R_Motor_RPM 40 //speed
  13. #define MICROSTEPS 16
  14. #define ENB_Pin 19
  15. #define L_Motor_DIR 26
  16. #define L_Motor_STEP 33
  17. #define R_Motor_DIR 32
  18. #define R_Motor_STEP 25
  19. BasicStepperDriver L_Motor(MOTOR_STEPS, L_Motor_DIR, L_Motor_STEP);
  20. BasicStepperDriver R_Motor(MOTOR_STEPS, R_Motor_DIR, R_Motor_STEP);
  21. SyncDriver controller(L_Motor, R_Motor);
  22. float wheel_dia = 68.9; // # mm (increase = spiral out)
  23. float wheel_base = 74.0; // # mm (increase = spiral in, ccw)
  24. int steps_rev = 3200; // # 200*16 MICROSTEPS
  25. // LFS more to the Left is "0"
  26. const int lineFollowSensor0 = 18;
  27. const int lineFollowSensor1 = 5;
  28. const int lineFollowSensor2 = 17;
  29. const int lineFollowSensor3 = 16;
  30. const int lineFollowSensor4 = 4;
  31. int LFS[5] = {0, 0, 0, 0, 0};
  32. unsigned char dir;
  33. char path[100] = "";
  34. unsigned char pathLength = 0; // the length of the path
  35. int pathIndex = 0;
  36. unsigned int status = 0; // solving = 0; reach end = 1
  37. int EndCount = 0;
  38. String command;
  39. int mode = 0;
  40. const int ledPin = 27;
  41. const int buttonPin = 23;
  42. //---------------------------------------------
  43. void setup() {
  44.   Serial.begin(115200);
  45.   pinMode(buttonPin, INPUT_PULLUP); // Start Button
  46.   pinMode(lineFollowSensor0, INPUT); // Left out side sensor
  47.   pinMode(lineFollowSensor1, INPUT); // Left in side sensor
  48.   pinMode(lineFollowSensor2, INPUT); // Center sensor
  49.   pinMode(lineFollowSensor3, INPUT); // Right in side sensor
  50.   pinMode(lineFollowSensor4, INPUT); // Right out side sensor
  51.   pinMode(ENB_Pin, OUTPUT); digitalWrite(ENB_Pin, HIGH);
  52.   pinMode(ledPin, OUTPUT);
  53.   L_Motor.begin(L_Motor_RPM, MICROSTEPS);
  54.   R_Motor.begin(R_Motor_RPM, MICROSTEPS);
  55.   while (digitalRead(buttonPin)) {
  56.     command = "";
  57.   }
  58.   mode = 0;
  59.   status = 0; // 1st pass
  60. }
  61. void loop() {
  62.   ledBlink(1);
  63.   Serial.println("Start First Pass");
  64.   readLFSsensors();
  65.   mazeSolve(); // First pass to solve the maze
  66.   ledBlink(2);
  67.   Serial.println("End First Pass");
  68.   
  69.    while (digitalRead(buttonPin)){
  70.     command = "";
  71.   }
  72.   
  73.   pathIndex = 0;
  74.   status = 0;
  75.   mazeOptimization(); //run the maze as fast as possible
  76.   ledBlink(3);
  77.   Serial.println("End 2nd Pass");
  78.   while (digitalRead(buttonPin)){
  79.     command = "";
  80.   }
  81.      
  82.   mode = STOPPED;
  83.   status = 0; // 1st pass
  84.   pathIndex = 0;
  85.   pathLength = 0;
  86. }

by Paradise
元のページヘ戻る