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@1:c5b58b10970d, 2019-01-16 (annotated)

Committer:
mtag
Date:
Wed Jan 16 17:38:09 2019 +0000
Revision:
1:c5b58b10970d
Parent:
0:da3669e7df20
Child:
2:f0610c06721d
Changed the code for the the line following logic

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
mtag 0:da3669e7df20 8 //For motor control
mtag 0:da3669e7df20 9 #define PWM_PERIOD_US 1000 //For setting PWM periods to 1 milliseconds. I made this number up
mtag 0:da3669e7df20 10 #define STOP 0
mtag 0:da3669e7df20 11 #define FORWARD 1
mtag 0:da3669e7df20 12 #define BACKWARD 2
mtag 0:da3669e7df20 13 //For line following, use the previous defines and the follwoing
mtag 0:da3669e7df20 14 #define LEFT 3
mtag 0:da3669e7df20 15 #define RIGHT 4
mtag 0:da3669e7df20 16
mtag 1:c5b58b10970d 17 //For colour detection
mtag 0:da3669e7df20 18 #define COLOUR_THRESHOLD 150 //Will have to tune this value
mtag 0:da3669e7df20 19
mtag 0:da3669e7df20 20
mtag 0:da3669e7df20 21 DigitalOut myled(LED1); // Debug led
mtag 0:da3669e7df20 22
mtag 0:da3669e7df20 23 //For the colour sensor
mtag 0:da3669e7df20 24 I2C i2c(I2C_SDA, I2C_SCL); //pins for I2C communication (SDA, SCL)
mtag 0:da3669e7df20 25
mtag 0:da3669e7df20 26 int sensor_addr = 41 << 1;
mtag 0:da3669e7df20 27
mtag 0:da3669e7df20 28 //Set PWMs for controlling the H-bridge for the motor speed
mtag 0:da3669e7df20 29 PwmOut PWMmotorLeft(PTA4); //Connect to EN1 of L298N
mtag 0:da3669e7df20 30 PwmOut PWMmotorRight(PTA5); //Connect to EN1 of L298N
mtag 0:da3669e7df20 31
mtag 0:da3669e7df20 32 BusOut leftMotorMode(PTC17,PTC16); //Connect D4 to IN1, D5 to IN2 of L298N
mtag 0:da3669e7df20 33 BusOut rightMotorMode(PTC13,PTC12); //Connect D6 to IN3, D7 to IN4 of L298N
mtag 0:da3669e7df20 34
mtag 0:da3669e7df20 35 DigitalOut solenoid(PTA3); //Switch for the solenoid
mtag 0:da3669e7df20 36
mtag 0:da3669e7df20 37 //For black line detection
mtag 0:da3669e7df20 38 DigitalIn lineSensor1(PTA6);
mtag 0:da3669e7df20 39 DigitalIn lineSensor2(PTA7);
mtag 0:da3669e7df20 40 DigitalIn lineSensor3(PTA8);
mtag 0:da3669e7df20 41
mtag 0:da3669e7df20 42 bool red_path = false;
mtag 0:da3669e7df20 43 bool blue_path = false;
mtag 0:da3669e7df20 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 0:da3669e7df20 183 break;
mtag 0:da3669e7df20 184
mtag 0:da3669e7df20 185 case BACKWARD:
mtag 0:da3669e7df20 186 goBackward();
mtag 0:da3669e7df20 187 break;
mtag 0:da3669e7df20 188
mtag 0:da3669e7df20 189 case LEFT:
mtag 0:da3669e7df20 190 turnLeft();
mtag 0:da3669e7df20 191 break;
mtag 0:da3669e7df20 192
mtag 0:da3669e7df20 193 case RIGHT:
mtag 0:da3669e7df20 194 turnRight();
mtag 0:da3669e7df20 195 break;
mtag 0:da3669e7df20 196
mtag 0:da3669e7df20 197 default:
mtag 0:da3669e7df20 198 stopMotors();
mtag 0:da3669e7df20 199 break;
mtag 0:da3669e7df20 200
mtag 0:da3669e7df20 201 }
mtag 0:da3669e7df20 202 }
mtag 0:da3669e7df20 203
mtag 0:da3669e7df20 204 void MotorController::setSpeed(int pulsewidth_us) {
mtag 0:da3669e7df20 205 speed = pulsewidth_us;
mtag 0:da3669e7df20 206 }
mtag 0:da3669e7df20 207
mtag 0:da3669e7df20 208 class ColourSensor {
mtag 0:da3669e7df20 209 public:
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 0:da3669e7df20 215 };
mtag 0:da3669e7df20 216
mtag 0:da3669e7df20 217
mtag 0:da3669e7df20 218 void ColourSensor::initialize() {
mtag 0:da3669e7df20 219
mtag 0:da3669e7df20 220 i2c.frequency(200000);
mtag 0:da3669e7df20 221
mtag 0:da3669e7df20 222 blue_detected = false;
mtag 0:da3669e7df20 223 red_detected = false;
mtag 0:da3669e7df20 224
mtag 0:da3669e7df20 225 char id_regval[1] = {146};
mtag 0:da3669e7df20 226 char data[1] = {0};
mtag 0:da3669e7df20 227 i2c.write(sensor_addr,id_regval,1, true);
mtag 0:da3669e7df20 228 i2c.read(sensor_addr,data,1,false);
mtag 0:da3669e7df20 229
mtag 0:da3669e7df20 230 if (data[0]==68) {
mtag 0:da3669e7df20 231 myled = 0;
mtag 0:da3669e7df20 232 wait (2);
mtag 0:da3669e7df20 233 myled = 1;
mtag 0:da3669e7df20 234 } else {
mtag 0:da3669e7df20 235 myled = 1;
mtag 0:da3669e7df20 236 }
mtag 0:da3669e7df20 237
mtag 0:da3669e7df20 238 char timing_register[2] = {129,0};
mtag 0:da3669e7df20 239 i2c.write(sensor_addr,timing_register,2,false);
mtag 0:da3669e7df20 240
mtag 0:da3669e7df20 241 char control_register[2] = {143,0};
mtag 0:da3669e7df20 242 i2c.write(sensor_addr,control_register,2,false);
mtag 0:da3669e7df20 243
mtag 0:da3669e7df20 244 char enable_register[2] = {128,3};
mtag 0:da3669e7df20 245 i2c.write(sensor_addr,enable_register,2,false);
mtag 0:da3669e7df20 246 }
mtag 0:da3669e7df20 247
mtag 0:da3669e7df20 248 void ColourSensor::read() {
mtag 0:da3669e7df20 249
mtag 0:da3669e7df20 250 char clear_reg[1] = {148};
mtag 0:da3669e7df20 251 char clear_data[2] = {0,0};
mtag 0:da3669e7df20 252 i2c.write(sensor_addr,clear_reg,1, true);
mtag 0:da3669e7df20 253 i2c.read(sensor_addr,clear_data,2, false);
mtag 0:da3669e7df20 254
mtag 0:da3669e7df20 255 int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
mtag 0:da3669e7df20 256
mtag 0:da3669e7df20 257 char red_reg[1] = {150};
mtag 0:da3669e7df20 258 char red_data[2] = {0,0};
mtag 0:da3669e7df20 259 i2c.write(sensor_addr,red_reg,1, true);
mtag 0:da3669e7df20 260 i2c.read(sensor_addr,red_data,2, false);
mtag 0:da3669e7df20 261
mtag 0:da3669e7df20 262 int red_value = ((int)red_data[1] << 8) | red_data[0];
mtag 0:da3669e7df20 263
mtag 0:da3669e7df20 264 char green_reg[1] = {152};
mtag 0:da3669e7df20 265 char green_data[2] = {0,0};
mtag 0:da3669e7df20 266 i2c.write(sensor_addr,green_reg,1, true);
mtag 0:da3669e7df20 267 i2c.read(sensor_addr,green_data,2, false);
mtag 0:da3669e7df20 268
mtag 0:da3669e7df20 269 int green_value = ((int)green_data[1] << 8) | green_data[0];
mtag 0:da3669e7df20 270
mtag 0:da3669e7df20 271 char blue_reg[1] = {154};
mtag 0:da3669e7df20 272 char blue_data[2] = {0,0};
mtag 0:da3669e7df20 273 i2c.write(sensor_addr,blue_reg,1, true);
mtag 0:da3669e7df20 274 i2c.read(sensor_addr,blue_data,2, false);
mtag 0:da3669e7df20 275
mtag 0:da3669e7df20 276 int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
mtag 0:da3669e7df20 277
mtag 0:da3669e7df20 278
mtag 0:da3669e7df20 279 //Detect the colour of the paper
mtag 0:da3669e7df20 280
mtag 0:da3669e7df20 281 if(red_value >= COLOUR_THRESHOLD) {
mtag 0:da3669e7df20 282 red_detected = true;
mtag 0:da3669e7df20 283 }
mtag 0:da3669e7df20 284
mtag 0:da3669e7df20 285 else {
mtag 0:da3669e7df20 286 red_detected = false;
mtag 0:da3669e7df20 287 }
mtag 0:da3669e7df20 288
mtag 0:da3669e7df20 289 if(blue_value >= COLOUR_THRESHOLD) {
mtag 0:da3669e7df20 290 blue_detected = true;
mtag 0:da3669e7df20 291 }
mtag 0:da3669e7df20 292
mtag 0:da3669e7df20 293 else {
mtag 0:da3669e7df20 294 blue_detected = false;
mtag 0:da3669e7df20 295 }
mtag 0:da3669e7df20 296
mtag 0:da3669e7df20 297
mtag 0:da3669e7df20 298 }
mtag 0:da3669e7df20 299
mtag 0:da3669e7df20 300 class LineFollower {
mtag 0:da3669e7df20 301 public:
mtag 0:da3669e7df20 302 bool lineDetected1;
mtag 0:da3669e7df20 303 bool lineDetected2;
mtag 0:da3669e7df20 304 bool lineDetected3;
mtag 0:da3669e7df20 305 int direction;
mtag 0:da3669e7df20 306
mtag 0:da3669e7df20 307 void initialize();
mtag 0:da3669e7df20 308
mtag 0:da3669e7df20 309 void readSensor1();
mtag 0:da3669e7df20 310 void readSensor2();
mtag 0:da3669e7df20 311 void readSensor3();
mtag 0:da3669e7df20 312 void readSensors();
mtag 0:da3669e7df20 313
mtag 0:da3669e7df20 314 int chooseDirection();
mtag 0:da3669e7df20 315
mtag 0:da3669e7df20 316 };
mtag 0:da3669e7df20 317
mtag 0:da3669e7df20 318 void LineFollower::initialize() {
mtag 0:da3669e7df20 319 lineDetected1 = false;
mtag 0:da3669e7df20 320 lineDetected2 = false;
mtag 0:da3669e7df20 321 lineDetected3 = false;
mtag 0:da3669e7df20 322 direction = STOP;
mtag 0:da3669e7df20 323 }
mtag 0:da3669e7df20 324
mtag 0:da3669e7df20 325 void LineFollower::readSensor1() {
mtag 0:da3669e7df20 326 lineDetected1 = lineSensor1;
mtag 0:da3669e7df20 327 }
mtag 0:da3669e7df20 328
mtag 0:da3669e7df20 329 void LineFollower::readSensor2() {
mtag 0:da3669e7df20 330 lineDetected2 = lineSensor2;
mtag 0:da3669e7df20 331 }
mtag 0:da3669e7df20 332
mtag 0:da3669e7df20 333 void LineFollower::readSensor3() {
mtag 0:da3669e7df20 334 lineDetected3 = lineSensor3;
mtag 0:da3669e7df20 335 }
mtag 0:da3669e7df20 336
mtag 0:da3669e7df20 337 void LineFollower::readSensors() {
mtag 0:da3669e7df20 338 readSensor1();
mtag 0:da3669e7df20 339 readSensor2();
mtag 0:da3669e7df20 340 readSensor3();
mtag 0:da3669e7df20 341 }
mtag 0:da3669e7df20 342
mtag 0:da3669e7df20 343 int LineFollower::chooseDirection() {
mtag 0:da3669e7df20 344
mtag 1:c5b58b10970d 345 int sensorData = 0x00 & ((lineDetected1 << 2) + (lineDetected2 << 1) + (lineDetected3));
mtag 1:c5b58b10970d 346 sensorData = sensorData & 0x07
mtag 0:da3669e7df20 347
mtag 0:da3669e7df20 348 switch(sensorData) {
mtag 0:da3669e7df20 349
mtag 1:c5b58b10970d 350 //000
mtag 1:c5b58b10970d 351 case 0x0
mtag 0:da3669e7df20 352 direction = STOP;
mtag 0:da3669e7df20 353 break;
mtag 0:da3669e7df20 354
mtag 1:c5b58b10970d 355 //001
mtag 1:c5b58b10970d 356 case 0x1:
mtag 0:da3669e7df20 357 direction = RIGHT;
mtag 0:da3669e7df20 358 break;
mtag 0:da3669e7df20 359
mtag 1:c5b58b10970d 360 //010
mtag 1:c5b58b10970d 361 case 0x2:
mtag 0:da3669e7df20 362 direction = FORWARD;
mtag 0:da3669e7df20 363 break;
mtag 0:da3669e7df20 364
mtag 1:c5b58b10970d 365 //011
mtag 1:c5b58b10970d 366 case 0x3:
mtag 0:da3669e7df20 367 direction = RIGHT;
mtag 0:da3669e7df20 368 break;
mtag 0:da3669e7df20 369
mtag 1:c5b58b10970d 370 //100
mtag 1:c5b58b10970d 371 case 0x4:
mtag 0:da3669e7df20 372 direction = LEFT;
mtag 0:da3669e7df20 373 break;
mtag 0:da3669e7df20 374
mtag 1:c5b58b10970d 375 //101
mtag 1:c5b58b10970d 376 case 0x5:
mtag 0:da3669e7df20 377 if(red_path) {
mtag 0:da3669e7df20 378 direction = LEFT;
mtag 0:da3669e7df20 379 }
mtag 0:da3669e7df20 380
mtag 0:da3669e7df20 381 if(blue_path) {
mtag 0:da3669e7df20 382 direction = RIGHT;
mtag 0:da3669e7df20 383 }
mtag 0:da3669e7df20 384
mtag 0:da3669e7df20 385 break;
mtag 1:c5b58b10970d 386
mtag 1:c5b58b10970d 387 //110
mtag 1:c5b58b10970d 388 case 0x06:
mtag 1:c5b58b10970d 389 direction = RIGHT;
mtag 1:c5b58b10970d 390 break;
mtag 1:c5b58b10970d 391
mtag 1:c5b58b10970d 392 //111
mtag 1:c5b58b10970d 393 case 0x7:
mtag 0:da3669e7df20 394 direction = FORWARD;
mtag 0:da3669e7df20 395 break;
mtag 0:da3669e7df20 396
mtag 0:da3669e7df20 397 default:
mtag 0:da3669e7df20 398 direction = FORWARD;
mtag 0:da3669e7df20 399 break;
mtag 0:da3669e7df20 400 }
mtag 0:da3669e7df20 401 return direction;
mtag 0:da3669e7df20 402 }
mtag 0:da3669e7df20 403
mtag 0:da3669e7df20 404 int main() {
mtag 0:da3669e7df20 405
mtag 0:da3669e7df20 406 //Blink LED to let you know it's on
mtag 0:da3669e7df20 407 myled = 0;
mtag 0:da3669e7df20 408 wait(0.5);
mtag 0:da3669e7df20 409 myled = 1;
mtag 0:da3669e7df20 410 wait(0.5);
mtag 0:da3669e7df20 411 myled = 0;
mtag 0:da3669e7df20 412
mtag 0:da3669e7df20 413 bool paper_detected = false;
mtag 0:da3669e7df20 414
mtag 0:da3669e7df20 415 MotorController motorController;
mtag 0:da3669e7df20 416 SolenoidController solenoidController;
mtag 0:da3669e7df20 417 LineFollower lineFollower;
mtag 0:da3669e7df20 418 ColourSensor colourSensor;
mtag 0:da3669e7df20 419
mtag 0:da3669e7df20 420 motorController.initialize();
mtag 0:da3669e7df20 421 lineFollower.initialize();
mtag 0:da3669e7df20 422 colourSensor.initialize();
mtag 0:da3669e7df20 423 solenoidController.off();
mtag 0:da3669e7df20 424
mtag 0:da3669e7df20 425 motorController.setSpeed(200);
mtag 0:da3669e7df20 426 motorController.goForward();
mtag 0:da3669e7df20 427
mtag 0:da3669e7df20 428
mtag 0:da3669e7df20 429 while(true) {
mtag 0:da3669e7df20 430
mtag 0:da3669e7df20 431 lineFollower.readSensors();
mtag 0:da3669e7df20 432 motorController.changeDirection(lineFollower.chooseDirection());
mtag 0:da3669e7df20 433
mtag 0:da3669e7df20 434 colourSensor.read();
mtag 0:da3669e7df20 435
mtag 0:da3669e7df20 436 //Logic for the solenoid based on colour detected
mtag 0:da3669e7df20 437
mtag 0:da3669e7df20 438 //Detect the first sheet of paper if blue and pick up the disc
mtag 0:da3669e7df20 439 if(colourSensor.blue_detected && !paper_detected && !solenoidController.state) {
mtag 0:da3669e7df20 440 paper_detected = true;
mtag 0:da3669e7df20 441 blue_path = true;
mtag 0:da3669e7df20 442 solenoidController.on();
mtag 0:da3669e7df20 443 }
mtag 0:da3669e7df20 444
mtag 0:da3669e7df20 445 //Detect the first sheet of paper if red and pick up the disc
mtag 0:da3669e7df20 446 if(colourSensor.red_detected && !paper_detected && !solenoidController.state) {
mtag 0:da3669e7df20 447 paper_detected = true;
mtag 0:da3669e7df20 448 red_path = true;
mtag 0:da3669e7df20 449 solenoidController.on();
mtag 0:da3669e7df20 450 }
mtag 0:da3669e7df20 451
mtag 0:da3669e7df20 452 //Detect the end of the first sheet of paper
mtag 0:da3669e7df20 453 if(!colourSensor.blue_detected && !colourSensor.red_detected && paper_detected) {
mtag 0:da3669e7df20 454 paper_detected = false;
mtag 0:da3669e7df20 455 }
mtag 0:da3669e7df20 456
mtag 1:c5b58b10970d 457 //Drop the disc once the second blue paper is detected
mtag 0:da3669e7df20 458 if(colourSensor.blue_detected && blue_path && !paper_detected && solenoidController.state) {
mtag 0:da3669e7df20 459 paper_detected = true;
mtag 0:da3669e7df20 460 solenoidController.off();
mtag 0:da3669e7df20 461 }
mtag 0:da3669e7df20 462
mtag 1:c5b58b10970d 463 //Drop the disc once the second red paper is detected
mtag 0:da3669e7df20 464 if(colourSensor.red_detected && red_path && !paper_detected && solenoidController.state) {
mtag 0:da3669e7df20 465 paper_detected = true;
mtag 0:da3669e7df20 466 solenoidController.off();
mtag 0:da3669e7df20 467 }
mtag 0:da3669e7df20 468
mtag 0:da3669e7df20 469 }
mtag 0:da3669e7df20 470
mtag 0:da3669e7df20 471 }