//RIGHT-WALL-FOLLOWING ROBOT // the AIR430BoostFCC library uses the SPI library internally. Energia does not // copy the library to the output folder unless it is referenced here. // the order of includes is also important due to this fact. #include #include #include // pins used by Radio Module: 3.3V, P6.5, P3.2, P3.1, P3.0, P2.2, GND // make sure that you don't use these pins except for 3V3 and the GND when using the radio module // declare the I/O pins on the microcontroller used for all sensors // ultrasonic sensor has trigger and echo pins whereas IR sensor only has 1 signal pin // trigger pin can be attached to a digital or analog I/O pin // echo and IR signal pin must be attached to an analog I/O pin // L = left, F = front, R = right const int LTrig = P1_2; const int LEcho = P6_4; const int FTrig = P6_2; const int FEcho = P6_3; const int RSensor = P7_0; // create servo objects to control the servos // a maximum of 8 servo objects can be created Servo LeftServo, RightServo, FrontServo; const int TimeOut = 50000; const int FastCCW = 1600; const int FastCW = 1400; const int CCW = 1550; const int CW = 1450; const int SlowCCW = 1530; const int SlowCW = 1470; const int StopPulse = 1500; const int ArrSize = 40; const int Deviation = 10; const int SampleSize = 15; const int TJuncThreshold = 10; int index, i, FirstRun = 0.; int LeftRange, FrontRange, RightRange; int LeftTemp, RightTemp; int FrontLargeCount = 0, FrontServoTurn = 0; int ArrayLength; //********************************************************************** void setup() { // the radio library uses the SPI library internally, this call initializes // SPI/CSn and GDO0 lines. Also setup initial address, channel, and TX power. Radio.begin(0x01, CHANNEL_1, POWER_MAX); // setup serial for debug printing, runs at 9600 baud rate // Serial.begin(9600); // print any character in the quotes to Serial Monitor then enter a new line // Serial.println("Characters to print"); // print a variable // Serial.print(VariableName); LeftServo.attach(P1_5); // attach the servo on pin P1.5 to the LeftServo object RightServo.attach(P1_4); FrontServo.attach(P1_6); // set trigger pins as outputs and echo pins as analog inputs pinMode(LTrig, OUTPUT); pinMode(LEcho, INPUT); pinMode(FTrig, OUTPUT); pinMode(FEcho, INPUT); pinMode(P1_1, OUTPUT); // an LED is attached to P1.1 digitalWrite(P1_1, LOW); // turn off the LED } //********************************************************************** void loop() { int k, RxData = 0; char PathArray[ArrSize]; // PathArray stores the path of the robot char SolArray[ArrSize]; // SolArray stores the solution to exit out the maz unsigned char RX[2] = {'\0', '\0'}; // RX stores only 1 acknowledgement character for (index=0; index= 20) { MotorsControl(50, StopPulse, StopPulse); // robot stops temporarily OptimizeSol(PathArray); // optimize the path to eliminate dead ends while (RX[0] != 'Y') // while the receiver doesn't receive an acknowledgement from the other robot, keept transmitting the solution { Radio.transmit(ADDRESS_BROADCAST, (unsigned char*)PathArray, strlen(PathArray)); while (Radio.busy()); /* the radio transmitter and receiver cannot be operated at the same time. * if busy is not checked between two successive radio operations * receiverOn/transmit, the radio may not perform the specified task. The * radio must be complete with the transmission before it can begin the next */ Radio.receiverOn(RX, 2, 30); // turn on the receiver and listen for the acknowledgement. If there's data, then store it in RX digitalWrite(P1_1, HIGH); // LED is blinking while the robot transmits delay(30); // delay for 30 ms digitalWrite(P1_1, LOW); delay(30); } MotorsControl(200, SlowCCW, SlowCW); // right robot moves forward to leave enough room for left robot when it arrives } //_____________________SOLUTION RECEIVED_____________________ else if (RxData > 0) { MotorsControl(50, StopPulse, StopPulse); // robot stops temporarily RX[0]='Y'; // initialize RX array to Y to send as acknowledgement for (k=0; k<200; k++) { Radio.transmit(ADDRESS_BROADCAST, RX, 2); // send Y to the left robot to acknowledge that right robot received the solution while (Radio.busy()); // the other robot might not receive the Y if this robot only transmits once. Therefore, transmit 200 times digitalWrite(P1_1, HIGH); // turn the LED on to indicate that solution is received } index = OptimizeSol(PathArray); // optimize PathArray and returns the length of the optimized array int Match = CompareSol(PathArray, SolArray); // compare PathArray against the solution and return a value if (PathArray[index-1] != 'B' && Match == 0) // if PathArray and SolArray do not match and the last character stored in PathArray is not a B { // then turn around to get back to the previous junction MotorsControl(78, CCW, CCW); // if the last character is a B, the robot just completed a U-turn so it doesn't have to turn around again MotorsControl(50, StopPulse, StopPulse); // robot stops temporarily PathArray[index] = 'B'; // store a B in PathArray index++; } while (Match == 0) // navigate through the maze and optimize the array until it matches the solution { Navigate(PathArray); index = OptimizeSol(PathArray); Match = CompareSol(PathArray, SolArray); } FollowSol(SolArray); // once PathArray matches part of the solution, follow the path in the solution to find the exi } while(1) { MotorsControl(20, StopPulse, StopPulse); // robot stops forever } } //********************************************************************** void Navigate(char PathArray[]) // NOTE: a short stop follows every turn to ensure that the motors do not rotate more than they need to { int k; ReadSensors(); // read all 3 sensors //_____________________TURN RIGHT_____________________ if (RightRange >20 && FrontRange > 10) // right sensor sees opening and there's no wall in the front so it's safe to turn right { MotorsControl(20, StopPulse, StopPulse); // robot stops temporarily MotorsControl(80, SlowCCW, SlowCW); // move slowly forward a little to be at the middle of the junction MotorsControl(20, StopPulse, StopPulse); // robot stops again ReadSensors(); // read the sensors again //------------------CHECKING FOR EXIT------------------ if (LeftRange >40 && FrontRange >40) // if the front and left sensors detect a distance >40 cm, it may be an exit { FrontLargeCount=0; // this counter is used to count the number of times the front sensor sees a big opening FrontServoControl(18, CW); // swerve the front servo to the right, getting ready to scan the area FrontServoControl(20, StopPulse); // front servo stops temporarily for (k=0; k<133; k++) // 133 = the number of pulses needed for the front servo to scan about 120 degrees { FrontServo.writeMicroseconds(1510); // 1.51 ms pulse => counterclockwise, very slow delay(20); // a 20ms delay between pulses is necessary for smooth rotation ReadFrontSensor(); while (FrontRange <= 0) // while front sensor resets to 0 or negative value, read the sensor again { ReadFrontSensor(); } if (FrontRange > 90) // count the number of times the front sensor detects obstacles more than 90 cm away { FrontLargeCount++; } } FrontServoControl(50, StopPulse); // number of stop pulses <50 might not be sufficient to stop the servo from rotating FrontServoControl(18, CW); // return the front sensor to the original center position by rotating clockwise FrontServoControl(20, StopPulse); // the front servo doesn't return to the exact center position after multiple scans // there's a strong tendency for it to be tilted right with respect to the center position // therefore, return to the center position by tilting left FrontServoTurn++; // keeps track of the number of times the front sensor scans if (FrontServoTurn <= 2) { FrontServoControl(FrontServoTurn*2, SlowCCW); // 1st scan => tilt left a little, 2nd scan => tilt left more than 1st scan FrontServoControl(50, StopPulse); // stop after every tilt } else { FrontServoTurn = 0; // 3rd scan => don't tilt } if (FrontLargeCount >= 20) // if the front sensor detects large opening for more than 19 times { return; // then it's an exit, return to the main function } } MotorsControl(39, CCW, CCW); // if it's not an exit, proceed to turn right MotorsControl(20, StopPulse, StopPulse); MotorsControl(80, SlowCCW, SlowCW); // move forward a little to pass the junction MotorsControl(20, StopPulse, StopPulse); PathArray[index] = 'R'; // store an R in PathArray index ++; if (RightRange >20) // after turning right, the robot may be a little far away from the right wall { // therefore, it might mistake that there's an opening on the right for (k=0; k<20; k++) // give the robot a decent time to keep a good distance from the right wall { MaintainRight(FastCCW, FastCW); ReadRightSensor(); } } MotorsControl(20, StopPulse, StopPulse); } //_____________________GOING STRAIGHT_____________________ else if (RightRange <=20 && LeftRange <=20 && FrontRange > 10) // maintain straight if there are walls on both sides { MaintainRight(FastCCW, FastCW); } //_____________________|- JUNCTION_____________________ else if (RightRange <=20 && LeftRange >20 && FrontRange >30) { MotorsControl(20, StopPulse, StopPulse); int TJuncCheck = 0; // sometimes the left sensor reading jumps up for a short period of time // causing the robot to detect a false |- junction // TJuncCheck keeps track of the number of times the left sensor detects an opening while (RightRange <=20 && LeftRange >20 && FrontRange >30) // keep going straight as the robot passes the junction { MaintainRight(CCW, CW); ReadSensors(); TJuncCheck++; } MotorsControl(20, StopPulse, StopPulse); if (TJuncCheck >=TJuncThreshold) // if TJuncCheck >= 10, it's a true |- junction, store an S in PathArray { // if TJuncCheck < 10, it's a false |- junction, don't store S PathArray[index] = 'S'; index ++; } MotorsControl(20, StopPulse, StopPulse); } // _____________________TURN LEFT_____________________ else if (RightRange <=20 && LeftRange >20 && FrontRange <= 10) { MotorsControl(20, StopPulse, StopPulse); RightRear(); // back off to keep a safe distance from the right wall before turning left MotorsControl(20, StopPulse, StopPulse); MotorsControl(39, CW, CW); // turn left MotorsControl(20, StopPulse, StopPulse); PathArray[index] = 'L'; // store an L in PathArray index ++; for (i=0; i<35; i++) // after turning left, the robot might immediately detect a -| junction { // give the robot a decent time to pass the junction MaintainRight(FastCCW, FastCW); ReadRightSensor(); } MotorsControl(20, StopPulse, StopPulse); } //_____________________TURN AROUND_____________________ else if (RightRange <=20 && LeftRange <=20 && FrontRange <= 10) { TurnAround(); PathArray[index] = 'B'; // store a B in PathArray index ++; } else if (FrontRange > 10 && FrontRange <= 30) // we specify in the above cases that if FrontRange <= 10, it's a wall { // if FrontRange > 30, it's a |- junction MaintainRight(FastCCW, FastCW); // so when FrontRange falls between these 2 values, just go straight } // if we don't specify this case, when FrontRange falls into this range, it just stops return; } //********************************************************************** int OptimizeSol(char PathArray[]) { int j; ArrayLength = strlen(PathArray); for (i=0; i i-- [i][][] // BBX BBX } else if (angle == 0) { PathArray[i-1] = 'S'; // RBR and LBL are substituted by S } i--; // at the beginning of this loop, we will have: // originally [][i][] ===> i-- [i][][] ===> i++ from for loop [][i][] // Y BX Y BX Y BX // Y is a substituting letter, can be L, R, or S // continue to scan the rest of the array, index starts after the substituting letter // if B is the substituting letter: // i-- [i][][] ===> i-- [i][][][] ===> i++ from for loop [][i][][] // B BX XBBX X BBX for (j=ShiftingIndex; j [][S][][][] // Y B L R S YRLRS YRSRS YRS\0S Y R S\0\0 } ArrayLength = strlen(PathArray); } } return ArrayLength; } //********************************************************************** int CompareSol(char PathArray[], char SolArray[]) { for (i=0; i 20 || LeftRange > 30) // the robot detects a junction whenever the left or the right sensor sees an opening { MotorsControl(20, StopPulse, StopPulse); //_____________________TURN LEFT_____________________ if (SolArray[index] == 'L') // read the next character in SolArray to determine the turn { MotorsControl(80, SlowCCW, SlowCW); // move slowly forward to the middle of the junction MotorsControl(20, StopPulse, StopPulse); MotorsControl(39, CW, CW); // turn left MotorsControl(20, StopPulse, StopPulse); MotorsControl(80, SlowCCW, SlowCW); // move slowly forward to pass the junction } //_____________________TURN RIGHT_____________________ else if (SolArray[index] == 'R') { MotorsControl(80, SlowCCW, SlowCW); MotorsControl(20, StopPulse, StopPulse); MotorsControl(39, CCW, CCW); // turn right MotorsControl(20, StopPulse, StopPulse); MotorsControl(80, SlowCCW, SlowCW); } //_____________________GO STRAIGHT_____________________ else if (SolArray[index] == 'S') { if (RightRange <= 20) // if right sensor detects a wall, use the right wall to maintain straight { while (LeftRange > 30) // go straight until the robot passes the junction { MaintainRight(FastCCW, FastCW); ReadSensors(); } } else if (LeftRange <= 30) // if left sensor detects a wall, use the left wall to maintain straight { while (RightRange > 20) // go straight until the robot passes the junction { MaintainLeft(); ReadSensors(); } } else // it'a a 4-way junction { while (RightRange > 20 && LeftRange > 30) // there's no wall on either side so the robot doesn't have a reference to maintain a straight path { MotorsControl(10, SlowCCW, SlowCW); // just go forward slowly ReadSensors(); } } } MotorsControl(20, StopPulse, StopPulse); index++; } MaintainRight(FastCCW, FastCW); // go straight to search for the next junction } int k; // the robot reaches the exit for (k=0;k<20;k++) // it moves up a little to leave the maze { MaintainRight(FastCCW, FastCW); ReadRightSensor(); } return; } //********************************************************************** // this function maintains straight by telling the robot to maintain a safe distance from the right wall void MaintainRight(int CCWPulse, int CWPulse) { if (RightRange > 7 && RightRange < 10) // if the robot is 8-9 cm from the right wall { MotorsControl(1, CCWPulse, CWPulse); // go straight } else if (RightRange >= 10) // if the robot is far from the right wall { MotorsControl(1, CCWPulse, SlowCW); // slight right } else if (RightRange <= 7) // if the robot is close to the right wall { MotorsControl(1, SlowCCW, CWPulse); // slight left } return; } //********************************************************************** // this function maintains straight by telling the robot to maintain a safe distance from the left wall void MaintainLeft() { if (LeftRange > 7 && LeftRange < 10) // if the robot is 8-9 cm from the left wall { MotorsControl(1, FastCCW, FastCW); // go straight } else if (LeftRange >= 10) // if the robot is far from the left wall { MotorsControl(1, SlowCCW, FastCW); // slight left } else if (LeftRange <= 7) // if the robot is close the left wall { MotorsControl(1, FastCCW, SlowCW); // slight right } return; } //********************************************************************** void TurnAround() { MotorsControl(20, StopPulse, StopPulse); RightRear(); // backoff to keep a safe distance from the right wall before turning left MotorsControl(20, StopPulse, StopPulse); MotorsControl(39, CW, CW); // turn left 90 degree MotorsControl(20, StopPulse, StopPulse); ReadFrontSensor(); while (FrontRange > 10) // go straight if the front wall is still more than 10 cm away { MotorsControl(1, SlowCCW, SlowCW); ReadFrontSensor(); while (FrontRange <= 0) // reread the front sensor until the reading is positive { ReadFrontSensor(); } } MotorsControl(20, StopPulse, StopPulse); MotorsControl(39, CW, CW); // turn left again, 2 left turns = 1 U turn MotorsControl(20, StopPulse, StopPulse); return; } //********************************************************************** void RightRear() { if (RightRange <= 8) // if the robot is too close to the right wall { MotorsControl(10, CCW, CCW); // slight right to prepare for rearing MotorsControl(30, SlowCW, SlowCCW); // rear MotorsControl(10, CW, CW); // slight left to straighten } return; } //********************************************************************** void ReadSensors() { ReadRightSensor(); // read all 3 sensors ReadLeftSensor(); ReadFrontSensor(); while (LeftRange <= 0 || FrontRange <= 0) // retake reading until all ultrasonic sensors stop giving zero or negative reading { ReadLeftSensor(); ReadFrontSensor(); } if (FirstRun==0) // if it's the first time reading the sensors { RightTemp = RightRange; // store the readings for later comparison LeftTemp = LeftRange; FirstRun++; } else { int RightDiff = abs(RightRange - RightTemp); // find the difference between current and previous reading for left and right sensors int LeftDiff = abs(LeftRange - LeftTemp); if (RightDiff > Deviation) // if the readings differ more than 10 cm, the reading must have jumped { CorrectRReading(); // select the correct reading } else { RightTemp = RightRange; // if the readings differ less than 10 cm, store current reading for the next comparison } if (LeftDiff > Deviation) { CorrectLReading(); } else { LeftTemp = LeftRange; } } return; } //********************************************************************** void CorrectRReading() { int LowestRReading = 1000; for (i=0; i<10; i++) // take 10 reading samples and only keep the lowest value { // don't take the highest value because sensor reading tends to jump up, not down ReadRightSensor(); if (LowestRReading > RightRange) { LowestRReading = RightRange; } } RightRange = LowestRReading; RightTemp = RightRange; return; } //********************************************************************** void CorrectLReading() { int LowestLReading = 1000; // since ultrasonic sensor reading is more likely to fluctuate than IR sensor reading for (i=0; i LeftRange) { LowestLReading = LeftRange; } } LeftRange = LowestLReading; LeftTemp = LeftRange; return; } //********************************************************************** void ReadRightSensor () { int RReading = analogRead(RSensor); // equation is generated from test data in the "IR" Excel sheet RightRange = 20000 / (RReading + 186) - 2; // each sensor generates different equations return; // however, since the distance is approximated, the same equation can be used for other IR sensors of the same model } //********************************************************************** void ReadLeftSensor () { digitalWrite(LTrig, LOW); delayMicroseconds(2); digitalWrite(LTrig, HIGH); // trigger is generating a series of pulses at 40kHz delayMicroseconds(15); // make sure the delay is >10 us digitalWrite(LTrig, LOW); int LDuration = pulseIn(LEcho, HIGH, TimeOut); // measure the time it takes for the echo line to receive the wave // the echo line is set low after the timeout period is over // the speed of sound is 340 m/s or 29 us/cm and the wave travels out and back // => distance of obstacle = 1/2 of travelled distance = duration / 29 / 2 = duration / 58 LeftRange = LDuration/58; delay(10); // wait for some time to allow the ghost echoes to fade away return; } //********************************************************************** void ReadFrontSensor() { digitalWrite(FTrig, LOW); delayMicroseconds(2); digitalWrite(FTrig, HIGH); delayMicroseconds(15); digitalWrite(FTrig, LOW); int FDuration = pulseIn(FEcho, HIGH, TimeOut); FrontRange = FDuration/58; delay(10); return; } //********************************************************************** // pulse width modulation (PWM) // different pulse widths result in different speed and direction of rotation // Limit = duration of rotation // 1.5 ms pulse = no rotation // 1.7 ms pulse = full-speed counterclockwise rotation // 1.3 ms pulse = full-speed clockwise rotation // 1.3<-------CW------- 1.5 -------CCW------->1.7 // a 20ms delay between pulses is necessary for smooth rotation void MotorsControl(int Limit, int LeftPulse, int RightPulse) { int k; for (k=0; k