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@7:1e5fa5952695, 2019-03-05 (annotated)

Committer:
mtag
Date:
Tue Mar 05 13:58:37 2019 +0000
Revision:
7:1e5fa5952695
Parent:
6:1056ed1d6d97
Child:
8:88e72c6deac9
A LITTLE IMPROVEMENT;

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