//LEFT 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 // library for servo motors // pins used by Radio Module: 3V3, 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 Servo LeftServo, RightServo, FrontServo; // declare objects for the servo motors const int LSensor = P7_0; // pin used for left IR sensor const int RTrig = P1_3; // pin used for right sensor's trigger const int REcho = P6_4; // pin used for right sensor's echo const int FTrig = P1_2; // pin used for front sensor's trigger const int FEcho = P6_6; // pin used for front sensor's echo const int TimeOut = 50000; const int FastCCW = 1600; // the fastest speed that the motors can go in couter clockwise direction const int FastCW = 1400; // the fastest speed that the motors can go in clockwise direction const int CCW = 1550; const int CW = 1450; const int SlowCCW = 1530; const int SlowCW = 1470; const int StopPulse = 1500; // at 1.5 ms, the motors stop const int ArrSize = 40; const int Deviation = 10; const int SampleSize = 15; const int TJuncThreshold = 10; int ArrayLength; int index, i, FirstRun = 0; int LeftRange, FrontRange, RightRange; int LeftTemp, RightTemp; int FrontLargeCount = 0, FrontServoTurn = 0; //********************************************************************** 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); Serial.begin(9600); // 9600 is the baud rate for the serial monitor LeftServo.attach(P1_5); // pins used for the motors RightServo.attach(P1_4); FrontServo.attach(P1_6); pinMode(RTrig, OUTPUT); // trigger pins of right and front sensors are output pinMode(REcho, INPUT); // echo pins of right and front sensors are input pinMode(FTrig, OUTPUT); pinMode(FEcho, INPUT); pinMode(P6_1, OUTPUT); // red LED on breadboard is used as indicator digitalWrite(P6_1, LOW); } //********************************************************************** void loop() { int RxData = 0; int k; char PathArray[ArrSize]; // this array stores the path of the robot char SolArray[ArrSize]; // this array stores the solution of the robot unsigned char RX [2] = {'\0', '\0'}; // this array stores the acknowledgement character for (index=0; index= 20) { MotorsControl(50, StopPulse, StopPulse); // robot stops temporarily OptimizeSol(PathArray); // optimizes PathArray by getting rid of all the Bs in the array while (RX [0] != 'Y') // left robot keeps transmitting the solution until right robot sends a Y as an acknowledgement { Radio.transmit(ADDRESS_BROADCAST, (unsigned char*)PathArray, strlen(PathArray)); while (Radio.busy()); // if busy is not checked between 2 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, 50); // listen for acknowledgement digitalWrite(P6_1, HIGH); // LED is blinking while the robot transmits delay(30); digitalWrite(P6_1, LOW); delay(30); } MotorsControl(200, SlowCCW, SlowCW); // left robot moves forward to leave enough room for right robot when it arrives } //_____________________SOLUTION RECEIVED_____________________ else if (RxData > 0) // if received data > 0, solution is received { 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 as acknowledgment while (Radio.busy()); digitalWrite(P6_1, HIGH); // the LED goes high when 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); PathArray[index] = 'B'; // store a B in PathArray index++; } while (Match == 0) { Navigate(PathArray); // navigate through the maze and index = OptimizeSol(PathArray); // optimize PathArray until Match = CompareSol(PathArray, SolArray); // it matches the first portion of the solution } FollowSol(SolArray); // once PathArray matches part of the solution, follow the path in the solution to find the exit } while(1) { MotorsControl(20, StopPulse, StopPulse); // the robot stops forever } } //********************************************************************** void Navigate (char PathArray[]) { // NOTE: a short stop follows every movement to ensure that the motors do not rotate more than they need to int k; ReadSensors(); // read all 3 sensors //_____________________TURN LEFT_____________________ if (LeftRange >20 && FrontRange > 10) // left sensor sees opening and there's no wall in the front so it's safe to turn left { MotorsControl(20, StopPulse, StopPulse); // stop temporarily MotorsControl(80, SlowCCW, SlowCW); // move forward a little MotorsControl(20, StopPulse, StopPulse); // stop again ReadSensors(); // read the sensors again //------------------CHECKING FOR EXIT------------------ if (RightRange >40 && FrontRange >40) // if the front and right 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 sensor to the right, getting ready to scan the area of 180 degrees FrontServoControl(20, StopPulse); for (k=0; k<127; k++) { FrontServo.writeMicroseconds(1510); // front motor rotates from right to left delay(20); ReadFrontSensor(); // read front sensor as the front motor rotates while (FrontRange <= 0) // if front sensor resets to 0 or negative value, read the sensor again { ReadFrontSensor(); } if (FrontRange > 90) // if the front sensor sees a distance of more than 90 cm, count up from 0 { FrontLargeCount++; } } FrontServoControl(50, StopPulse); FrontServoControl(18, CW); // return to the front sensor's original position by rotating right FrontServoControl(20, StopPulse); // when front motor rotates multiple times, it fails to return to its orginal straight orientation // as a result, front sensor tilts left or right which can cause it to take in wrong reading // so we need to readjust it FrontServoTurn++; // FrontServoTurn keeps track of the number of times the front sensor scans if (FrontServoTurn == 1) // if front sensor scans for an odd number of time, 1st, 3rd...time { FrontServoControl(4, SlowCW); // readjust the front motor by slighting towards the right FrontServoControl(50, StopPulse); } else if (FrontServoTurn == 2) // if the front sensor scans for an even number of time, 2nd, 4th...time { FrontServoControl(2, SlowCCW); // readjust the front motor by slighting towards the left FrontServoControl(50, StopPulse); FrontServoTurn=0; } if (FrontLargeCount >= 20) // if the front sensor detects opening of >90 cm for 20 times or more { // then it's an exit, return to the main function return; } } MotorsControl(39, CW, CW); // if it's not an exit, proceed to turn left MotorsControl(20, StopPulse, StopPulse); MotorsControl(80, SlowCCW, SlowCW); // move forward a little MotorsControl(20, StopPulse, StopPulse); PathArray[index] = 'L'; // store an L in PathArray index ++; if (LeftRange >15) // after turning left, the robot may be farther away from the left wall { // so readjust to keep a good distance from the left wall for (k=0; k<20; k++) { MaintainLeft(FastCCW, FastCW); ReadLeftSensor(); } } MotorsControl(20, StopPulse, StopPulse); } //_____________________GOING STRAIGHT_____________________ else if (LeftRange <=20 && RightRange <=20 && FrontRange >10) { MaintainLeft(FastCCW, FastCW); // maintain a straight line if there are walls on both sides } //_____________________|- JUNCTION_____________________ else if (LeftRange <=20 && RightRange >20 && FrontRange >30) { MotorsControl(20, StopPulse, StopPulse); int TJuncCheck = 0; // sometimes the right sensor reading jumps for a short period of time // making the robot falsely think that it encounters a |- junction // so TJuncCheck keeps track of the number of times the right sensor detects an opening // if TJuncCheck passes a threshold value, then it is a true |- junction while (LeftRange <= 20 && RightRange >20 && FrontRange >30) { MaintainLeft(CCW, CW); // keep going straight as the robot passes the junction ReadSensors(); TJuncCheck++; } if (TJuncCheck >=TJuncThreshold) { PathArray[index] = 'S'; // if TJuncCheck >= 10, it's a true |- junction, store an S in PathArray index ++; // if TJuncCheck < 10, it's a false |- junction, don't store anything } MotorsControl(20, StopPulse, StopPulse); } //_____________________TURN RIGHT_____________________ else if (LeftRange <=20 && RightRange >20 && FrontRange <=10) { MotorsControl(20, StopPulse, StopPulse); LeftRear(); // back off to keep a safe distance from the front wall before turning right MotorsControl(20, StopPulse, StopPulse); MotorsControl(39, CCW, CCW); // turn right MotorsControl(20, StopPulse, StopPulse); PathArray[index] = 'R'; // store an R in PathArray index ++; for (k=0; k<35; k++) { MaintainLeft(FastCCW, FastCW); // after turning right, the robot may be slanted ReadLeftSensor(); // so readjust to keep a good distance from the left wall } MotorsControl(20, StopPulse, StopPulse); } //_____________________TURN AROUND_____________________ else if (LeftRange <= 20 && RightRange <=20 && FrontRange <=10) { TurnAround(); // turn around once the robot encounters a dead end PathArray[index] = 'B'; // store a B in PathArray index ++; if (LeftRange >15) // after turning around, the robot may be farther away from the left wall { // so readjust to keep a good distance from the left wall for (k=0; k<20; k++) { MaintainLeft(FastCCW, FastCW); ReadLeftSensor(); } } MotorsControl(20, StopPulse, StopPulse); } 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 MaintainLeft(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][][] // B B X B B X } 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 B X Y B X Y B X // 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 B X X B B X X B B X for (j=ShiftingIndex; j 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 || RightRange > 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 forward a little MotorsControl(20, StopPulse, StopPulse); MotorsControl(39, CW, CW); // turn left MotorsControl(20, StopPulse, StopPulse); MotorsControl(80, SlowCCW, SlowCW); // move forward again } //_____________________TURN RIGHT_____________________ else if (SolArray[index] == 'R') { MotorsControl(80, SlowCCW, SlowCW); // move forward a little MotorsControl(20, StopPulse, StopPulse); MotorsControl(39, CCW, CCW); // turn right MotorsControl(20, StopPulse, StopPulse); MotorsControl(80, SlowCCW, SlowCW); // move forward again } //_____________________GO STRAIGHT_____________________ else if (SolArray[index] == 'S') { if (LeftRange <= 20) // if left sensor sees walls, use the left wall to maintain going straight { while (RightRange > 30) { MaintainLeft(FastCCW, FastCW); // go straight until the robot passes the junction ReadSensors(); } } else if (RightRange <= 30) // if right sensor sees walls, use the right wall to maintain going straight { while (LeftRange > 20) { MaintainRight(); // go straight until the robot passes the junction ReadSensors(); } } else // it'a a 4-way junction { while (LeftRange > 20 && RightRange > 30) { MotorsControl(10, SlowCCW, SlowCW); // there's no wall on either side so the robot doesn't have a reference to maintain a straight path ReadSensors(); // just go forward slowly } } } MotorsControl(20, StopPulse, StopPulse); index++; } MaintainLeft(FastCCW, FastCW); // go straight to search for the next junction } int k; for (k=0; k<20; k++) { MaintainLeft(FastCCW, FastCW); // the robot reaches the exit after it's done reading SolArray ReadLeftSensor(); // it moves up a little to leave the maze } return; } //********************************************************************** // this function maintains straight path by telling the robot to maintain a safe distance from the left wall void MaintainLeft(int CCWPulse, int CWPulse) { if (LeftRange == 9 || LeftRange == 10) // it's ideal that the robot maintains 9 or 10 cm away from the left wall { MotorsControl(1, CCWPulse, CWPulse); // go straight } else if (LeftRange > 10) // if it's farther away from the left wall { MotorsControl(1, SlowCCW, CWPulse); // slight left } else if (LeftRange < 9) // if it's too close from the left wall { MotorsControl(1, CCWPulse, SlowCW); // slight right } return; } //********************************************************************** // this function maintains straight path by telling the robot to maintain a safe distance from the right wall void MaintainRight() { if (RightRange == 9 || RightRange == 10) { MotorsControl(1, FastCCW, FastCW); // go straight } else if (RightRange > 10) { MotorsControl(1, FastCCW, SlowCW); // slight right } else if (RightRange < 9) { MotorsControl(1, SlowCCW, FastCW); // slight left } return; } //********************************************************************** void TurnAround() { MotorsControl(20, StopPulse, StopPulse); LeftRear(); // backoff to keep a safe distance from the front wall before turning right MotorsControl(20, StopPulse, StopPulse); MotorsControl(39, CCW, CCW); // turn right 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) { ReadFrontSensor(); } } MotorsControl(20, StopPulse, StopPulse); MotorsControl(39, CCW, CCW); // turn right again MotorsControl(20, StopPulse, StopPulse); // turning right 2 times is equivalent to a U-turn return; } //********************************************************************** void LeftRear() { if (LeftRange <= 10) { MotorsControl(10, CW, CW); // slight left MotorsControl(30, SlowCW, SlowCCW); // rear MotorsControl(10, CCW, CCW); // slight right to return to straight orientation } return; } //********************************************************************** void ReadSensors() { ReadLeftSensor(); // read 3 sensors ReadRightSensor(); ReadFrontSensor(); while (RightRange <= 0 || FrontRange <= 0) // retake reading until all ultrasonic sensors stop giving zero or negative reading { ReadRightSensor(); ReadFrontSensor(); } if (FirstRun==0) { LeftTemp = LeftRange; // if it's the first time we read the sensors RightTemp = RightRange; // let the previous reading = current reading FirstRun++; } else { int LeftDiff = abs(LeftRange - LeftTemp); // find the difference between previous and current reading int RightDiff = abs(RightRange - RightTemp); if (LeftDiff > Deviation) // if they differ more than 10 cm, the reading must have jumped { CorrectLReading(); // sample sensor reading } else { LeftTemp = LeftRange; // if they differ less than 10 cm, store current reading for the next comparison } if (RightDiff > Deviation) // do the same for right sensor { CorrectRReading(); } else { RightTemp = RightRange; } } return; } //********************************************************************** void CorrectLReading() { // take 10 samples and only keep the lowest value // don't take the highest value because sensor reading tend to jump up, not down int LowestLReading = 1000; for (i=0; i<10; i++) { ReadLeftSensor(); if (LowestLReading > LeftRange) { LowestLReading = LeftRange; } } LeftRange = LowestLReading; LeftTemp = LeftRange; return; } //********************************************************************** void CorrectRReading() { // take 15 samples and only keep the lowest value int LowestRReading = 1000; for (i=0; i RightRange) { LowestRReading = RightRange; } } RightRange = LowestRReading; RightTemp = RightRange; return; } //********************************************************************** void ReadLeftSensor () { // equation is generated by the data in the "IR" Excel sheet // each sensor generates different equations // however, we only use the estimated distance so we can use the same equation for other IR sensors of the same model int LReading = analogRead(LSensor); LeftRange = 20000 / (LReading + 186) - 2; return; } //********************************************************************** void ReadRightSensor () { digitalWrite(RTrig, LOW); // trigger is generating a series of pulses at 40kHz delayMicroseconds(2); digitalWrite(RTrig, HIGH); delayMicroseconds(15); // make sure the delay is >10 digitalWrite(RTrig, LOW); int RDuration = pulseIn(REcho, HIGH, TimeOut); // measure the time it takes for the echo line to receive the wave // sometimes when the object is so far away, the echo line stays high forever // so if the echo line still stays high after a period of time (timeout), we set it to low // the speed of sound is 340 m/s or 29 microseconds per centimeter // the wave travels out and back, so to find the distance of the object // we take half of the distance travelled // duration / 29 / 2 = duration / 58 RightRange = RDuration/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; } //********************************************************************** // we use pulse width modulation to control the motors // Limit is the duration of the rotation // LeftPulse and RightPulse is the pulse sent to the servos // 1500 = no rotation, > 1500 counter clockwise, < 1500 clockwise // a delay of 20 milliseconds between the pulses allow for smooth rotation void MotorsControl(int Limit, int LeftPulse, int RightPulse) { int k; for (k=0; k