//2-2023MazeB: 2/13/2023 : KEY Predecessor Program: 1-2023MazeH:: //Modify Reverse function ROBOT WORKS EXCELLENTLY. // Variables for ReadSensors and CenterPath long SenseLO = 0; //Left Outer sensor reading long SenseLM = 0; //Left Middle sensor reading long SenseLI = 0; //Left line Sensor reading. Keep long; int no good long SenseRI = 0; //Right line Sensor reading Keep long; int no good long SenseRM = 0; //Right Middle Sensor reading Keep long; int no good long SenseRO = 0; //Right Outer sensor reading long CurPos; //Current position. Keep long; int no good int Error = 0; //Distance from center to current position-range -90 to +90 int MotorR; //Default motor value for right motor-range 0-255 for forward int MotorL; //Default motor value for left motor-range 0-255 for forward int BatV ; //Save battery voltage on power up. int RunNo = 1; //Initial run = 1; optimal run = 2. Use int for case option //Variables for saving path. int Path[30]; //Path direction saved as array int PathIndex = 0; void setup() { pinMode(2, INPUT_PULLUP); //Pin 2 is start switch, pulls low to start +++++++++ pinMode(8, OUTPUT); //Pin 8 is Yellow pinMode(7, OUTPUT); //Pin 7 is Red pinMode(11, OUTPUT); //Pin 11 is White pinMode(12, OUTPUT); //Pin 12 is Blue pinMode(5, OUTPUT); //Left motor direction pinMode(10, OUTPUT); //Right motor direction pinMode(3, OUTPUT); //Left motor PWM pinMode(9, OUTPUT); //Right motor PWM digitalWrite(5, LOW); //L motor forward digitalWrite(10, LOW); //R motor forward analogWrite(3, 0); //L Motor stop 0 to stop on forward; 255 on reverse analogWrite(9, 0); //R motor stop 0 to stop on forward; 255 on reverse // Serial.begin(9600); delay (500); //Wait for battery voltage to settle. BatV = analogRead(7); // Read battery voltage; new battery=4.85 volts if (BatV > 940) {RedOn(); } // Battery voltage > 4.6 volts else if (BatV > 880) {WhiteOn(); } // Battery voltage > 4.4 volts else if (BatV > 800) {BlueOn(); } // Battery voltage > 4.2 volts else YellowOn(); // Battery low, replace if robot misbehaves while (digitalRead(2) == HIGH) { //Wait for start switch } LEDoff(); //Turn off battery voltage LED delay(1000); //Wait to get hand clear of robot BotStart(); //Soft start of robot motors to avoid twists off path } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void loop(){ if (RunNo == 1) RunOne(); else if (RunNo == 2) RunTwo(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void RunOne() { ReadSensors(); //Test straight run CenterPath(); //Run straight until following--- while ((SenseLO < 60 && SenseRO < 60) && (SenseLI < 300 && SenseRI > 300)) { ReadSensors(); CenterPath(); } if (SenseLO > 300) { TurnLeft(); } //Left turn has priority else if (SenseLI < 60 && SenseRI < 60) { EOL(); } //Reverse and goal-End of Line else if (SenseRO > 300) { //Right turn lowest priority delay(30); //Delay to test for late left sense ReadSensors(); if (SenseLO > 300) { TurnLeft(); } //This places robot beyond wheels over line else RtCheck(); } } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void RunTwo() //Make optimized run { LEDoff(); // WhiteOn(); ReadSensors(); CenterPath(); while (SenseLO < 60 && SenseRO < 60) { //While both outer sensors see white ReadSensors(); CenterPath(); // YellowOn(); } { if (Path[PathIndex] == 1) { //Left turn TurnLeft(); } else if (Path[PathIndex] == 2) { //Straight WhiteOn(); // for (int K = 1; K <= 16; K++) { //Advance to clear turn // ReadSensors(); //Continue on straight run // CenterPath(); // } delay(400); LEDoff(); } else if (Path[PathIndex] == 3) { //Right turn delay(300); TurnRight(); } else if (Path[PathIndex] == 5) { //Goal Goal2(); } } PathIndex++; } //************************************************************** void ReadSensors() //Includes 5ms delay { digitalWrite(13, HIGH); //Turn on sensor LED's delay(5); SenseRO = analogRead(0); //Sensor 1, right most, A0 SenseRM = analogRead(1); //Sensor 2, right most, A1 SenseRI = analogRead(2); //Sensor 3, right center, A2 SenseLI = analogRead(3); //Sensor 4, left center, A3 SenseLM = analogRead(4); //Sensor 5, left center, A4 SenseLO = analogRead(5); //Sensor 6, left most, A5 digitalWrite(13, LOW); //Turn off sensor LED's delay(5); //Delay for 10msec read loop } //************************************************************** void CenterPath() //Navigate robot down center of line { static int Kp = 3; //PID proportional value; Kp=4 gives errors CurPos = ((SenseLI * 100) + (SenseRI * 1)) / (SenseLI + SenseRI); Error = 51 - CurPos; //On line center CurPos=51 Error = Error * Kp; //Kp is proportional factor for PID if (Error < 0) { //Right side error Error = -Error; MotorR = 100+ Error; //100 is standard PWM for straight analogWrite(9, MotorR); //Right motor MotorL = 102; //102 is standard PWM for straight analogWrite(3, MotorL); //Left motor } else if (Error > 0) { //Left side error MotorL = 102+ Error; //102 is standard PWM for straight analogWrite(3, MotorL); //Left motor MotorR = 100; //100 is standard PWM for straight analogWrite(9, MotorR); //Right motor } } //************************************************************** void TurnLeft() { delay (270); //Wait for wheels to center over line LEDoff(); RedOn(); //digitalWrite(5, HIGH); //L motor reverse--Rotate around axle center analogWrite(3, 0); //L speed stop left motor analogWrite(9, 50); //Rotate around right delay(600); ReadSensors(); //Continue rotate until right sensor sees black while (SenseRI < 150) { ReadSensors(); } digitalWrite(5, LOW); //L motor forward ReadSensors(); //Continue on CenterPath(); LEDoff(); // Save Path------------------------ if (RunNo == 1) { Path[PathIndex] = 1; Simplify(); PathIndex++; } } //************************************************************** void EOL() { delay(200); //Advance to check for goal ReadSensors(); if (SenseLO > 60 || SenseRO > 60) { Goal(); } else { Reverse(); } } //************************************************************** void Reverse() { // for (int K = 1; K <= 12; K++) { //No delay means radius goes from 2" to 1" // delay(20); // } LEDoff(); YellowOn(); digitalWrite(5, HIGH); //L motor reverse analogWrite(3, 150); //L speed analogWrite(9, 40); //R speed delay(600); ReadSensors(); //Continue rotate until right sensor sees black while (SenseRI < 150) { ReadSensors(); } digitalWrite(5, LOW); //L motor forward LEDoff(); // Save Path------------------------ if (RunNo == 1) { Path[PathIndex] = 4; Simplify(); PathIndex++; } } //************************************************************** void Goal() { digitalWrite(5, LOW); //L motor forward digitalWrite(10, LOW); //R motor forward analogWrite(3, 0); //L Motor stop 0 analogWrite(9, 0); //R motor stop 0 Path[PathIndex] = 5; //------------------------------Display path PathIndex = 0; while (Path[PathIndex] != 5) { LEDoff(); //Turn last LED off delay(500); if ( Path[PathIndex] == 1) RedOn(); //Left is red else if ( Path[PathIndex] == 2) WhiteOn(); //Straight is white else if ( Path[PathIndex] == 3) BlueOn(); //Right is blue else if ( Path[PathIndex] == 4) YellowOn(); //Reverse is yellow delay(1500); PathIndex++; } LEDoff(); delay(1000), YellowOn(); // Yellow at end of display while (digitalRead(2) == HIGH) { //Wait for start switch } RedOn(); PathIndex = 0; RunNo = 2; // while(1==1); //Stop delay(1000); } //************************************************************** void RtCheck() { delay (270); ReadSensors(); if (SenseLI > 300 || SenseRI > 300) Straight(); else TurnRight(); } //************************************************************** void TurnRight() { LEDoff(); BlueOn(); // digitalWrite(10, HIGH); //R motor reverse--Rotate analogWrite(3, 50); //L speed analogWrite(9, 0); //R speed delay(700); ReadSensors(); //Continue rotate until left sensor sees black while (SenseLI < 150) { ReadSensors(); } digitalWrite(10, LOW); //R motor forward ReadSensors(); //Continue on CenterPath(); LEDoff(); // Save Path------------------------ if (RunNo == 1) { Path[PathIndex] = 3; Simplify(); PathIndex++; } } //************************************************************** void Straight() { LEDoff(); WhiteOn(); // Save Path------------------------ if (RunNo == 1) { Path[PathIndex] = 2; Simplify(); PathIndex++; } for (int K = 1; K <= 25; K++) { //Delay used to clear turn after straight ReadSensors(); //Continue on CenterPath(); } LEDoff(); } //************************************************************** //************************************************************** void Goal2() { //Robot at goal second run analogWrite(9, 0); analogWrite(3, 0); RedOn(); // while (digitalRead(1) == HIGH) { //Wait for start switch second run // } // PathIndex = 0; while(1==1); } //************************************************************** void Simplify() { //Eliminates Reverses from dead ends if (Path[PathIndex-1] == 4) //Perform if previous char is a 'B' { if(Path[PathIndex-2]==1 && Path[PathIndex]==3) { Path[PathIndex-2]=4; } //LBR=>B else if(Path[PathIndex-2]==1 && Path[PathIndex]==2) { Path[PathIndex-2]=3; } //LBS=>R else if(Path[PathIndex-2]==1 && Path[PathIndex]==1) { Path[PathIndex-2]=2; } //LBL=>S else if(Path[PathIndex-2]==2 && Path[PathIndex]==1) { Path[PathIndex-2]=3; } //SBL=>R else if(Path[PathIndex-2]==2 && Path[PathIndex]==2) { Path[PathIndex-2]=4; } //SBS=>B else if(Path[PathIndex-2]==3 && Path[PathIndex]==1) { Path[PathIndex-2]=4; } //RBL=>B // Path[PathIndex-1] = 0; //Blank out two char removed by simplify // Path[PathIndex] = 0; PathIndex = PathIndex-2; //Move back two char for char removed } } //************************************************************** void RedOn() { digitalWrite(7, HIGH); } void WhiteOn() { digitalWrite(8, HIGH); } void BlueOn() { digitalWrite(12, HIGH); } void YellowOn() { digitalWrite(11, HIGH); } void LEDoff() { digitalWrite(12, LOW); //All LED's off digitalWrite(11, LOW); digitalWrite(7, LOW); digitalWrite(8, LOW); } //************************************************************** void BotStart() { //Softstart for robot motors for( int K=50; K<100; K++) { analogWrite(3, K); analogWrite(9, K); delay(10); } } //************************************************************** //End of Code*************************************************************