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@6:1056ed1d6d97, 2019-03-04 (annotated)

Committer:
mtag
Date:
Mon Mar 04 10:52:03 2019 +0000
Revision:
6:1056ed1d6d97
Parent:
5:92510334cdfe
Child:
7:1e5fa5952695
a

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