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@9:7d74c22ed54e, 2019-03-15 (annotated)

Committer:
mtag
Date:
Fri Mar 15 11:52:32 2019 +0000
Revision:
9:7d74c22ed54e
Parent:
8:88e72c6deac9
Child:
10:c7e0c94c8cd1
follows line somewhat

Who changed what in which revision?

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