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@12:0b40dc152fe2, 2019-03-19 (annotated)

Committer:
mtag
Date:
Tue Mar 19 14:46:27 2019 +0000
Revision:
12:0b40dc152fe2
Parent:
11:aaab65e884be
Child:
13:3f239fa868f2
This accomplishes the task. May be a slight error with losing the line

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