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

Committer:
mtag
Date:
Wed Jan 30 18:12:53 2019 +0000
Revision:
2:f0610c06721d
Parent:
1:c5b58b10970d
Child:
3:d7bda2ab309d
Edited some stuff to be more correct, sensor calibration;

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