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@8:88e72c6deac9, 2019-03-14 (annotated)

Committer:
mtag
Date:
Thu Mar 14 14:11:58 2019 +0000
Revision:
8:88e72c6deac9
Parent:
7:1e5fa5952695
Child:
9:7d74c22ed54e
About to test this

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