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@3:d7bda2ab309d, 2019-01-30 (annotated)

Committer:
mtag
Date:
Wed Jan 30 18:14:58 2019 +0000
Revision:
3:d7bda2ab309d
Parent:
2:f0610c06721d
Child:
4:ace17b63da3c
Removed digital

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mtag 0:da3669e7df20 1 #include "mbed.h"
mtag 0:da3669e7df20 2
mtag 0:da3669e7df20 3 //For the solenoid
mtag 0:da3669e7df20 4 #define OFF 0
mtag 0:da3669e7df20 5 #define ON 1
mtag 0:da3669e7df20 6
mtag 0:da3669e7df20 7 //For motor control
mtag 0:da3669e7df20 8 #define PWM_PERIOD_US 1000 //For setting PWM periods to 1 milliseconds. I made this number up
mtag 0:da3669e7df20 9 #define STOP 0
mtag 0:da3669e7df20 10 #define FORWARD 1
mtag 0:da3669e7df20 11 #define BACKWARD 2
mtag 0:da3669e7df20 12 //For line following, use the previous defines and the follwoing
mtag 0:da3669e7df20 13 #define LEFT 3
mtag 0:da3669e7df20 14 #define RIGHT 4
mtag 0:da3669e7df20 15
mtag 1:c5b58b10970d 16 //For colour detection
mtag 0:da3669e7df20 17 #define COLOUR_THRESHOLD 150 //Will have to tune this value
mtag 0:da3669e7df20 18
mtag 0:da3669e7df20 19
mtag 0:da3669e7df20 20 DigitalOut myled(LED1); // Debug led
mtag 0:da3669e7df20 21
mtag 0:da3669e7df20 22 //For the colour sensor
mtag 3:d7bda2ab309d 23 I2C i2c(PTC11, PTC10); //pins for I2C communication (SDA, SCL)
mtag 0:da3669e7df20 24
mtag 2:f0610c06721d 25
mtag 0:da3669e7df20 26
mtag 0:da3669e7df20 27 //Set PWMs for controlling the H-bridge for the motor speed
mtag 0:da3669e7df20 28 PwmOut PWMmotorLeft(PTA4); //Connect to EN1 of L298N
mtag 0:da3669e7df20 29 PwmOut PWMmotorRight(PTA5); //Connect to EN1 of L298N
mtag 0:da3669e7df20 30
mtag 0:da3669e7df20 31 BusOut leftMotorMode(PTC17,PTC16); //Connect D4 to IN1, D5 to IN2 of L298N
mtag 0:da3669e7df20 32 BusOut rightMotorMode(PTC13,PTC12); //Connect D6 to IN3, D7 to IN4 of L298N
mtag 0:da3669e7df20 33
mtag 0:da3669e7df20 34 DigitalOut solenoid(PTA3); //Switch for the solenoid
mtag 0:da3669e7df20 35
mtag 0:da3669e7df20 36 //For black line detection
mtag 2:f0610c06721d 37 AnalogIn QTR3A_1(A0);
mtag 2:f0610c06721d 38 AnalogIn QTR3A_2(A1);
mtag 2:f0610c06721d 39 AnalogIn QTR3A_3(A2);
mtag 0:da3669e7df20 40
mtag 2:f0610c06721d 41 Serial bluetooth(PTE0,PTE1);
mtag 2:f0610c06721d 42
mtag 0:da3669e7df20 43 bool red_path = false;
mtag 0:da3669e7df20 44 bool blue_path = false;
mtag 0:da3669e7df20 45
mtag 2:f0610c06721d 46 int sensor_addr = 41 << 1;
mtag 2:f0610c06721d 47
mtag 2:f0610c06721d 48
mtag 0:da3669e7df20 49 class SolenoidController {
mtag 0:da3669e7df20 50 public:
mtag 0:da3669e7df20 51 bool state;
mtag 0:da3669e7df20 52
mtag 0:da3669e7df20 53 void off();
mtag 0:da3669e7df20 54 void on();
mtag 0:da3669e7df20 55 };
mtag 0:da3669e7df20 56
mtag 0:da3669e7df20 57 void SolenoidController::off() {
mtag 0:da3669e7df20 58 state = OFF;
mtag 0:da3669e7df20 59 solenoid = OFF;
mtag 0:da3669e7df20 60 }
mtag 0:da3669e7df20 61
mtag 0:da3669e7df20 62 void SolenoidController::on() {
mtag 0:da3669e7df20 63 state = ON;
mtag 0:da3669e7df20 64 solenoid = ON;
mtag 0:da3669e7df20 65 }
mtag 0:da3669e7df20 66
mtag 0:da3669e7df20 67
mtag 0:da3669e7df20 68 class MotorController {
mtag 0:da3669e7df20 69 public:
mtag 0:da3669e7df20 70 int state;
mtag 0:da3669e7df20 71 int speed;
mtag 0:da3669e7df20 72
mtag 0:da3669e7df20 73
mtag 0:da3669e7df20 74 void initialize();
mtag 0:da3669e7df20 75 void setSpeed(int pulsewidth_us);
mtag 0:da3669e7df20 76 void setLeftMotorSpeed(int pulsewidth_us);
mtag 0:da3669e7df20 77 void setRightMotorSpeed(int pulsewidth_us);
mtag 0:da3669e7df20 78 void stopMotors();
mtag 0:da3669e7df20 79 void goForward();
mtag 0:da3669e7df20 80 void goBackward();
mtag 0:da3669e7df20 81 void turnLeft();
mtag 0:da3669e7df20 82 void turnRight();
mtag 0:da3669e7df20 83 void changeDirection(int direction);
mtag 1:c5b58b10970d 84
mtag 1:c5b58b10970d 85 private:
mtag 1:c5b58b10970d 86 void setLeftMotorMode(int mode);
mtag 1:c5b58b10970d 87 void setRightMotorMode(int mode);
mtag 0:da3669e7df20 88 };
mtag 0:da3669e7df20 89
mtag 0:da3669e7df20 90 void MotorController::initialize()
mtag 0:da3669e7df20 91 {
mtag 0:da3669e7df20 92 state = STOP;
mtag 0:da3669e7df20 93 speed = 0;
mtag 0:da3669e7df20 94 PWMmotorLeft.period_us(PWM_PERIOD_US);
mtag 0:da3669e7df20 95 PWMmotorRight.period_us(PWM_PERIOD_US);
mtag 0:da3669e7df20 96
mtag 0:da3669e7df20 97 }
mtag 0:da3669e7df20 98
mtag 0:da3669e7df20 99
mtag 0:da3669e7df20 100 void MotorController::setLeftMotorSpeed(int pulsewidth_us)
mtag 0:da3669e7df20 101 {
mtag 0:da3669e7df20 102 PWMmotorLeft.pulsewidth_us(pulsewidth_us);
mtag 0:da3669e7df20 103 }
mtag 0:da3669e7df20 104
mtag 0:da3669e7df20 105
mtag 0:da3669e7df20 106 void MotorController::setRightMotorSpeed(int pulsewidth_us)
mtag 0:da3669e7df20 107 {
mtag 0:da3669e7df20 108 PWMmotorRight.pulsewidth_us(pulsewidth_us);
mtag 0:da3669e7df20 109 }
mtag 0:da3669e7df20 110
mtag 0:da3669e7df20 111
mtag 0:da3669e7df20 112 void MotorController::setLeftMotorMode(int mode)
mtag 0:da3669e7df20 113 {
mtag 0:da3669e7df20 114 leftMotorMode = mode;
mtag 0:da3669e7df20 115 }
mtag 0:da3669e7df20 116
mtag 0:da3669e7df20 117 void MotorController::setRightMotorMode(int mode)
mtag 0:da3669e7df20 118 {
mtag 0:da3669e7df20 119 rightMotorMode = mode;
mtag 0:da3669e7df20 120 }
mtag 0:da3669e7df20 121
mtag 0:da3669e7df20 122
mtag 0:da3669e7df20 123 void MotorController::stopMotors()
mtag 0:da3669e7df20 124 {
mtag 0:da3669e7df20 125 setLeftMotorMode(STOP);
mtag 0:da3669e7df20 126 setRightMotorMode(STOP);
mtag 0:da3669e7df20 127 }
mtag 0:da3669e7df20 128
mtag 0:da3669e7df20 129 void MotorController::goForward()
mtag 0:da3669e7df20 130 {
mtag 0:da3669e7df20 131 state = FORWARD;
mtag 0:da3669e7df20 132
mtag 0:da3669e7df20 133 setLeftMotorMode(FORWARD);
mtag 0:da3669e7df20 134 setRightMotorMode(FORWARD);
mtag 0:da3669e7df20 135
mtag 0:da3669e7df20 136 setLeftMotorSpeed(speed);
mtag 0:da3669e7df20 137 setRightMotorSpeed(speed);
mtag 0:da3669e7df20 138
mtag 0:da3669e7df20 139 }
mtag 0:da3669e7df20 140
mtag 0:da3669e7df20 141 void MotorController::goBackward()
mtag 0:da3669e7df20 142 {
mtag 0:da3669e7df20 143 state = BACKWARD;
mtag 0:da3669e7df20 144
mtag 0:da3669e7df20 145 setLeftMotorMode(BACKWARD);
mtag 0:da3669e7df20 146 setRightMotorMode(BACKWARD);
mtag 0:da3669e7df20 147
mtag 0:da3669e7df20 148 setLeftMotorSpeed(speed);
mtag 0:da3669e7df20 149 setRightMotorSpeed(speed);
mtag 0:da3669e7df20 150
mtag 0:da3669e7df20 151 }
mtag 0:da3669e7df20 152
mtag 0:da3669e7df20 153 void MotorController::turnLeft()
mtag 0:da3669e7df20 154 {
mtag 0:da3669e7df20 155 state = LEFT;
mtag 0:da3669e7df20 156
mtag 0:da3669e7df20 157 setLeftMotorMode(BACKWARD);
mtag 0:da3669e7df20 158 setRightMotorMode(FORWARD);
mtag 0:da3669e7df20 159
mtag 0:da3669e7df20 160 setLeftMotorSpeed(speed);
mtag 0:da3669e7df20 161 setRightMotorSpeed(speed);
mtag 0:da3669e7df20 162
mtag 0:da3669e7df20 163 }
mtag 0:da3669e7df20 164
mtag 0:da3669e7df20 165
mtag 0:da3669e7df20 166 void MotorController::turnRight()
mtag 0:da3669e7df20 167 {
mtag 0:da3669e7df20 168 state = RIGHT;
mtag 0:da3669e7df20 169
mtag 0:da3669e7df20 170 setLeftMotorMode(FORWARD);
mtag 0:da3669e7df20 171 setRightMotorMode(BACKWARD);
mtag 0:da3669e7df20 172
mtag 0:da3669e7df20 173 setLeftMotorSpeed(speed);
mtag 0:da3669e7df20 174 setRightMotorSpeed(speed);
mtag 0:da3669e7df20 175 }
mtag 0:da3669e7df20 176
mtag 0:da3669e7df20 177 void MotorController::changeDirection(int direction) {
mtag 0:da3669e7df20 178
mtag 0:da3669e7df20 179 switch(direction) {
mtag 0:da3669e7df20 180
mtag 0:da3669e7df20 181 case STOP:
mtag 0:da3669e7df20 182 stopMotors();
mtag 0:da3669e7df20 183 break;
mtag 0:da3669e7df20 184
mtag 0:da3669e7df20 185 case FORWARD:
mtag 0:da3669e7df20 186 goForward();
mtag 0:da3669e7df20 187 break;
mtag 0:da3669e7df20 188
mtag 0:da3669e7df20 189 case BACKWARD:
mtag 0:da3669e7df20 190 goBackward();
mtag 0:da3669e7df20 191 break;
mtag 0:da3669e7df20 192
mtag 0:da3669e7df20 193 case LEFT:
mtag 0:da3669e7df20 194 turnLeft();
mtag 0:da3669e7df20 195 break;
mtag 0:da3669e7df20 196
mtag 0:da3669e7df20 197 case RIGHT:
mtag 0:da3669e7df20 198 turnRight();
mtag 0:da3669e7df20 199 break;
mtag 0:da3669e7df20 200
mtag 0:da3669e7df20 201 default:
mtag 0:da3669e7df20 202 stopMotors();
mtag 0:da3669e7df20 203 break;
mtag 0:da3669e7df20 204
mtag 0:da3669e7df20 205 }
mtag 0:da3669e7df20 206 }
mtag 0:da3669e7df20 207
mtag 0:da3669e7df20 208 void MotorController::setSpeed(int pulsewidth_us) {
mtag 0:da3669e7df20 209 speed = pulsewidth_us;
mtag 0:da3669e7df20 210 }
mtag 0:da3669e7df20 211
mtag 0:da3669e7df20 212 class ColourSensor {
mtag 0:da3669e7df20 213 public:
mtag 2:f0610c06721d 214
mtag 0:da3669e7df20 215 bool blue_detected;
mtag 0:da3669e7df20 216 bool red_detected;
mtag 0:da3669e7df20 217
mtag 0:da3669e7df20 218 void initialize();
mtag 0:da3669e7df20 219 void read();
mtag 2:f0610c06721d 220
mtag 2:f0610c06721d 221 private:
mtag 2:f0610c06721d 222 const static int RED_CLEAR_VALUE_MAX = 20000;
mtag 2:f0610c06721d 223 const static int BLUE_CLEAR_VALUE_MAX = 55000;
mtag 2:f0610c06721d 224
mtag 0:da3669e7df20 225 };
mtag 0:da3669e7df20 226
mtag 0:da3669e7df20 227
mtag 0:da3669e7df20 228 void ColourSensor::initialize() {
mtag 0:da3669e7df20 229
mtag 0:da3669e7df20 230 i2c.frequency(200000);
mtag 0:da3669e7df20 231
mtag 0:da3669e7df20 232 blue_detected = false;
mtag 0:da3669e7df20 233 red_detected = false;
mtag 0:da3669e7df20 234
mtag 0:da3669e7df20 235 char id_regval[1] = {146};
mtag 0:da3669e7df20 236 char data[1] = {0};
mtag 0:da3669e7df20 237 i2c.write(sensor_addr,id_regval,1, true);
mtag 0:da3669e7df20 238 i2c.read(sensor_addr,data,1,false);
mtag 0:da3669e7df20 239
mtag 0:da3669e7df20 240 if (data[0]==68) {
mtag 0:da3669e7df20 241 myled = 0;
mtag 0:da3669e7df20 242 wait (2);
mtag 0:da3669e7df20 243 myled = 1;
mtag 0:da3669e7df20 244 } else {
mtag 0:da3669e7df20 245 myled = 1;
mtag 0:da3669e7df20 246 }
mtag 0:da3669e7df20 247
mtag 0:da3669e7df20 248 char timing_register[2] = {129,0};
mtag 0:da3669e7df20 249 i2c.write(sensor_addr,timing_register,2,false);
mtag 0:da3669e7df20 250
mtag 0:da3669e7df20 251 char control_register[2] = {143,0};
mtag 0:da3669e7df20 252 i2c.write(sensor_addr,control_register,2,false);
mtag 0:da3669e7df20 253
mtag 0:da3669e7df20 254 char enable_register[2] = {128,3};
mtag 0:da3669e7df20 255 i2c.write(sensor_addr,enable_register,2,false);
mtag 0:da3669e7df20 256 }
mtag 0:da3669e7df20 257
mtag 0:da3669e7df20 258 void ColourSensor::read() {
mtag 0:da3669e7df20 259
mtag 0:da3669e7df20 260 char clear_reg[1] = {148};
mtag 0:da3669e7df20 261 char clear_data[2] = {0,0};
mtag 0:da3669e7df20 262 i2c.write(sensor_addr,clear_reg,1, true);
mtag 0:da3669e7df20 263 i2c.read(sensor_addr,clear_data,2, false);
mtag 0:da3669e7df20 264
mtag 0:da3669e7df20 265 int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
mtag 0:da3669e7df20 266
mtag 0:da3669e7df20 267 char red_reg[1] = {150};
mtag 0:da3669e7df20 268 char red_data[2] = {0,0};
mtag 0:da3669e7df20 269 i2c.write(sensor_addr,red_reg,1, true);
mtag 0:da3669e7df20 270 i2c.read(sensor_addr,red_data,2, false);
mtag 0:da3669e7df20 271
mtag 0:da3669e7df20 272 int red_value = ((int)red_data[1] << 8) | red_data[0];
mtag 0:da3669e7df20 273
mtag 0:da3669e7df20 274 char green_reg[1] = {152};
mtag 0:da3669e7df20 275 char green_data[2] = {0,0};
mtag 0:da3669e7df20 276 i2c.write(sensor_addr,green_reg,1, true);
mtag 0:da3669e7df20 277 i2c.read(sensor_addr,green_data,2, false);
mtag 0:da3669e7df20 278
mtag 0:da3669e7df20 279 int green_value = ((int)green_data[1] << 8) | green_data[0];
mtag 0:da3669e7df20 280
mtag 0:da3669e7df20 281 char blue_reg[1] = {154};
mtag 0:da3669e7df20 282 char blue_data[2] = {0,0};
mtag 0:da3669e7df20 283 i2c.write(sensor_addr,blue_reg,1, true);
mtag 0:da3669e7df20 284 i2c.read(sensor_addr,blue_data,2, false);
mtag 0:da3669e7df20 285
mtag 0:da3669e7df20 286 int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
mtag 0:da3669e7df20 287
mtag 0:da3669e7df20 288
mtag 0:da3669e7df20 289 //Detect the colour of the paper
mtag 0:da3669e7df20 290
mtag 2:f0610c06721d 291 if(clear_value < RED_CLEAR_VALUE_MAX && (red_value > (blue_value*2))) {
mtag 0:da3669e7df20 292 red_detected = true;
mtag 0:da3669e7df20 293 }
mtag 0:da3669e7df20 294
mtag 0:da3669e7df20 295 else {
mtag 0:da3669e7df20 296 red_detected = false;
mtag 0:da3669e7df20 297 }
mtag 0:da3669e7df20 298
mtag 2:f0610c06721d 299 if(clear_value < BLUE_CLEAR_VALUE_MAX && blue_value > (red_value*2)) {
mtag 0:da3669e7df20 300 blue_detected = true;
mtag 0:da3669e7df20 301 }
mtag 0:da3669e7df20 302
mtag 0:da3669e7df20 303 else {
mtag 0:da3669e7df20 304 blue_detected = false;
mtag 0:da3669e7df20 305 }
mtag 0:da3669e7df20 306
mtag 0:da3669e7df20 307
mtag 0:da3669e7df20 308 }
mtag 0:da3669e7df20 309
mtag 0:da3669e7df20 310 class LineFollower {
mtag 2:f0610c06721d 311
mtag 0:da3669e7df20 312 public:
mtag 0:da3669e7df20 313 bool lineDetected1;
mtag 0:da3669e7df20 314 bool lineDetected2;
mtag 0:da3669e7df20 315 bool lineDetected3;
mtag 0:da3669e7df20 316 int direction;
mtag 0:da3669e7df20 317
mtag 0:da3669e7df20 318 void initialize();
mtag 2:f0610c06721d 319 void readSensors();
mtag 2:f0610c06721d 320 int chooseDirection();
mtag 0:da3669e7df20 321
mtag 2:f0610c06721d 322 private:
mtag 2:f0610c06721d 323
mtag 2:f0610c06721d 324 const static float LINE_THRESHOLD = 0.5;
mtag 0:da3669e7df20 325 void readSensor1();
mtag 0:da3669e7df20 326 void readSensor2();
mtag 0:da3669e7df20 327 void readSensor3();
mtag 0:da3669e7df20 328
mtag 2:f0610c06721d 329
mtag 0:da3669e7df20 330
mtag 0:da3669e7df20 331 };
mtag 0:da3669e7df20 332
mtag 0:da3669e7df20 333 void LineFollower::initialize() {
mtag 0:da3669e7df20 334 lineDetected1 = false;
mtag 0:da3669e7df20 335 lineDetected2 = false;
mtag 0:da3669e7df20 336 lineDetected3 = false;
mtag 0:da3669e7df20 337 direction = STOP;
mtag 0:da3669e7df20 338 }
mtag 0:da3669e7df20 339
mtag 0:da3669e7df20 340 void LineFollower::readSensor1() {
mtag 2:f0610c06721d 341 if(QTR3A_1.read() > LINE_THRESHOLD) {
mtag 2:f0610c06721d 342 lineDetected1 = true;
mtag 2:f0610c06721d 343 }
mtag 2:f0610c06721d 344
mtag 2:f0610c06721d 345 else {
mtag 2:f0610c06721d 346 lineDetected1 = false;
mtag 2:f0610c06721d 347 }
mtag 0:da3669e7df20 348 }
mtag 0:da3669e7df20 349
mtag 0:da3669e7df20 350 void LineFollower::readSensor2() {
mtag 2:f0610c06721d 351 if(QTR3A_2.read() > LINE_THRESHOLD) {
mtag 2:f0610c06721d 352 lineDetected2 = true;
mtag 2:f0610c06721d 353 }
mtag 2:f0610c06721d 354
mtag 2:f0610c06721d 355 else {
mtag 2:f0610c06721d 356 lineDetected2 = false;
mtag 2:f0610c06721d 357 }
mtag 0:da3669e7df20 358 }
mtag 0:da3669e7df20 359
mtag 0:da3669e7df20 360 void LineFollower::readSensor3() {
mtag 2:f0610c06721d 361 if(QTR3A_3.read() > LINE_THRESHOLD) {
mtag 2:f0610c06721d 362 lineDetected3 = true;
mtag 2:f0610c06721d 363 }
mtag 2:f0610c06721d 364
mtag 2:f0610c06721d 365 else {
mtag 2:f0610c06721d 366 lineDetected3 = false;
mtag 2:f0610c06721d 367 }
mtag 0:da3669e7df20 368 }
mtag 0:da3669e7df20 369
mtag 0:da3669e7df20 370 void LineFollower::readSensors() {
mtag 0:da3669e7df20 371 readSensor1();
mtag 0:da3669e7df20 372 readSensor2();
mtag 0:da3669e7df20 373 readSensor3();
mtag 0:da3669e7df20 374 }
mtag 0:da3669e7df20 375
mtag 0:da3669e7df20 376 int LineFollower::chooseDirection() {
mtag 0:da3669e7df20 377
mtag 2:f0610c06721d 378 int sensorData = 0x00 & ((((int) lineDetected1) << 2) + (((int) lineDetected2) << 1) + ((int) lineDetected3));
mtag 2:f0610c06721d 379 sensorData = sensorData & 0x07;
mtag 0:da3669e7df20 380
mtag 0:da3669e7df20 381 switch(sensorData) {
mtag 0:da3669e7df20 382
mtag 1:c5b58b10970d 383 //000
mtag 2:f0610c06721d 384 case 0x0:
mtag 0:da3669e7df20 385 direction = STOP;
mtag 0:da3669e7df20 386 break;
mtag 0:da3669e7df20 387
mtag 1:c5b58b10970d 388 //001
mtag 1:c5b58b10970d 389 case 0x1:
mtag 0:da3669e7df20 390 direction = RIGHT;
mtag 0:da3669e7df20 391 break;
mtag 0:da3669e7df20 392
mtag 1:c5b58b10970d 393 //010
mtag 1:c5b58b10970d 394 case 0x2:
mtag 0:da3669e7df20 395 direction = FORWARD;
mtag 0:da3669e7df20 396 break;
mtag 0:da3669e7df20 397
mtag 1:c5b58b10970d 398 //011
mtag 1:c5b58b10970d 399 case 0x3:
mtag 0:da3669e7df20 400 direction = RIGHT;
mtag 0:da3669e7df20 401 break;
mtag 0:da3669e7df20 402
mtag 1:c5b58b10970d 403 //100
mtag 1:c5b58b10970d 404 case 0x4:
mtag 0:da3669e7df20 405 direction = LEFT;
mtag 0:da3669e7df20 406 break;
mtag 0:da3669e7df20 407
mtag 1:c5b58b10970d 408 //101
mtag 1:c5b58b10970d 409 case 0x5:
mtag 0:da3669e7df20 410 if(red_path) {
mtag 0:da3669e7df20 411 direction = LEFT;
mtag 0:da3669e7df20 412 }
mtag 0:da3669e7df20 413
mtag 0:da3669e7df20 414 if(blue_path) {
mtag 0:da3669e7df20 415 direction = RIGHT;
mtag 0:da3669e7df20 416 }
mtag 0:da3669e7df20 417
mtag 0:da3669e7df20 418 break;
mtag 1:c5b58b10970d 419
mtag 1:c5b58b10970d 420 //110
mtag 1:c5b58b10970d 421 case 0x06:
mtag 1:c5b58b10970d 422 direction = RIGHT;
mtag 1:c5b58b10970d 423 break;
mtag 1:c5b58b10970d 424
mtag 1:c5b58b10970d 425 //111
mtag 1:c5b58b10970d 426 case 0x7:
mtag 0:da3669e7df20 427 direction = FORWARD;
mtag 0:da3669e7df20 428 break;
mtag 0:da3669e7df20 429
mtag 0:da3669e7df20 430 default:
mtag 0:da3669e7df20 431 direction = FORWARD;
mtag 0:da3669e7df20 432 break;
mtag 0:da3669e7df20 433 }
mtag 0:da3669e7df20 434 return direction;
mtag 0:da3669e7df20 435 }
mtag 0:da3669e7df20 436
mtag 2:f0610c06721d 437
mtag 2:f0610c06721d 438 void bluetoothControl(MotorController motorController) {
mtag 2:f0610c06721d 439 bluetooth.baud(9600);
mtag 2:f0610c06721d 440
mtag 2:f0610c06721d 441 char c = '0';
mtag 2:f0610c06721d 442 char state = 'F';
mtag 2:f0610c06721d 443 int speed = 0;
mtag 2:f0610c06721d 444
mtag 2:f0610c06721d 445 while(true) {
mtag 2:f0610c06721d 446
mtag 2:f0610c06721d 447 c='0';
mtag 2:f0610c06721d 448
mtag 2:f0610c06721d 449 if(bluetooth.readable()) {
mtag 2:f0610c06721d 450 c = bluetooth.getc();
mtag 2:f0610c06721d 451 }
mtag 2:f0610c06721d 452
mtag 2:f0610c06721d 453
mtag 2:f0610c06721d 454
mtag 2:f0610c06721d 455 switch(c) {
mtag 2:f0610c06721d 456
mtag 2:f0610c06721d 457 case 'F':
mtag 2:f0610c06721d 458 if(state != 'F') {
mtag 2:f0610c06721d 459 state = 'F';
mtag 2:f0610c06721d 460 speed = 400;
mtag 2:f0610c06721d 461 motorController.setSpeed(speed);
mtag 2:f0610c06721d 462 motorController.goForward();
mtag 2:f0610c06721d 463 }
mtag 2:f0610c06721d 464
mtag 2:f0610c06721d 465 else {
mtag 2:f0610c06721d 466 speed += 100;
mtag 2:f0610c06721d 467 motorController.setSpeed(speed);
mtag 2:f0610c06721d 468 motorController.goForward();
mtag 2:f0610c06721d 469 }
mtag 2:f0610c06721d 470 break;
mtag 2:f0610c06721d 471
mtag 2:f0610c06721d 472 case 'B':
mtag 2:f0610c06721d 473 if(state != 'B') {
mtag 2:f0610c06721d 474 state = 'B';
mtag 2:f0610c06721d 475 speed = 400;
mtag 2:f0610c06721d 476 motorController.setSpeed(speed);
mtag 2:f0610c06721d 477 motorController.goBackward();
mtag 2:f0610c06721d 478 }
mtag 2:f0610c06721d 479
mtag 2:f0610c06721d 480 else {
mtag 2:f0610c06721d 481 speed += 100;
mtag 2:f0610c06721d 482 motorController.setSpeed(speed);
mtag 2:f0610c06721d 483 motorController.goBackward();
mtag 2:f0610c06721d 484 }
mtag 2:f0610c06721d 485 break;
mtag 2:f0610c06721d 486
mtag 2:f0610c06721d 487 case 'L':
mtag 2:f0610c06721d 488 if(state != 'L') {
mtag 2:f0610c06721d 489 state = 'L';
mtag 2:f0610c06721d 490 speed = 800;
mtag 2:f0610c06721d 491 motorController.setSpeed(speed);
mtag 2:f0610c06721d 492 motorController.turnLeft();
mtag 2:f0610c06721d 493 }
mtag 2:f0610c06721d 494
mtag 2:f0610c06721d 495 else {
mtag 2:f0610c06721d 496 speed += 100;
mtag 2:f0610c06721d 497 motorController.setSpeed(speed);
mtag 2:f0610c06721d 498 motorController.turnLeft();
mtag 2:f0610c06721d 499 }
mtag 2:f0610c06721d 500 break;
mtag 2:f0610c06721d 501
mtag 2:f0610c06721d 502 case 'R':
mtag 2:f0610c06721d 503 if(state != 'R') {
mtag 2:f0610c06721d 504 state = 'R';
mtag 2:f0610c06721d 505 speed = 800;
mtag 2:f0610c06721d 506 motorController.setSpeed(speed);
mtag 2:f0610c06721d 507 motorController.turnRight();
mtag 2:f0610c06721d 508 }
mtag 2:f0610c06721d 509
mtag 2:f0610c06721d 510 else {
mtag 2:f0610c06721d 511 speed += 100;
mtag 2:f0610c06721d 512 motorController.setSpeed(speed);
mtag 2:f0610c06721d 513 motorController.turnRight();
mtag 2:f0610c06721d 514 }
mtag 2:f0610c06721d 515 break;
mtag 2:f0610c06721d 516
mtag 2:f0610c06721d 517 case 'S':
mtag 2:f0610c06721d 518 state = 'S';
mtag 2:f0610c06721d 519 speed = 0;
mtag 2:f0610c06721d 520 motorController.setSpeed(speed);
mtag 2:f0610c06721d 521 motorController.stopMotors();
mtag 2:f0610c06721d 522 break;
mtag 2:f0610c06721d 523
mtag 2:f0610c06721d 524 }
mtag 2:f0610c06721d 525 }
mtag 2:f0610c06721d 526 }
mtag 2:f0610c06721d 527
mtag 2:f0610c06721d 528
mtag 2:f0610c06721d 529
mtag 0:da3669e7df20 530 int main() {
mtag 0:da3669e7df20 531
mtag 0:da3669e7df20 532 //Blink LED to let you know it's on
mtag 0:da3669e7df20 533 myled = 0;
mtag 0:da3669e7df20 534 wait(0.5);
mtag 0:da3669e7df20 535 myled = 1;
mtag 0:da3669e7df20 536 wait(0.5);
mtag 0:da3669e7df20 537 myled = 0;
mtag 0:da3669e7df20 538
mtag 0:da3669e7df20 539 bool paper_detected = false;
mtag 0:da3669e7df20 540
mtag 0:da3669e7df20 541 MotorController motorController;
mtag 0:da3669e7df20 542 SolenoidController solenoidController;
mtag 0:da3669e7df20 543 LineFollower lineFollower;
mtag 0:da3669e7df20 544 ColourSensor colourSensor;
mtag 0:da3669e7df20 545
mtag 0:da3669e7df20 546 motorController.initialize();
mtag 0:da3669e7df20 547 lineFollower.initialize();
mtag 0:da3669e7df20 548 colourSensor.initialize();
mtag 0:da3669e7df20 549 solenoidController.off();
mtag 0:da3669e7df20 550
mtag 2:f0610c06721d 551
mtag 2:f0610c06721d 552 //Start off going straight
mtag 2:f0610c06721d 553 motorController.setSpeed(700);
mtag 0:da3669e7df20 554 motorController.goForward();
mtag 0:da3669e7df20 555
mtag 0:da3669e7df20 556
mtag 0:da3669e7df20 557 while(true) {
mtag 0:da3669e7df20 558
mtag 2:f0610c06721d 559 if(bluetooth.readable()) {
mtag 2:f0610c06721d 560 bluetoothControl(motorController);
mtag 2:f0610c06721d 561 }
mtag 2:f0610c06721d 562
mtag 0:da3669e7df20 563 lineFollower.readSensors();
mtag 0:da3669e7df20 564 motorController.changeDirection(lineFollower.chooseDirection());
mtag 0:da3669e7df20 565
mtag 0:da3669e7df20 566 colourSensor.read();
mtag 0:da3669e7df20 567
mtag 0:da3669e7df20 568 //Logic for the solenoid based on colour detected
mtag 0:da3669e7df20 569
mtag 0:da3669e7df20 570 //Detect the first sheet of paper if blue and pick up the disc
mtag 0:da3669e7df20 571 if(colourSensor.blue_detected && !paper_detected && !solenoidController.state) {
mtag 0:da3669e7df20 572 paper_detected = true;
mtag 0:da3669e7df20 573 blue_path = true;
mtag 0:da3669e7df20 574 solenoidController.on();
mtag 0:da3669e7df20 575 }
mtag 0:da3669e7df20 576
mtag 0:da3669e7df20 577 //Detect the first sheet of paper if red and pick up the disc
mtag 0:da3669e7df20 578 if(colourSensor.red_detected && !paper_detected && !solenoidController.state) {
mtag 0:da3669e7df20 579 paper_detected = true;
mtag 0:da3669e7df20 580 red_path = true;
mtag 0:da3669e7df20 581 solenoidController.on();
mtag 0:da3669e7df20 582 }
mtag 0:da3669e7df20 583
mtag 0:da3669e7df20 584 //Detect the end of the first sheet of paper
mtag 0:da3669e7df20 585 if(!colourSensor.blue_detected && !colourSensor.red_detected && paper_detected) {
mtag 0:da3669e7df20 586 paper_detected = false;
mtag 0:da3669e7df20 587 }
mtag 0:da3669e7df20 588
mtag 1:c5b58b10970d 589 //Drop the disc once the second blue paper is detected
mtag 0:da3669e7df20 590 if(colourSensor.blue_detected && blue_path && !paper_detected && solenoidController.state) {
mtag 0:da3669e7df20 591 paper_detected = true;
mtag 0:da3669e7df20 592 solenoidController.off();
mtag 0:da3669e7df20 593 }
mtag 0:da3669e7df20 594
mtag 1:c5b58b10970d 595 //Drop the disc once the second red paper is detected
mtag 0:da3669e7df20 596 if(colourSensor.red_detected && red_path && !paper_detected && solenoidController.state) {
mtag 0:da3669e7df20 597 paper_detected = true;
mtag 0:da3669e7df20 598 solenoidController.off();
mtag 0:da3669e7df20 599 }
mtag 0:da3669e7df20 600
mtag 0:da3669e7df20 601 }
mtag 0:da3669e7df20 602
mtag 0:da3669e7df20 603 }