- #include "BasicStepperDriver.h"
- #include "SyncDriver.h"
- # define STOPPED 0
- # define FOLLOWING_LINE 1
- # define NO_LINE 2
- # define CONT_LINE 3
- # define POS_LINE 4
- # define RIGHT_TURN 5
- # define LEFT_TURN 6
- #define MOTOR_STEPS 200
- #define L_Motor_RPM 40 //speed
- #define R_Motor_RPM 40 //speed
- #define MICROSTEPS 16
- #define ENB_Pin 19
- #define L_Motor_DIR 26
- #define L_Motor_STEP 33
- #define R_Motor_DIR 32
- #define R_Motor_STEP 25
- BasicStepperDriver L_Motor(MOTOR_STEPS, L_Motor_DIR, L_Motor_STEP);
- BasicStepperDriver R_Motor(MOTOR_STEPS, R_Motor_DIR, R_Motor_STEP);
- SyncDriver controller(L_Motor, R_Motor);
- float wheel_dia = 68.9; // # mm (increase = spiral out)
- float wheel_base = 74.0; // # mm (increase = spiral in, ccw)
- int steps_rev = 3200; // # 200*16 MICROSTEPS
- // LFS more to the Left is "0"
- const int lineFollowSensor0 = 18;
- const int lineFollowSensor1 = 5;
- const int lineFollowSensor2 = 17;
- const int lineFollowSensor3 = 16;
- const int lineFollowSensor4 = 4;
- int LFS[5] = {0, 0, 0, 0, 0};
- unsigned char dir;
- char path[100] = "";
- unsigned char pathLength = 0; // the length of the path
- int pathIndex = 0;
- unsigned int status = 0; // solving = 0; reach end = 1
- int EndCount = 0;
- String command;
- int mode = 0;
- const int ledPin = 27;
- const int buttonPin = 23;
- //---------------------------------------------
- void setup() {
- Serial.begin(115200);
- pinMode(buttonPin, INPUT_PULLUP); // Start Button
- pinMode(lineFollowSensor0, INPUT); // Left out side sensor
- pinMode(lineFollowSensor1, INPUT); // Left in side sensor
- pinMode(lineFollowSensor2, INPUT); // Center sensor
- pinMode(lineFollowSensor3, INPUT); // Right in side sensor
- pinMode(lineFollowSensor4, INPUT); // Right out side sensor
- pinMode(ENB_Pin, OUTPUT); digitalWrite(ENB_Pin, HIGH);
- pinMode(ledPin, OUTPUT);
- L_Motor.begin(L_Motor_RPM, MICROSTEPS);
- R_Motor.begin(R_Motor_RPM, MICROSTEPS);
- while (digitalRead(buttonPin)) {
- command = "";
- }
- mode = 0;
- status = 0; // 1st pass
- }
- void loop() {
- ledBlink(1);
- Serial.println("Start First Pass");
- readLFSsensors();
- mazeSolve(); // First pass to solve the maze
- ledBlink(2);
- Serial.println("End First Pass");
-
- while (digitalRead(buttonPin)){
- command = "";
- }
-
- pathIndex = 0;
- status = 0;
- mazeOptimization(); //run the maze as fast as possible
- ledBlink(3);
- Serial.println("End 2nd Pass");
- while (digitalRead(buttonPin)){
- command = "";
- }
-
- mode = STOPPED;
- status = 0; // 1st pass
- pathIndex = 0;
- pathLength = 0;
- }
|