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@10:c7e0c94c8cd1, 2019-03-18 (annotated)

Committer:
mtag
Date:
Mon Mar 18 17:54:48 2019 +0000
Revision:
10:c7e0c94c8cd1
Parent:
9:7d74c22ed54e
Child:
11:aaab65e884be
Yet to test

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