Team Design Project 3 / Mbed 2 deprecated TDP3_Robot_Control

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@13:3f239fa868f2, 2019-03-20 (annotated)

Committer:
mtag
Date:
Wed Mar 20 13:07:42 2019 +0000
Revision:
13:3f239fa868f2
Parent:
12:0b40dc152fe2 
Should be final;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mtag 13:3f239fa868f2 1 #include "mbed.h"
mtag 0:da3669e7df20 2
mtag 0:da3669e7df20 3 //For the solenoid
mtag 0:da3669e7df20 4 #define OFF 0
mtag 0:da3669e7df20 5 #define ON 1
mtag 0:da3669e7df20 6
mtag 0:da3669e7df20 7 //For motor control
mtag 7:1e5fa5952695 8 #define PWM_PERIOD_US 1000 //For setting PWM periods to 1 milliseconds. I made this number up
mtag 0:da3669e7df20 9 #define STOP 0
mtag 5:92510334cdfe 10 #define FORWARD 1
mtag 5:92510334cdfe 11 #define BACKWARD 2
mtag 13:3f239fa868f2 12
mtag 13:3f239fa868f2 13 //For line following
mtag 6:1056ed1d6d97 14 #define LEFT 3
mtag 6:1056ed1d6d97 15 #define RIGHT 4
mtag 8:88e72c6deac9 16 #define CHOOSEPATH 5
mtag 10:c7e0c94c8cd1 17 #define LOST 6
mtag 10:c7e0c94c8cd1 18 #define LEFTHARD 7
mtag 10:c7e0c94c8cd1 19 #define RIGHTHARD 8
mtag 7:1e5fa5952695 20
mtag 13:3f239fa868f2 21 //The Analog threshold for line detection in the range of 0 to 1
mtag 8:88e72c6deac9 22 #define LINE_THRESHOLD 0.8
mtag 0:da3669e7df20 23
mtag 13:3f239fa868f2 24 //The red and blue paper reflect different levels of clear light.
mtag 13:3f239fa868f2 25 //These maximums are needed so it doesn't think the floow is the paper
mtag 10:c7e0c94c8cd1 26 #define RED_CLEAR_VALUE_MAX 44000
mtag 10:c7e0c94c8cd1 27 #define BLUE_CLEAR_VALUE_MAX 66000
mtag 8:88e72c6deac9 28
mtag 13:3f239fa868f2 29 //\Allows the user to see if the robot is taking the red or blue path
mtag 8:88e72c6deac9 30 DigitalOut redled(LED_RED);
mtag 10:c7e0c94c8cd1 31 DigitalOut blueled(LED_BLUE);
mtag 8:88e72c6deac9 32
mtag 8:88e72c6deac9 33 //Connections for the Adafruit TCS34725 colour sensor
mtag 8:88e72c6deac9 34 I2C i2c(PTC9, PTC8); //(SDA, SCL)
mtag 0:da3669e7df20 35
mtag 13:3f239fa868f2 36 //Set PWMs for controlling the H-bridge for the motor speeds
mtag 0:da3669e7df20 37 PwmOut PWMmotorLeft(PTA4); //Connect to EN1 of L298N
mtag 8:88e72c6deac9 38 PwmOut PWMmotorRight(PTA5); //Connect to EN2 of L298N
mtag 0:da3669e7df20 39
mtag 13:3f239fa868f2 40 //For setting forward or backward for each set of motors
mtag 8:88e72c6deac9 41 BusOut leftMotorMode(PTC17,PTC16); //Connect to IN1 and IN2 of L298N
mtag 8:88e72c6deac9 42 BusOut rightMotorMode(PTC13,PTC12); //Connect to IN3 and IN4 of L298N
mtag 0:da3669e7df20 43
mtag 8:88e72c6deac9 44 DigitalOut solenoid(PTC3); //For the gate of the solenoid control MOSFET
mtag 0:da3669e7df20 45
mtag 12:0b40dc152fe2 46 //For black line detection. The six sensors create a line across the track
mtag 9:7d74c22ed54e 47 AnalogIn QTR3A_1(PTB3);
mtag 9:7d74c22ed54e 48 AnalogIn QTR3A_2(PTC2);
mtag 9:7d74c22ed54e 49 AnalogIn QTR3A_3(PTC1);
mtag 9:7d74c22ed54e 50 AnalogIn QTR3A_4(PTB0);
mtag 9:7d74c22ed54e 51 AnalogIn QTR3A_5(PTB1);
mtag 9:7d74c22ed54e 52 AnalogIn QTR3A_6(PTB2);
mtag 9:7d74c22ed54e 53
mtag 8:88e72c6deac9 54 //Remote control novel feature
mtag 13:3f239fa868f2 55 Serial bluetooth(PTE0,PTE1); //TX, RX
mtag 2:f0610c06721d 56
mtag 9:7d74c22ed54e 57
mtag 9:7d74c22ed54e 58 //A lookup table of which direction to turn the robot based on the values of all 6 sensor readings
mtag 12:0b40dc152fe2 59 //e.g. 3 = 000011 meaning no sensors detect a line and directionLookup[3] = RIGHT
mtag 9:7d74c22ed54e 60 //e.g. 12 = 001100 meaning the middle two sensors detect a line directionLookup[12] = FORWARD
mtag 9:7d74c22ed54e 61
mtag 10:c7e0c94c8cd1 62 int directionLookup[64] = {LOST, RIGHTHARD, RIGHT, RIGHT, FORWARD, RIGHT, RIGHT, RIGHT, FORWARD, FORWARD, //0-9
mtag 10:c7e0c94c8cd1 63 FORWARD, RIGHT, FORWARD, FORWARD, FORWARD, FORWARD, RIGHT, CHOOSEPATH, CHOOSEPATH, RIGHT, //10-19
mtag 10:c7e0c94c8cd1 64 FORWARD, CHOOSEPATH, RIGHT, RIGHT, LEFT, CHOOSEPATH, LEFT, CHOOSEPATH, FORWARD, FORWARD, //20-29
mtag 10:c7e0c94c8cd1 65 FORWARD, FORWARD, LEFTHARD, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, RIGHT, CHOOSEPATH, //30-39
mtag 10:c7e0c94c8cd1 66 LEFT, CHOOSEPATH, CHOOSEPATH, CHOOSEPATH, FORWARD, CHOOSEPATH, FORWARD, RIGHT, LEFT, CHOOSEPATH, //40-49
mtag 10:c7e0c94c8cd1 67 LEFT, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, CHOOSEPATH, LEFT, CHOOSEPATH, LEFT, CHOOSEPATH, //50-59
mtag 12:0b40dc152fe2 68 FORWARD, FORWARD, FORWARD, FORWARD //60-63
mtag 13:3f239fa868f2 69 };
mtag 9:7d74c22ed54e 70
mtag 10:c7e0c94c8cd1 71
mtag 9:7d74c22ed54e 72
mtag 12:0b40dc152fe2 73 const int sensor_addr = 41 << 1; //this should just be equal to 82, haven't touched it though. It's for the Colour Sensor
mtag 2:f0610c06721d 74
mtag 8:88e72c6deac9 75 class MotorController
mtag 8:88e72c6deac9 76 {
mtag 8:88e72c6deac9 77 public:
mtag 0:da3669e7df20 78 int state;
mtag 0:da3669e7df20 79 int speed;
mtag 12:0b40dc152fe2 80 int lastTurn;
mtag 8:88e72c6deac9 81
mtag 8:88e72c6deac9 82
mtag 0:da3669e7df20 83 void initialize();
mtag 0:da3669e7df20 84 void setSpeed(int pulsewidth_us);
mtag 0:da3669e7df20 85 void stopMotors();
mtag 0:da3669e7df20 86 void goForward();
mtag 0:da3669e7df20 87 void goBackward();
mtag 0:da3669e7df20 88 void turnLeft();
mtag 8:88e72c6deac9 89 void turnRight();
mtag 10:c7e0c94c8cd1 90 void turnLeftHard();
mtag 10:c7e0c94c8cd1 91 void turnRightHard();
mtag 9:7d74c22ed54e 92 void changeDirection(int);
mtag 13:3f239fa868f2 93
mtag 8:88e72c6deac9 94
mtag 8:88e72c6deac9 95 private:
mtag 9:7d74c22ed54e 96 void setLeftMotorMode(int);
mtag 9:7d74c22ed54e 97 void setRightMotorMode(int);
mtag 9:7d74c22ed54e 98 void setLeftMotorSpeed(int);
mtag 9:7d74c22ed54e 99 void setRightMotorSpeed(int);
mtag 0:da3669e7df20 100 };
mtag 0:da3669e7df20 101
mtag 0:da3669e7df20 102 void MotorController::initialize()
mtag 8:88e72c6deac9 103 {
mtag 0:da3669e7df20 104 state = STOP;
mtag 0:da3669e7df20 105 speed = 0;
mtag 0:da3669e7df20 106 PWMmotorLeft.period_us(PWM_PERIOD_US);
mtag 0:da3669e7df20 107 PWMmotorRight.period_us(PWM_PERIOD_US);
mtag 0:da3669e7df20 108
mtag 0:da3669e7df20 109 }
mtag 0:da3669e7df20 110
mtag 8:88e72c6deac9 111 void MotorController::setSpeed(int pulsewidth_us)
mtag 8:88e72c6deac9 112 {
mtag 8:88e72c6deac9 113 speed = pulsewidth_us;
mtag 8:88e72c6deac9 114 }
mtag 0:da3669e7df20 115
mtag 0:da3669e7df20 116 void MotorController::setLeftMotorSpeed(int pulsewidth_us)
mtag 0:da3669e7df20 117 {
mtag 0:da3669e7df20 118 PWMmotorLeft.pulsewidth_us(pulsewidth_us);
mtag 0:da3669e7df20 119 }
mtag 0:da3669e7df20 120
mtag 0:da3669e7df20 121
mtag 0:da3669e7df20 122 void MotorController::setRightMotorSpeed(int pulsewidth_us)
mtag 0:da3669e7df20 123 {
mtag 0:da3669e7df20 124 PWMmotorRight.pulsewidth_us(pulsewidth_us);
mtag 0:da3669e7df20 125 }
mtag 0:da3669e7df20 126
mtag 0:da3669e7df20 127
mtag 0:da3669e7df20 128 void MotorController::setLeftMotorMode(int mode)
mtag 0:da3669e7df20 129 {
mtag 0:da3669e7df20 130 leftMotorMode = mode;
mtag 0:da3669e7df20 131 }
mtag 0:da3669e7df20 132
mtag 0:da3669e7df20 133 void MotorController::setRightMotorMode(int mode)
mtag 0:da3669e7df20 134 {
mtag 0:da3669e7df20 135 rightMotorMode = mode;
mtag 0:da3669e7df20 136 }
mtag 0:da3669e7df20 137
mtag 0:da3669e7df20 138
mtag 0:da3669e7df20 139 void MotorController::stopMotors()
mtag 0:da3669e7df20 140 {
mtag 0:da3669e7df20 141 setLeftMotorMode(STOP);
mtag 0:da3669e7df20 142 setRightMotorMode(STOP);
mtag 0:da3669e7df20 143 }
mtag 0:da3669e7df20 144
mtag 0:da3669e7df20 145 void MotorController::goForward()
mtag 0:da3669e7df20 146 {
mtag 0:da3669e7df20 147 state = FORWARD;
mtag 8:88e72c6deac9 148
mtag 0:da3669e7df20 149 setLeftMotorMode(FORWARD);
mtag 0:da3669e7df20 150 setRightMotorMode(FORWARD);
mtag 8:88e72c6deac9 151
mtag 0:da3669e7df20 152
mtag 0:da3669e7df20 153 setLeftMotorSpeed(speed);
mtag 0:da3669e7df20 154 setRightMotorSpeed(speed);
mtag 0:da3669e7df20 155 }
mtag 0:da3669e7df20 156
mtag 0:da3669e7df20 157 void MotorController::goBackward()
mtag 0:da3669e7df20 158 {
mtag 0:da3669e7df20 159 state = BACKWARD;
mtag 8:88e72c6deac9 160
mtag 0:da3669e7df20 161 setLeftMotorMode(BACKWARD);
mtag 0:da3669e7df20 162 setRightMotorMode(BACKWARD);
mtag 0:da3669e7df20 163
mtag 0:da3669e7df20 164 setLeftMotorSpeed(speed);
mtag 0:da3669e7df20 165 setRightMotorSpeed(speed);
mtag 0:da3669e7df20 166 }
mtag 0:da3669e7df20 167
mtag 0:da3669e7df20 168 void MotorController::turnLeft()
mtag 8:88e72c6deac9 169 {
mtag 0:da3669e7df20 170 state = LEFT;
mtag 12:0b40dc152fe2 171 lastTurn = LEFT;
mtag 10:c7e0c94c8cd1 172
mtag 10:c7e0c94c8cd1 173 setLeftMotorMode(BACKWARD);
mtag 10:c7e0c94c8cd1 174 setRightMotorMode(FORWARD);
mtag 10:c7e0c94c8cd1 175
mtag 10:c7e0c94c8cd1 176 setLeftMotorSpeed(speed);
mtag 10:c7e0c94c8cd1 177 setRightMotorSpeed(speed);
mtag 13:3f239fa868f2 178
mtag 12:0b40dc152fe2 179 wait(0.1);
mtag 12:0b40dc152fe2 180 speed = PWM_PERIOD_US*0.4;
mtag 12:0b40dc152fe2 181 goForward();
mtag 13:3f239fa868f2 182
mtag 13:3f239fa868f2 183
mtag 10:c7e0c94c8cd1 184
mtag 10:c7e0c94c8cd1 185 }
mtag 10:c7e0c94c8cd1 186 void MotorController::turnLeftHard()
mtag 10:c7e0c94c8cd1 187 {
mtag 10:c7e0c94c8cd1 188 state = LEFT;
mtag 12:0b40dc152fe2 189 lastTurn = LEFT;
mtag 8:88e72c6deac9 190
mtag 0:da3669e7df20 191 setLeftMotorMode(BACKWARD);
mtag 0:da3669e7df20 192 setRightMotorMode(FORWARD);
mtag 0:da3669e7df20 193
mtag 0:da3669e7df20 194 setLeftMotorSpeed(speed);
mtag 0:da3669e7df20 195 setRightMotorSpeed(speed);
mtag 13:3f239fa868f2 196
mtag 12:0b40dc152fe2 197 wait(0.25);
mtag 12:0b40dc152fe2 198 speed = PWM_PERIOD_US*0.4;
mtag 12:0b40dc152fe2 199 goForward();
mtag 13:3f239fa868f2 200
mtag 0:da3669e7df20 201
mtag 7:1e5fa5952695 202 }
mtag 7:1e5fa5952695 203
mtag 0:da3669e7df20 204 void MotorController::turnRight()
mtag 0:da3669e7df20 205 {
mtag 0:da3669e7df20 206 state = RIGHT;
mtag 12:0b40dc152fe2 207 lastTurn = RIGHT;
mtag 13:3f239fa868f2 208
mtag 0:da3669e7df20 209 setLeftMotorMode(FORWARD);
mtag 0:da3669e7df20 210 setRightMotorMode(BACKWARD);
mtag 0:da3669e7df20 211
mtag 0:da3669e7df20 212 setLeftMotorSpeed(speed);
mtag 0:da3669e7df20 213 setRightMotorSpeed(speed);
mtag 12:0b40dc152fe2 214
mtag 13:3f239fa868f2 215 wait(0.1);
mtag 12:0b40dc152fe2 216 speed = PWM_PERIOD_US*0.4;
mtag 12:0b40dc152fe2 217 goForward();
mtag 12:0b40dc152fe2 218
mtag 10:c7e0c94c8cd1 219 }
mtag 10:c7e0c94c8cd1 220
mtag 10:c7e0c94c8cd1 221 void MotorController::turnRightHard()
mtag 10:c7e0c94c8cd1 222 {
mtag 10:c7e0c94c8cd1 223 state = RIGHT;
mtag 12:0b40dc152fe2 224 lastTurn = RIGHT;
mtag 13:3f239fa868f2 225
mtag 10:c7e0c94c8cd1 226 setLeftMotorMode(FORWARD);
mtag 10:c7e0c94c8cd1 227 setRightMotorMode(BACKWARD);
mtag 10:c7e0c94c8cd1 228
mtag 10:c7e0c94c8cd1 229 setLeftMotorSpeed(speed);
mtag 10:c7e0c94c8cd1 230 setRightMotorSpeed(speed);
mtag 13:3f239fa868f2 231
mtag 13:3f239fa868f2 232
mtag 12:0b40dc152fe2 233 wait(0.25);
mtag 12:0b40dc152fe2 234 speed = PWM_PERIOD_US*0.4;
mtag 12:0b40dc152fe2 235 goForward();
mtag 0:da3669e7df20 236 }
mtag 0:da3669e7df20 237
mtag 8:88e72c6deac9 238 void MotorController::changeDirection(int direction)
mtag 7:1e5fa5952695 239 {
mtag 13:3f239fa868f2 240 //Sometimes when going around corners the robot would turn, then go straight
mtag 13:3f239fa868f2 241 //and lose the track. If thart happens just turn the last direction it
mtag 13:3f239fa868f2 242 //turned in before
mtag 10:c7e0c94c8cd1 243 if(direction == LOST) {
mtag 12:0b40dc152fe2 244 direction = lastTurn;
mtag 10:c7e0c94c8cd1 245 }
mtag 13:3f239fa868f2 246
mtag 8:88e72c6deac9 247 switch(direction) {
mtag 7:1e5fa5952695 248
mtag 0:da3669e7df20 249 case STOP:
mtag 0:da3669e7df20 250 stopMotors();
mtag 0:da3669e7df20 251 break;
mtag 8:88e72c6deac9 252
mtag 0:da3669e7df20 253 case FORWARD:
mtag 0:da3669e7df20 254 goForward();
mtag 13:3f239fa868f2 255 setSpeed(PWM_PERIOD_US * 0.4);
mtag 8:88e72c6deac9 256 break;
mtag 8:88e72c6deac9 257
mtag 0:da3669e7df20 258 case BACKWARD:
mtag 0:da3669e7df20 259 goBackward();
mtag 13:3f239fa868f2 260 setSpeed(PWM_PERIOD_US * 0.4);
mtag 0:da3669e7df20 261 break;
mtag 8:88e72c6deac9 262
mtag 0:da3669e7df20 263 case LEFT:
mtag 0:da3669e7df20 264 turnLeft();
mtag 13:3f239fa868f2 265 setSpeed(PWM_PERIOD_US * 0.7);
mtag 0:da3669e7df20 266 break;
mtag 8:88e72c6deac9 267
mtag 0:da3669e7df20 268 case RIGHT:
mtag 0:da3669e7df20 269 turnRight();
mtag 13:3f239fa868f2 270 setSpeed(PWM_PERIOD_US *0.7);
mtag 10:c7e0c94c8cd1 271 break;
mtag 13:3f239fa868f2 272
mtag 10:c7e0c94c8cd1 273 case LEFTHARD:
mtag 10:c7e0c94c8cd1 274 turnLeftHard();
mtag 13:3f239fa868f2 275 setSpeed(PWM_PERIOD_US*0.7);
mtag 10:c7e0c94c8cd1 276 break;
mtag 13:3f239fa868f2 277
mtag 10:c7e0c94c8cd1 278 case RIGHTHARD:
mtag 10:c7e0c94c8cd1 279 turnRightHard();
mtag 13:3f239fa868f2 280 setSpeed(PWM_PERIOD_US*0.7);
mtag 8:88e72c6deac9 281 break;
mtag 0:da3669e7df20 282 }
mtag 0:da3669e7df20 283 }
mtag 0:da3669e7df20 284
mtag 11:aaab65e884be 285 class SolenoidController
mtag 11:aaab65e884be 286 {
mtag 11:aaab65e884be 287 public:
mtag 11:aaab65e884be 288 bool state;
mtag 11:aaab65e884be 289
mtag 11:aaab65e884be 290
mtag 11:aaab65e884be 291 void initialize();
mtag 11:aaab65e884be 292 void off();
mtag 11:aaab65e884be 293 void on();
mtag 11:aaab65e884be 294 void controlLogic(bool, bool, bool, bool, MotorController *);
mtag 11:aaab65e884be 295
mtag 11:aaab65e884be 296 private:
mtag 11:aaab65e884be 297 bool paper_detected;
mtag 11:aaab65e884be 298 };
mtag 11:aaab65e884be 299
mtag 11:aaab65e884be 300
mtag 11:aaab65e884be 301 void SolenoidController::off()
mtag 11:aaab65e884be 302 {
mtag 11:aaab65e884be 303 state = OFF;
mtag 11:aaab65e884be 304 solenoid = OFF;
mtag 11:aaab65e884be 305 }
mtag 11:aaab65e884be 306
mtag 11:aaab65e884be 307 void SolenoidController::on()
mtag 11:aaab65e884be 308 {
mtag 11:aaab65e884be 309 state = ON;
mtag 11:aaab65e884be 310 solenoid = ON;
mtag 11:aaab65e884be 311 }
mtag 11:aaab65e884be 312
mtag 11:aaab65e884be 313 void SolenoidController::initialize()
mtag 11:aaab65e884be 314 {
mtag 11:aaab65e884be 315 paper_detected = false;
mtag 11:aaab65e884be 316 off();
mtag 11:aaab65e884be 317 }
mtag 11:aaab65e884be 318
mtag 11:aaab65e884be 319 void SolenoidController::controlLogic(bool red_path, bool blue_path, bool red_detected, bool blue_detected, MotorController *motorController)
mtag 11:aaab65e884be 320 {
mtag 11:aaab65e884be 321 //Logic for the solenoid based on colour detected
mtag 11:aaab65e884be 322
mtag 11:aaab65e884be 323 //Detect the first sheet of paper if blue and pick up the disc
mtag 11:aaab65e884be 324 if(blue_detected && !paper_detected && !state) {
mtag 11:aaab65e884be 325 paper_detected = true;
mtag 11:aaab65e884be 326 blue_path = true;
mtag 11:aaab65e884be 327 on();
mtag 11:aaab65e884be 328 motorController->goForward();
mtag 11:aaab65e884be 329 motorController->setSpeed(PWM_PERIOD_US * 0.45);
mtag 12:0b40dc152fe2 330 wait(1);
mtag 11:aaab65e884be 331 }
mtag 11:aaab65e884be 332
mtag 11:aaab65e884be 333 //Detect the first sheet of paper if red and pick up the disc
mtag 11:aaab65e884be 334 else if(red_detected && !paper_detected && !state) {
mtag 11:aaab65e884be 335 paper_detected = true;
mtag 11:aaab65e884be 336 red_path = true;
mtag 11:aaab65e884be 337 on();
mtag 11:aaab65e884be 338 motorController->goForward();
mtag 11:aaab65e884be 339 motorController->setSpeed(PWM_PERIOD_US * 0.45);
mtag 12:0b40dc152fe2 340 wait(1);
mtag 11:aaab65e884be 341 }
mtag 11:aaab65e884be 342
mtag 11:aaab65e884be 343 //Detect the end of the first sheet of paper
mtag 11:aaab65e884be 344 else if(!blue_detected && !red_detected && paper_detected) {
mtag 11:aaab65e884be 345 paper_detected = false;
mtag 11:aaab65e884be 346 }
mtag 11:aaab65e884be 347
mtag 11:aaab65e884be 348 //Drop the disc once the second blue paper is detected
mtag 11:aaab65e884be 349 else if(blue_detected && blue_path && !paper_detected && state) {
mtag 11:aaab65e884be 350 paper_detected = true;
mtag 11:aaab65e884be 351 off();
mtag 11:aaab65e884be 352 }
mtag 11:aaab65e884be 353
mtag 11:aaab65e884be 354 //Drop the disc once the second red paper is detected
mtag 11:aaab65e884be 355 else if(red_detected && red_path && !paper_detected && state) {
mtag 11:aaab65e884be 356 paper_detected = true;
mtag 11:aaab65e884be 357 off();
mtag 11:aaab65e884be 358 }
mtag 11:aaab65e884be 359 }
mtag 11:aaab65e884be 360
mtag 11:aaab65e884be 361
mtag 8:88e72c6deac9 362 class ColourSensor
mtag 8:88e72c6deac9 363 {
mtag 8:88e72c6deac9 364 public:
mtag 0:da3669e7df20 365
mtag 0:da3669e7df20 366 bool blue_detected;
mtag 0:da3669e7df20 367 bool red_detected;
mtag 8:88e72c6deac9 368
mtag 0:da3669e7df20 369 void initialize();
mtag 0:da3669e7df20 370 void read();
mtag 0:da3669e7df20 371 };
mtag 0:da3669e7df20 372
mtag 0:da3669e7df20 373
mtag 8:88e72c6deac9 374 void ColourSensor::initialize()
mtag 8:88e72c6deac9 375 {
mtag 8:88e72c6deac9 376
mtag 0:da3669e7df20 377 i2c.frequency(200000);
mtag 8:88e72c6deac9 378
mtag 0:da3669e7df20 379 blue_detected = false;
mtag 0:da3669e7df20 380 red_detected = false;
mtag 8:88e72c6deac9 381
mtag 0:da3669e7df20 382 char id_regval[1] = {146};
mtag 0:da3669e7df20 383 char data[1] = {0};
mtag 0:da3669e7df20 384 i2c.write(sensor_addr,id_regval,1, true);
mtag 0:da3669e7df20 385 i2c.read(sensor_addr,data,1,false);
mtag 8:88e72c6deac9 386
mtag 0:da3669e7df20 387 char timing_register[2] = {129,0};
mtag 0:da3669e7df20 388 i2c.write(sensor_addr,timing_register,2,false);
mtag 8:88e72c6deac9 389
mtag 0:da3669e7df20 390 char control_register[2] = {143,0};
mtag 0:da3669e7df20 391 i2c.write(sensor_addr,control_register,2,false);
mtag 8:88e72c6deac9 392
mtag 0:da3669e7df20 393 char enable_register[2] = {128,3};
mtag 0:da3669e7df20 394 i2c.write(sensor_addr,enable_register,2,false);
mtag 0:da3669e7df20 395 }
mtag 0:da3669e7df20 396
mtag 8:88e72c6deac9 397 void ColourSensor::read()
mtag 8:88e72c6deac9 398 {
mtag 8:88e72c6deac9 399
mtag 8:88e72c6deac9 400 char clear_reg[1] = {148};
mtag 8:88e72c6deac9 401 char clear_data[2] = {0,0};
mtag 8:88e72c6deac9 402 i2c.write(sensor_addr,clear_reg,1, true);
mtag 8:88e72c6deac9 403 i2c.read(sensor_addr,clear_data,2, false);
mtag 8:88e72c6deac9 404
mtag 8:88e72c6deac9 405 int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
mtag 8:88e72c6deac9 406
mtag 8:88e72c6deac9 407 char red_reg[1] = {150};
mtag 8:88e72c6deac9 408 char red_data[2] = {0,0};
mtag 8:88e72c6deac9 409 i2c.write(sensor_addr,red_reg,1, true);
mtag 8:88e72c6deac9 410 i2c.read(sensor_addr,red_data,2, false);
mtag 8:88e72c6deac9 411
mtag 8:88e72c6deac9 412 int red_value = ((int)red_data[1] << 8) | red_data[0];
mtag 8:88e72c6deac9 413
mtag 8:88e72c6deac9 414 char green_reg[1] = {152};
mtag 8:88e72c6deac9 415 char green_data[2] = {0,0};
mtag 8:88e72c6deac9 416 i2c.write(sensor_addr,green_reg,1, true);
mtag 8:88e72c6deac9 417 i2c.read(sensor_addr,green_data,2, false);
mtag 8:88e72c6deac9 418
mtag 8:88e72c6deac9 419 int green_value = ((int)green_data[1] << 8) | green_data[0];
mtag 0:da3669e7df20 420
mtag 8:88e72c6deac9 421 char blue_reg[1] = {154};
mtag 8:88e72c6deac9 422 char blue_data[2] = {0,0};
mtag 8:88e72c6deac9 423 i2c.write(sensor_addr,blue_reg,1, true);
mtag 8:88e72c6deac9 424 i2c.read(sensor_addr,blue_data,2, false);
mtag 0:da3669e7df20 425
mtag 8:88e72c6deac9 426 int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
mtag 8:88e72c6deac9 427
mtag 8:88e72c6deac9 428
mtag 8:88e72c6deac9 429 //Detect the colour of the paper
mtag 0:da3669e7df20 430
mtag 8:88e72c6deac9 431 //Red is detected if their is the unfiltered light is below a threshold
mtag 13:3f239fa868f2 432 //and there is at least twice as much red light compared to blue light
mtag 8:88e72c6deac9 433 if(clear_value < RED_CLEAR_VALUE_MAX && (red_value > (blue_value*2))) {
mtag 8:88e72c6deac9 434 red_detected = true;
mtag 2:f0610c06721d 435 }
mtag 0:da3669e7df20 436
mtag 2:f0610c06721d 437 else {
mtag 8:88e72c6deac9 438 red_detected = false;
mtag 2:f0610c06721d 439 }
mtag 8:88e72c6deac9 440
mtag 8:88e72c6deac9 441
mtag 8:88e72c6deac9 442 //Similar to detection for red, but with a different threshold
mtag 8:88e72c6deac9 443 if(clear_value < BLUE_CLEAR_VALUE_MAX && blue_value > (red_value*2)) {
mtag 8:88e72c6deac9 444 blue_detected = true;
mtag 8:88e72c6deac9 445 }
mtag 8:88e72c6deac9 446
mtag 2:f0610c06721d 447 else {
mtag 8:88e72c6deac9 448 blue_detected = false;
mtag 8:88e72c6deac9 449 }
mtag 0:da3669e7df20 450 }
mtag 0:da3669e7df20 451
mtag 8:88e72c6deac9 452 class LineFollower
mtag 8:88e72c6deac9 453 {
mtag 8:88e72c6deac9 454
mtag 8:88e72c6deac9 455 public:
mtag 8:88e72c6deac9 456 bool lineDetected[6];
mtag 8:88e72c6deac9 457 bool red_path;
mtag 8:88e72c6deac9 458 bool blue_path;
mtag 8:88e72c6deac9 459
mtag 9:7d74c22ed54e 460
mtag 8:88e72c6deac9 461 void initialize();
mtag 8:88e72c6deac9 462 void readSensors();
mtag 9:7d74c22ed54e 463 int chooseDirection();
mtag 8:88e72c6deac9 464 };
mtag 8:88e72c6deac9 465
mtag 8:88e72c6deac9 466 void LineFollower::initialize()
mtag 8:88e72c6deac9 467 {
mtag 8:88e72c6deac9 468
mtag 8:88e72c6deac9 469 for(int i = 0; i < 6; i++) {
mtag 8:88e72c6deac9 470 lineDetected[i] = false;
mtag 8:88e72c6deac9 471 }
mtag 8:88e72c6deac9 472
mtag 8:88e72c6deac9 473 red_path = false;
mtag 8:88e72c6deac9 474 blue_path = false;
mtag 9:7d74c22ed54e 475
mtag 9:7d74c22ed54e 476
mtag 8:88e72c6deac9 477 }
mtag 8:88e72c6deac9 478
mtag 8:88e72c6deac9 479 void LineFollower::readSensors()
mtag 8:88e72c6deac9 480 {
mtag 8:88e72c6deac9 481 if(QTR3A_1.read() > LINE_THRESHOLD) {
mtag 8:88e72c6deac9 482 lineDetected[0] = true;
mtag 8:88e72c6deac9 483 }
mtag 8:88e72c6deac9 484
mtag 8:88e72c6deac9 485 else {
mtag 8:88e72c6deac9 486 lineDetected[0] = false;
mtag 8:88e72c6deac9 487 }
mtag 4:ace17b63da3c 488
mtag 8:88e72c6deac9 489 if(QTR3A_2.read() > LINE_THRESHOLD) {
mtag 8:88e72c6deac9 490 lineDetected[1] = true;
mtag 8:88e72c6deac9 491 }
mtag 8:88e72c6deac9 492
mtag 8:88e72c6deac9 493 else {
mtag 8:88e72c6deac9 494 lineDetected[1] = false;
mtag 8:88e72c6deac9 495 }
mtag 8:88e72c6deac9 496 if(QTR3A_3.read() > LINE_THRESHOLD) {
mtag 8:88e72c6deac9 497 lineDetected[2] = true;
mtag 8:88e72c6deac9 498 }
mtag 8:88e72c6deac9 499
mtag 8:88e72c6deac9 500 else {
mtag 8:88e72c6deac9 501 lineDetected[2] = false;
mtag 8:88e72c6deac9 502 }
mtag 8:88e72c6deac9 503 if(QTR3A_4.read() > LINE_THRESHOLD) {
mtag 8:88e72c6deac9 504 lineDetected[3] = true;
mtag 8:88e72c6deac9 505 }
mtag 7:1e5fa5952695 506
mtag 8:88e72c6deac9 507 else {
mtag 8:88e72c6deac9 508 lineDetected[3] = false;
mtag 8:88e72c6deac9 509 }
mtag 8:88e72c6deac9 510 if(QTR3A_5.read() > LINE_THRESHOLD) {
mtag 8:88e72c6deac9 511 lineDetected[4] = true;
mtag 8:88e72c6deac9 512 }
mtag 8:88e72c6deac9 513
mtag 8:88e72c6deac9 514 else {
mtag 8:88e72c6deac9 515 lineDetected[4] = false;
mtag 8:88e72c6deac9 516 }
mtag 8:88e72c6deac9 517 if(QTR3A_6.read() > LINE_THRESHOLD) {
mtag 8:88e72c6deac9 518 lineDetected[5] = true;
mtag 8:88e72c6deac9 519 }
mtag 8:88e72c6deac9 520
mtag 8:88e72c6deac9 521 else {
mtag 8:88e72c6deac9 522 lineDetected[5] = false;
mtag 8:88e72c6deac9 523 }
mtag 0:da3669e7df20 524 }
mtag 0:da3669e7df20 525
mtag 6:1056ed1d6d97 526
mtag 9:7d74c22ed54e 527 int LineFollower::chooseDirection()
mtag 8:88e72c6deac9 528 {
mtag 13:3f239fa868f2 529 //Default case
mtag 8:88e72c6deac9 530 int direction = STOP;
mtag 8:88e72c6deac9 531
mtag 13:3f239fa868f2 532 //Create a six digit binary number where each sensor changes a bit
mtag 9:7d74c22ed54e 533 int sensorData = 0x3F & ((lineDetected[5] << 5) + (lineDetected[4] << 4) + (lineDetected[3]<< 3) +
mtag 10:c7e0c94c8cd1 534 (lineDetected[2] << 2) + (lineDetected[1] << 1) + (lineDetected[0]));
mtag 10:c7e0c94c8cd1 535
mtag 13:3f239fa868f2 536 //The number is used as the index for the llopkup table
mtag 9:7d74c22ed54e 537 direction = directionLookup[sensorData];
mtag 8:88e72c6deac9 538
mtag 8:88e72c6deac9 539 if(direction == CHOOSEPATH) {
mtag 8:88e72c6deac9 540
mtag 11:aaab65e884be 541 if(blue_path) {
mtag 10:c7e0c94c8cd1 542 direction = LEFTHARD;
mtag 8:88e72c6deac9 543 }
mtag 8:88e72c6deac9 544
mtag 11:aaab65e884be 545 else if(red_path) {
mtag 10:c7e0c94c8cd1 546 direction = RIGHTHARD;
mtag 8:88e72c6deac9 547 } else {
mtag 8:88e72c6deac9 548 direction = FORWARD;
mtag 8:88e72c6deac9 549
mtag 8:88e72c6deac9 550 }
mtag 8:88e72c6deac9 551 }
mtag 10:c7e0c94c8cd1 552
mtag 9:7d74c22ed54e 553 return direction;
mtag 8:88e72c6deac9 554 }
mtag 10:c7e0c94c8cd1 555
mtag 11:aaab65e884be 556 void bluetoothControl(MotorController *motorController, SolenoidController *solenoidController)
mtag 8:88e72c6deac9 557 {
mtag 8:88e72c6deac9 558 bluetooth.baud(9600);
mtag 8:88e72c6deac9 559
mtag 2:f0610c06721d 560 char c = '0';
mtag 2:f0610c06721d 561 char state = 'F';
mtag 2:f0610c06721d 562 int speed = 0;
mtag 2:f0610c06721d 563
mtag 10:c7e0c94c8cd1 564 while(true) {
mtag 13:3f239fa868f2 565
mtag 10:c7e0c94c8cd1 566 if(bluetooth.readable()) {
mtag 13:3f239fa868f2 567 c = bluetooth.getc();
mtag 13:3f239fa868f2 568 } else {
mtag 13:3f239fa868f2 569 continue;
mtag 13:3f239fa868f2 570 }
mtag 2:f0610c06721d 571 switch(c) {
mtag 8:88e72c6deac9 572
mtag 2:f0610c06721d 573 case 'F':
mtag 2:f0610c06721d 574 if(state != 'F') {
mtag 2:f0610c06721d 575 state = 'F';
mtag 8:88e72c6deac9 576 speed = PWM_PERIOD_US * 0.4;
mtag 11:aaab65e884be 577 motorController->setSpeed(speed);
mtag 11:aaab65e884be 578 motorController->goForward();
mtag 8:88e72c6deac9 579 }
mtag 8:88e72c6deac9 580
mtag 8:88e72c6deac9 581 else {
mtag 8:88e72c6deac9 582 speed += PWM_PERIOD_US * 0.1;
mtag 11:aaab65e884be 583 motorController->setSpeed(speed);
mtag 11:aaab65e884be 584 motorController->goForward();
mtag 2:f0610c06721d 585 }
mtag 2:f0610c06721d 586 break;
mtag 8:88e72c6deac9 587
mtag 2:f0610c06721d 588 case 'B':
mtag 2:f0610c06721d 589 if(state != 'B') {
mtag 2:f0610c06721d 590 state = 'B';
mtag 8:88e72c6deac9 591 speed = PWM_PERIOD_US * 0.4;
mtag 11:aaab65e884be 592 motorController->setSpeed(speed);
mtag 11:aaab65e884be 593 motorController->goBackward();
mtag 8:88e72c6deac9 594 }
mtag 8:88e72c6deac9 595
mtag 8:88e72c6deac9 596 else {
mtag 8:88e72c6deac9 597 speed += PWM_PERIOD_US * 0.1;
mtag 11:aaab65e884be 598 motorController->setSpeed(speed);
mtag 11:aaab65e884be 599 motorController->goBackward();
mtag 2:f0610c06721d 600 }
mtag 2:f0610c06721d 601 break;
mtag 8:88e72c6deac9 602
mtag 8:88e72c6deac9 603 case 'L':
mtag 2:f0610c06721d 604 if(state != 'L') {
mtag 2:f0610c06721d 605 state = 'L';
mtag 8:88e72c6deac9 606 speed = PWM_PERIOD_US * 0.4;;
mtag 11:aaab65e884be 607 motorController->setSpeed(speed);
mtag 11:aaab65e884be 608 motorController->turnLeft();
mtag 8:88e72c6deac9 609 }
mtag 8:88e72c6deac9 610
mtag 8:88e72c6deac9 611 else {
mtag 8:88e72c6deac9 612 speed += PWM_PERIOD_US * 0.1;
mtag 11:aaab65e884be 613 motorController->setSpeed(speed);
mtag 11:aaab65e884be 614 motorController->turnLeft();
mtag 2:f0610c06721d 615 }
mtag 8:88e72c6deac9 616 break;
mtag 8:88e72c6deac9 617
mtag 8:88e72c6deac9 618 case 'R':
mtag 2:f0610c06721d 619 if(state != 'R') {
mtag 2:f0610c06721d 620 state = 'R';
mtag 8:88e72c6deac9 621 speed = PWM_PERIOD_US * 0.4;
mtag 11:aaab65e884be 622 motorController->setSpeed(speed);
mtag 11:aaab65e884be 623 motorController->turnRight();
mtag 8:88e72c6deac9 624 }
mtag 8:88e72c6deac9 625
mtag 8:88e72c6deac9 626 else {
mtag 8:88e72c6deac9 627 speed += PWM_PERIOD_US * 0.1;
mtag 11:aaab65e884be 628 motorController->setSpeed(speed);
mtag 11:aaab65e884be 629 motorController->turnRight();
mtag 2:f0610c06721d 630 }
mtag 2:f0610c06721d 631 break;
mtag 8:88e72c6deac9 632
mtag 8:88e72c6deac9 633 case 'X':
mtag 8:88e72c6deac9 634 state = 'X';
mtag 2:f0610c06721d 635 speed = 0;
mtag 11:aaab65e884be 636 motorController->setSpeed(speed);
mtag 11:aaab65e884be 637 motorController->stopMotors();
mtag 8:88e72c6deac9 638 break;
mtag 8:88e72c6deac9 639
mtag 8:88e72c6deac9 640 case 'S':
mtag 11:aaab65e884be 641 if(solenoidController->state) {
mtag 11:aaab65e884be 642 solenoidController->off();
mtag 11:aaab65e884be 643 } else if(!solenoidController->state) {
mtag 11:aaab65e884be 644 solenoidController->on();
mtag 8:88e72c6deac9 645 }
mtag 8:88e72c6deac9 646 break;
mtag 8:88e72c6deac9 647
mtag 2:f0610c06721d 648 }
mtag 8:88e72c6deac9 649
mtag 8:88e72c6deac9 650 c='0';
mtag 8:88e72c6deac9 651
mtag 2:f0610c06721d 652 }
mtag 2:f0610c06721d 653 }
mtag 2:f0610c06721d 654
mtag 2:f0610c06721d 655
mtag 2:f0610c06721d 656
mtag 8:88e72c6deac9 657
mtag 8:88e72c6deac9 658 int main()
mtag 8:88e72c6deac9 659 {
mtag 0:da3669e7df20 660
mtag 8:88e72c6deac9 661 bool path_found = false;
mtag 8:88e72c6deac9 662
mtag 0:da3669e7df20 663 MotorController motorController;
mtag 0:da3669e7df20 664 SolenoidController solenoidController;
mtag 0:da3669e7df20 665 LineFollower lineFollower;
mtag 0:da3669e7df20 666 ColourSensor colourSensor;
mtag 8:88e72c6deac9 667
mtag 8:88e72c6deac9 668
mtag 0:da3669e7df20 669 motorController.initialize();
mtag 0:da3669e7df20 670 lineFollower.initialize();
mtag 0:da3669e7df20 671 colourSensor.initialize();
mtag 8:88e72c6deac9 672 solenoidController.initialize();
mtag 8:88e72c6deac9 673
mtag 8:88e72c6deac9 674 //Blink LED after reset
mtag 8:88e72c6deac9 675 redled = 1;
mtag 8:88e72c6deac9 676 wait(0.5);
mtag 8:88e72c6deac9 677 redled = 0;
mtag 8:88e72c6deac9 678 wait(1);
mtag 8:88e72c6deac9 679 redled = 1;
mtag 13:3f239fa868f2 680 blueled = 1;
mtag 9:7d74c22ed54e 681
mtag 0:da3669e7df20 682 while(true) {
mtag 8:88e72c6deac9 683
mtag 8:88e72c6deac9 684
mtag 9:7d74c22ed54e 685
mtag 2:f0610c06721d 686 if(bluetooth.readable()) {
mtag 10:c7e0c94c8cd1 687 motorController.stopMotors();
mtag 11:aaab65e884be 688 bluetoothControl(&motorController, &solenoidController);
mtag 0:da3669e7df20 689 }
mtag 8:88e72c6deac9 690
mtag 8:88e72c6deac9 691
mtag 8:88e72c6deac9 692
mtag 8:88e72c6deac9 693 lineFollower.readSensors();
mtag 9:7d74c22ed54e 694 motorController.changeDirection(lineFollower.chooseDirection());
mtag 8:88e72c6deac9 695
mtag 10:c7e0c94c8cd1 696 colourSensor.read();
mtag 8:88e72c6deac9 697
mtag 8:88e72c6deac9 698 if(colourSensor.red_detected and !path_found) {
mtag 10:c7e0c94c8cd1 699 redled = 0;
mtag 8:88e72c6deac9 700 path_found = true;
mtag 8:88e72c6deac9 701 lineFollower.red_path = true;
mtag 0:da3669e7df20 702 }
mtag 8:88e72c6deac9 703
mtag 8:88e72c6deac9 704 else if(colourSensor.blue_detected and !path_found) {
mtag 10:c7e0c94c8cd1 705 blueled = 0;
mtag 8:88e72c6deac9 706 path_found = true;
mtag 8:88e72c6deac9 707 lineFollower.blue_path = true;
mtag 0:da3669e7df20 708 }
mtag 8:88e72c6deac9 709
mtag 11:aaab65e884be 710 solenoidController.controlLogic(lineFollower.red_path, lineFollower.blue_path, colourSensor.red_detected, colourSensor.blue_detected, &motorController);
mtag 8:88e72c6deac9 711
mtag 8:88e72c6deac9 712 }
mtag 8:88e72c6deac9 713
mtag 0:da3669e7df20 714 }