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@11:aaab65e884be, 2019-03-19 (annotated)

Committer:
mtag
Date:
Tue Mar 19 10:39:54 2019 +0000
Revision:
11:aaab65e884be
Parent:
10:c7e0c94c8cd1
Child:
12:0b40dc152fe2
Made line following faster

Who changed what in which revision?

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