A basic outline for the complete functionality of the robot.
Dependencies: mbed
Revision 13:3f239fa868f2, committed 2019-03-20
- Comitter:
- mtag
- Date:
- Wed Mar 20 13:07:42 2019 +0000
- Parent:
- 12:0b40dc152fe2
- Commit message:
- Should be final;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Mar 19 14:46:27 2019 +0000 +++ b/main.cpp Wed Mar 20 13:07:42 2019 +0000 @@ -1,7 +1,5 @@ - #include "mbed.h" +#include "mbed.h" - -Serial pc(USBTX, USBRX); //For the solenoid #define OFF 0 #define ON 1 @@ -11,7 +9,8 @@ #define STOP 0 #define FORWARD 1 #define BACKWARD 2 -//For line following, use the previous defines and the follwoing + +//For line following #define LEFT 3 #define RIGHT 4 #define CHOOSEPATH 5 @@ -19,23 +18,26 @@ #define LEFTHARD 7 #define RIGHTHARD 8 +//The Analog threshold for line detection in the range of 0 to 1 #define LINE_THRESHOLD 0.8 +//The red and blue paper reflect different levels of clear light. +//These maximums are needed so it doesn't think the floow is the paper #define RED_CLEAR_VALUE_MAX 44000 #define BLUE_CLEAR_VALUE_MAX 66000 -//Debug LED +//\Allows the user to see if the robot is taking the red or blue path DigitalOut redled(LED_RED); DigitalOut blueled(LED_BLUE); //Connections for the Adafruit TCS34725 colour sensor I2C i2c(PTC9, PTC8); //(SDA, SCL) - -//Set PWMs for controlling the H-bridge for the motor speed +//Set PWMs for controlling the H-bridge for the motor speeds PwmOut PWMmotorLeft(PTA4); //Connect to EN1 of L298N PwmOut PWMmotorRight(PTA5); //Connect to EN2 of L298N +//For setting forward or backward for each set of motors BusOut leftMotorMode(PTC17,PTC16); //Connect to IN1 and IN2 of L298N BusOut rightMotorMode(PTC13,PTC12); //Connect to IN3 and IN4 of L298N @@ -49,9 +51,8 @@ AnalogIn QTR3A_5(PTB1); AnalogIn QTR3A_6(PTB2); - //Remote control novel feature -Serial bluetooth(PTE0,PTE1); //TX, RX +Serial bluetooth(PTE0,PTE1); //TX, RX //A lookup table of which direction to turn the robot based on the values of all 6 sensor readings @@ -65,7 +66,7 @@ LEFT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, FORWARD, CHOOSEPATH, FORWARD, RIGHT, LEFT, CHOOSEPATH, //40-49 LEFT, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, LEFT, CHOOSEPATH, //50-59 FORWARD, FORWARD, FORWARD, FORWARD //60-63 - }; + }; @@ -89,7 +90,7 @@ void turnLeftHard(); void turnRightHard(); void changeDirection(int); - + private: void setLeftMotorMode(int); @@ -174,17 +175,12 @@ setLeftMotorSpeed(speed); setRightMotorSpeed(speed); - - //if(lastTurn[3] == LEFT and lastTurn[2] ==LEFT and lastTurn[1] == LEFT and lastTurn[0] == LEFT) { - // turnLeftHard(); - //} - - //else { + wait(0.1); speed = PWM_PERIOD_US*0.4; goForward(); - //} - + + } void MotorController::turnLeftHard() @@ -197,12 +193,11 @@ setLeftMotorSpeed(speed); setRightMotorSpeed(speed); - + wait(0.25); speed = PWM_PERIOD_US*0.4; goForward(); - //stopMotors(); - + } @@ -210,15 +205,14 @@ { state = RIGHT; lastTurn = RIGHT; - + setLeftMotorMode(FORWARD); setRightMotorMode(BACKWARD); setLeftMotorSpeed(speed); setRightMotorSpeed(speed); - //Turn for a short amount of time and then go straight - wait(0.1); + wait(0.1); speed = PWM_PERIOD_US*0.4; goForward(); @@ -228,15 +222,14 @@ { state = RIGHT; lastTurn = RIGHT; - - + setLeftMotorMode(FORWARD); setRightMotorMode(BACKWARD); setLeftMotorSpeed(speed); setRightMotorSpeed(speed); - - + + wait(0.25); speed = PWM_PERIOD_US*0.4; goForward(); @@ -244,9 +237,13 @@ void MotorController::changeDirection(int direction) { + //Sometimes when going around corners the robot would turn, then go straight + //and lose the track. If thart happens just turn the last direction it + //turned in before if(direction == LOST) { direction = lastTurn; } + switch(direction) { case STOP: @@ -255,32 +252,32 @@ case FORWARD: goForward(); - setSpeed(PWM_PERIOD_US * 0.40); + setSpeed(PWM_PERIOD_US * 0.4); break; case BACKWARD: goBackward(); - setSpeed(PWM_PERIOD_US * 0.40); + setSpeed(PWM_PERIOD_US * 0.4); break; case LEFT: turnLeft(); - setSpeed(PWM_PERIOD_US * 0.70); + setSpeed(PWM_PERIOD_US * 0.7); break; case RIGHT: turnRight(); - setSpeed(PWM_PERIOD_US *0.70); + setSpeed(PWM_PERIOD_US *0.7); break; - + case LEFTHARD: turnLeftHard(); - setSpeed(PWM_PERIOD_US*0.70); + setSpeed(PWM_PERIOD_US*0.7); break; - + case RIGHTHARD: turnRightHard(); - setSpeed(PWM_PERIOD_US*0.70); + setSpeed(PWM_PERIOD_US*0.7); break; } } @@ -362,7 +359,6 @@ } - class ColourSensor { public: @@ -433,7 +429,7 @@ //Detect the colour of the paper //Red is detected if their is the unfiltered light is below a threshold - //and there is at least twice as much red light copmared to blue light + //and there is at least twice as much red light compared to blue light if(clear_value < RED_CLEAR_VALUE_MAX && (red_value > (blue_value*2))) { red_detected = true; } @@ -530,19 +526,16 @@ int LineFollower::chooseDirection() { - - + //Default case int direction = STOP; + //Create a six digit binary number where each sensor changes a bit int sensorData = 0x3F & ((lineDetected[5] << 5) + (lineDetected[4] << 4) + (lineDetected[3]<< 3) + (lineDetected[2] << 2) + (lineDetected[1] << 1) + (lineDetected[0])); - pc.printf("\n\rSensor data = %d", sensorData); - + //The number is used as the index for the llopkup table direction = directionLookup[sensorData]; - pc.printf("\n\rTable result = %d", direction); - if(direction == CHOOSEPATH) { if(blue_path) { @@ -557,7 +550,6 @@ } } - pc.printf("\n\rChosen direction = %d", direction); return direction; } @@ -570,10 +562,12 @@ int speed = 0; while(true) { - + if(bluetooth.readable()) { - c = bluetooth.getc(); - } else {continue;} + c = bluetooth.getc(); + } else { + continue; + } switch(c) { case 'F': @@ -664,7 +658,6 @@ int main() { - bool path_found = false; MotorController motorController; @@ -684,13 +677,7 @@ redled = 0; wait(1); redled = 1; - blueled =1; - - //Start off going straight - - - - + blueled = 1; while(true) { @@ -722,9 +709,6 @@ solenoidController.controlLogic(lineFollower.red_path, lineFollower.blue_path, colourSensor.red_detected, colourSensor.blue_detected, &motorController); - //Blink LED every loop to ensure program isn't stuck - //redled = !redled; - //pc.printf("\n\rRound the loop"); } }