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@4:ace17b63da3c, 2019-03-01 (annotated)

Committer:
mtag
Date:
Fri Mar 01 15:22:34 2019 +0000
Revision:
4:ace17b63da3c
Parent:
3:d7bda2ab309d
Child:
5:92510334cdfe
fixed ifs

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