Yee Aung / Mbed 2 deprecated 4180_FINAL_PROJECTUWv3

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 Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library

main.cpp@13:dd2ac39ba5ba, 2018-12-12 (annotated)

Committer:
Arkantos1695
Date:
Wed Dec 12 16:13:08 2018 +0000
Revision:
13:dd2ac39ba5ba
Parent:
12:f57a51ec8399 
Random Path Final Version;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cbrice6 0:9e014841f2b7 1 #include "mbed.h"
cbrice6 0:9e014841f2b7 2 #include "LSM9DS1.h" // IMU
cbrice6 0:9e014841f2b7 3 #include "ultrasonic.h" // Ultrasonic Sensor
cbrice6 0:9e014841f2b7 4 #include "Motor.h" // Motor Drivers
cbrice6 3:c07ea8bf242e 5 #include "PinDetect.h" // Pin Detect (for switch)
cbrice6 0:9e014841f2b7 6
cbrice6 0:9e014841f2b7 7
cbrice6 1:92f6242c0196 8 // Global Variables:
cbrice6 3:c07ea8bf242e 9 bool tr = true;
cbrice6 1:92f6242c0196 10 bool turnflag = false;
Arkantos1695 13:dd2ac39ba5ba 11
Arkantos1695 13:dd2ac39ba5ba 12 int leftDist = 0;
Arkantos1695 13:dd2ac39ba5ba 13 int rightDist = 0;
Arkantos1695 13:dd2ac39ba5ba 14 int forwardDist = 0;
Arkantos1695 13:dd2ac39ba5ba 15 int leftForwardDist = 0;
Arkantos1695 13:dd2ac39ba5ba 16 int rightForwardDist = 0;
Arkantos1695 13:dd2ac39ba5ba 17
cbrice6 0:9e014841f2b7 18 //---------------------|
cbrice6 0:9e014841f2b7 19 // PIN INITIALIZATIONS |
cbrice6 0:9e014841f2b7 20 //---------------------|
cbrice6 3:c07ea8bf242e 21 // Debug LED pin:
cbrice6 3:c07ea8bf242e 22 //DigitalOut led(p25);
cbrice6 6:d25157f50f14 23 DigitalIn sw(p20);
cbrice6 6:d25157f50f14 24 Serial pc(USBTX,USBRX);
cbrice6 0:9e014841f2b7 25 // Setup Motor Driver pins:
uwang3 11:f95294698901 26 Motor Lfront(p21, p6, p5); // PWM to p21, forward to p5, reverse to p6...
uwang3 11:f95294698901 27 Motor Rfront(p22, p8, p7); // PWM to p22, forward to p7, reverse to p8...
cbrice6 1:92f6242c0196 28 Motor Lback(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9...
cbrice6 0:9e014841f2b7 29 Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11...
Arkantos1695 10:2a1c8ce9d76c 30 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
cbrice6 0:9e014841f2b7 31
cbrice6 0:9e014841f2b7 32 Timer t;
cbrice6 0:9e014841f2b7 33
cbrice6 0:9e014841f2b7 34 //-----------------------|
cbrice6 0:9e014841f2b7 35 // CLASS INITIALIZATIONS |
cbrice6 0:9e014841f2b7 36 //-----------------------|
cbrice6 0:9e014841f2b7 37 class mctrl {
cbrice6 0:9e014841f2b7 38 public:
cbrice6 1:92f6242c0196 39 // Stop all motors:
cbrice6 1:92f6242c0196 40 void stop(void) {
cbrice6 1:92f6242c0196 41 Lfront.speed(0);
cbrice6 1:92f6242c0196 42 Lback.speed(0);
cbrice6 1:92f6242c0196 43 Rfront.speed(0);
cbrice6 1:92f6242c0196 44 Rback.speed(0);
Arkantos1695 7:5667ce16d526 45 wait(0.55);
cbrice6 1:92f6242c0196 46 };
cbrice6 3:c07ea8bf242e 47
cbrice6 1:92f6242c0196 48 // Go forward at constant speed:
cbrice6 1:92f6242c0196 49 void fwd(void){
Arkantos1695 7:5667ce16d526 50 //stop();
cbrice6 1:92f6242c0196 51
uwang3 11:f95294698901 52 Lfront.speed(0.85);
uwang3 11:f95294698901 53 Lback.speed(0.85);
uwang3 11:f95294698901 54 Rfront.speed(0.85);
uwang3 11:f95294698901 55 Rback.speed(0.85);
cbrice6 1:92f6242c0196 56 wait(0.02);
cbrice6 1:92f6242c0196 57 };
cbrice6 3:c07ea8bf242e 58
cbrice6 1:92f6242c0196 59 // Reverse at constant speed:
cbrice6 1:92f6242c0196 60 void rev(void){
cbrice6 1:92f6242c0196 61 stop();
cbrice6 1:92f6242c0196 62 Lfront.speed(-1);
cbrice6 1:92f6242c0196 63 Lback.speed(-1);
cbrice6 1:92f6242c0196 64 Rfront.speed(-1);
cbrice6 1:92f6242c0196 65 Rback.speed(-1);
Arkantos1695 13:dd2ac39ba5ba 66 wait(0.25);
cbrice6 1:92f6242c0196 67 };
cbrice6 3:c07ea8bf242e 68
cbrice6 1:92f6242c0196 69 // Turn left 90 degrees:
uwang3 11:f95294698901 70 void turnLeft(){
cbrice6 1:92f6242c0196 71 stop();
uwang3 11:f95294698901 72 float degree = 0.0, angularV = 0.0;
uwang3 11:f95294698901 73 float currt = t.read();
uwang3 11:f95294698901 74 while(degree < 90) {
uwang3 11:f95294698901 75 while(imu.gyroAvailable());
uwang3 11:f95294698901 76 imu.readGyro();
uwang3 11:f95294698901 77 angularV = imu.gz;
Arkantos1695 13:dd2ac39ba5ba 78 Lfront.speed(1);
Arkantos1695 13:dd2ac39ba5ba 79 Lback.speed(1);
Arkantos1695 13:dd2ac39ba5ba 80 Rfront.speed(-1);
Arkantos1695 13:dd2ac39ba5ba 81 Rback.speed(-1);
Arkantos1695 13:dd2ac39ba5ba 82 if(angularV > 50.0 || angularV <-50.0)
Arkantos1695 13:dd2ac39ba5ba 83 {
uwang3 11:f95294698901 84 degree += (abs(angularV))/100.00;
uwang3 11:f95294698901 85 }
Arkantos1695 13:dd2ac39ba5ba 86 wait(.4);
cbrice6 1:92f6242c0196 87 }
cbrice6 1:92f6242c0196 88 stop();
uwang3 11:f95294698901 89 wait(0.02);
cbrice6 1:92f6242c0196 90 };
cbrice6 3:c07ea8bf242e 91
cbrice6 1:92f6242c0196 92 // Turn right 90 degrees:
Arkantos1695 10:2a1c8ce9d76c 93 void turnRight(){
cbrice6 1:92f6242c0196 94 stop();
Arkantos1695 10:2a1c8ce9d76c 95 float degree = 0.0, angularV = 0.0;
Arkantos1695 10:2a1c8ce9d76c 96 float currt = t.read();
Arkantos1695 10:2a1c8ce9d76c 97 while(degree < 90) {
Arkantos1695 10:2a1c8ce9d76c 98 while(imu.gyroAvailable());
Arkantos1695 10:2a1c8ce9d76c 99 imu.readGyro();
Arkantos1695 10:2a1c8ce9d76c 100 angularV = imu.gz;
Arkantos1695 10:2a1c8ce9d76c 101 Lfront.speed(1);
Arkantos1695 10:2a1c8ce9d76c 102 Lback.speed(1);
Arkantos1695 10:2a1c8ce9d76c 103 Rfront.speed(-1);
Arkantos1695 10:2a1c8ce9d76c 104 Rback.speed(-1);
Arkantos1695 10:2a1c8ce9d76c 105 if(angularV > 50.0 || angularV <-50.0) {
Arkantos1695 10:2a1c8ce9d76c 106 degree += (abs(angularV))/100.00;
Arkantos1695 10:2a1c8ce9d76c 107 }
Arkantos1695 13:dd2ac39ba5ba 108 wait(.4);
cbrice6 1:92f6242c0196 109 }
cbrice6 1:92f6242c0196 110 stop();
Arkantos1695 10:2a1c8ce9d76c 111 wait(0.02);
cbrice6 1:92f6242c0196 112 };
Arkantos1695 9:ec0ceec8f5f5 113
uwang3 11:f95294698901 114 // Turn L or R, "mag" of turn prop. to dist. from wall
uwang3 12:f57a51ec8399 115 // motor speed values are normalized to -1 to 1 (aka no 1.25)
uwang3 11:f95294698901 116 void turnLeftScaled(double scale) {
uwang3 12:f57a51ec8399 117 // try commenting out this stop otherwise robot will stop before correcting
uwang3 11:f95294698901 118 stop();
uwang3 11:f95294698901 119 Lfront.speed(-1 * scale);
uwang3 11:f95294698901 120 Lback.speed(-1 * scale);
uwang3 11:f95294698901 121 Rfront.speed(1 * scale);
uwang3 11:f95294698901 122 Rback.speed(1 * scale);
uwang3 11:f95294698901 123 //stop();
uwang3 11:f95294698901 124 wait(0.02);
uwang3 11:f95294698901 125 };
Arkantos1695 9:ec0ceec8f5f5 126
uwang3 11:f95294698901 127 void turnRightScaled(double scale) {
uwang3 11:f95294698901 128 stop();
uwang3 11:f95294698901 129 Lfront.speed(1 * scale);
uwang3 11:f95294698901 130 Lback.speed(1 * scale);
uwang3 11:f95294698901 131 Rfront.speed(-1 * scale);
uwang3 11:f95294698901 132 Rback.speed(-1 * scale);
uwang3 11:f95294698901 133 //stop();
uwang3 11:f95294698901 134 wait(0.02);
uwang3 11:f95294698901 135 };
cbrice6 1:92f6242c0196 136 } mctrl;
cbrice6 0:9e014841f2b7 137
cbrice6 0:9e014841f2b7 138 //------------------|
cbrice6 0:9e014841f2b7 139 // HELPER FUNCTIONS |
cbrice6 0:9e014841f2b7 140 //------------------|
uwang3 11:f95294698901 141 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
cbrice6 0:9e014841f2b7 142 void dist(int distance) // NOTE: by default "distance" is in mm...
cbrice6 0:9e014841f2b7 143 {
Arkantos1695 13:dd2ac39ba5ba 144 if (distance < 200) {
Arkantos1695 9:ec0ceec8f5f5 145
Arkantos1695 9:ec0ceec8f5f5 146 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 147
Arkantos1695 13:dd2ac39ba5ba 148 if ( leftDist < rightDist)
Arkantos1695 13:dd2ac39ba5ba 149 {
uwang3 11:f95294698901 150 mctrl.turnRight();
Arkantos1695 13:dd2ac39ba5ba 151 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 152 //mctrl.fwd();
Arkantos1695 13:dd2ac39ba5ba 153
uwang3 11:f95294698901 154 }
uwang3 11:f95294698901 155
Arkantos1695 13:dd2ac39ba5ba 156 else
Arkantos1695 13:dd2ac39ba5ba 157 {
Arkantos1695 13:dd2ac39ba5ba 158 mctrl.turnLeft();
Arkantos1695 13:dd2ac39ba5ba 159 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 160 //mctrl.fwd();
uwang3 11:f95294698901 161 }
Arkantos1695 13:dd2ac39ba5ba 162 }
Arkantos1695 13:dd2ac39ba5ba 163
Arkantos1695 13:dd2ac39ba5ba 164 else {
Arkantos1695 13:dd2ac39ba5ba 165 mctrl.fwd();
Arkantos1695 13:dd2ac39ba5ba 166 }
Arkantos1695 13:dd2ac39ba5ba 167 };
Arkantos1695 13:dd2ac39ba5ba 168
Arkantos1695 13:dd2ac39ba5ba 169 void dist2(int distance) // left sensor interrupt
Arkantos1695 13:dd2ac39ba5ba 170 {
Arkantos1695 13:dd2ac39ba5ba 171 if (distance < 150)
Arkantos1695 13:dd2ac39ba5ba 172 {
Arkantos1695 13:dd2ac39ba5ba 173 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 174 mctrl.turnRight();
Arkantos1695 13:dd2ac39ba5ba 175 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 176 }
Arkantos1695 13:dd2ac39ba5ba 177
Arkantos1695 13:dd2ac39ba5ba 178 else
Arkantos1695 13:dd2ac39ba5ba 179 {
Arkantos1695 13:dd2ac39ba5ba 180
Arkantos1695 13:dd2ac39ba5ba 181 mctrl.fwd();
Arkantos1695 13:dd2ac39ba5ba 182 }
Arkantos1695 13:dd2ac39ba5ba 183
Arkantos1695 13:dd2ac39ba5ba 184 }
uwang3 11:f95294698901 185
Arkantos1695 13:dd2ac39ba5ba 186 void dist3(int distance) // right sensor interrupt
Arkantos1695 13:dd2ac39ba5ba 187 {
Arkantos1695 13:dd2ac39ba5ba 188 if (distance < 150)
Arkantos1695 13:dd2ac39ba5ba 189 {
Arkantos1695 13:dd2ac39ba5ba 190 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 191 mctrl.turnLeft();
Arkantos1695 13:dd2ac39ba5ba 192 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 193 }
Arkantos1695 13:dd2ac39ba5ba 194
Arkantos1695 13:dd2ac39ba5ba 195 else
Arkantos1695 13:dd2ac39ba5ba 196 {
Arkantos1695 13:dd2ac39ba5ba 197 mctrl.fwd();
Arkantos1695 13:dd2ac39ba5ba 198 }
Arkantos1695 13:dd2ac39ba5ba 199
Arkantos1695 13:dd2ac39ba5ba 200 }
Arkantos1695 13:dd2ac39ba5ba 201
Arkantos1695 13:dd2ac39ba5ba 202 void dist4(int distance) {
Arkantos1695 13:dd2ac39ba5ba 203 if (distance < 130) {
Arkantos1695 13:dd2ac39ba5ba 204 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 205 mctrl.turnRight();
Arkantos1695 13:dd2ac39ba5ba 206 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 207 }
Arkantos1695 13:dd2ac39ba5ba 208
Arkantos1695 13:dd2ac39ba5ba 209 else {
Arkantos1695 13:dd2ac39ba5ba 210 mctrl.fwd();
cbrice6 0:9e014841f2b7 211 }
cbrice6 0:9e014841f2b7 212 }
cbrice6 0:9e014841f2b7 213
Arkantos1695 13:dd2ac39ba5ba 214 void dist5(int distance) {
Arkantos1695 13:dd2ac39ba5ba 215 if (distance < 130) {
Arkantos1695 13:dd2ac39ba5ba 216 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 217 mctrl.turnLeft();
Arkantos1695 13:dd2ac39ba5ba 218 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 219 } else {
Arkantos1695 13:dd2ac39ba5ba 220
Arkantos1695 13:dd2ac39ba5ba 221 mctrl.fwd();
Arkantos1695 8:2d0fc244cc65 222 }
Arkantos1695 8:2d0fc244cc65 223 }
Arkantos1695 8:2d0fc244cc65 224
cbrice6 0:9e014841f2b7 225 //------------------------------|
cbrice6 0:9e014841f2b7 226 // PIN INITIALIZATIONS (cont'd) |
cbrice6 0:9e014841f2b7 227 //------------------------------|
cbrice6 0:9e014841f2b7 228 // Setup Ultrasonic Sensor pins:
Arkantos1695 10:2a1c8ce9d76c 229 ultrasonic usensor(p15, p16, .07, 1, &dist); // trigger to p13, echo to p14...
Arkantos1695 8:2d0fc244cc65 230 // update every .07 secs w/ timeout after 1 sec...
cbrice6 0:9e014841f2b7 231 // call "dist" when the distance changes...
Arkantos1695 10:2a1c8ce9d76c 232 ultrasonic usensor2(p15, p26, .07, 1, &dist2); // left sensor trigger
Arkantos1695 10:2a1c8ce9d76c 233 ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
Arkantos1695 10:2a1c8ce9d76c 234
Arkantos1695 13:dd2ac39ba5ba 235 // new sonars added
Arkantos1695 13:dd2ac39ba5ba 236 ultrasonic usensor4(p15, p29, .07, 1, &dist4); //sonar to the left of the forward sensor
Arkantos1695 13:dd2ac39ba5ba 237 ultrasonic usensor5(p15, p30, .07, 1, &dist5); //sonar to the right of the forward sensor
Arkantos1695 13:dd2ac39ba5ba 238
cbrice6 0:9e014841f2b7 239
cbrice6 0:9e014841f2b7 240 //---------------|
cbrice6 0:9e014841f2b7 241 // MAIN FUNCTION |
cbrice6 0:9e014841f2b7 242 //---------------|
cbrice6 0:9e014841f2b7 243 int main() {
cbrice6 3:c07ea8bf242e 244 // Use internal pullup for the switch:
cbrice6 6:d25157f50f14 245 sw.mode(PullUp);
uwang3 11:f95294698901 246
cbrice6 0:9e014841f2b7 247 // Initialize the Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 248 usensor.startUpdates();
Arkantos1695 8:2d0fc244cc65 249 usensor2.startUpdates();
Arkantos1695 8:2d0fc244cc65 250 usensor3.startUpdates();
Arkantos1695 13:dd2ac39ba5ba 251 usensor4.startUpdates();
Arkantos1695 13:dd2ac39ba5ba 252 usensor5.startUpdates();
Arkantos1695 9:ec0ceec8f5f5 253 wait(0.5);
uwang3 11:f95294698901 254
uwang3 11:f95294698901 255 // obtain and print starting distances
Arkantos1695 13:dd2ac39ba5ba 256 forwardDist = usensor.getCurrentDistance();
Arkantos1695 13:dd2ac39ba5ba 257 leftDist = usensor2.getCurrentDistance();
Arkantos1695 13:dd2ac39ba5ba 258 rightDist = usensor3.getCurrentDistance();
Arkantos1695 13:dd2ac39ba5ba 259 leftForwardDist = usensor4.getCurrentDistance();
Arkantos1695 13:dd2ac39ba5ba 260 rightForwardDist = usensor5.getCurrentDistance();
Arkantos1695 9:ec0ceec8f5f5 261 wait(0.5);
Arkantos1695 9:ec0ceec8f5f5 262
cbrice6 0:9e014841f2b7 263 // Initialize the Timer:
cbrice6 0:9e014841f2b7 264 t.start();
Arkantos1695 13:dd2ac39ba5ba 265 int starttime = t.read();
Arkantos1695 9:ec0ceec8f5f5 266
Arkantos1695 13:dd2ac39ba5ba 267 while(1) {
Arkantos1695 13:dd2ac39ba5ba 268 while (!sw && t.read() - starttime < 60) {
uwang3 11:f95294698901 269
Arkantos1695 13:dd2ac39ba5ba 270
uwang3 11:f95294698901 271
Arkantos1695 13:dd2ac39ba5ba 272 usensor.checkDistance();
Arkantos1695 13:dd2ac39ba5ba 273 usensor2.checkDistance();
Arkantos1695 13:dd2ac39ba5ba 274 usensor3.checkDistance();
Arkantos1695 13:dd2ac39ba5ba 275 usensor4.checkDistance();
Arkantos1695 13:dd2ac39ba5ba 276 usensor5.checkDistance();
Arkantos1695 13:dd2ac39ba5ba 277 forwardDist = usensor.getCurrentDistance();
Arkantos1695 13:dd2ac39ba5ba 278 leftDist = usensor2.getCurrentDistance();
Arkantos1695 13:dd2ac39ba5ba 279 rightDist = usensor3.getCurrentDistance();
Arkantos1695 13:dd2ac39ba5ba 280 leftForwardDist = usensor4.getCurrentDistance();
Arkantos1695 13:dd2ac39ba5ba 281 rightForwardDist = usensor5.getCurrentDistance();
Arkantos1695 13:dd2ac39ba5ba 282
Arkantos1695 13:dd2ac39ba5ba 283 if ( leftForwardDist > 100 && rightForwardDist > 100 )
Arkantos1695 13:dd2ac39ba5ba 284 {
Arkantos1695 13:dd2ac39ba5ba 285 mctrl.fwd();
Arkantos1695 13:dd2ac39ba5ba 286 }
Arkantos1695 13:dd2ac39ba5ba 287
Arkantos1695 13:dd2ac39ba5ba 288 else{
Arkantos1695 13:dd2ac39ba5ba 289
Arkantos1695 13:dd2ac39ba5ba 290 mctrl.rev();
Arkantos1695 13:dd2ac39ba5ba 291 mctrl.turnRight();
Arkantos1695 13:dd2ac39ba5ba 292 mctrl.turnRight();
Arkantos1695 13:dd2ac39ba5ba 293 mctrl.fwd();
Arkantos1695 13:dd2ac39ba5ba 294
Arkantos1695 13:dd2ac39ba5ba 295 }
Arkantos1695 13:dd2ac39ba5ba 296 //rightForwardDist = usensor5.getCurrentDistance();
Arkantos1695 13:dd2ac39ba5ba 297 /*
Arkantos1695 13:dd2ac39ba5ba 298 pc.printf("fowardDist: %d\n ", usensor.getCurrentDistance());
Arkantos1695 13:dd2ac39ba5ba 299 wait (1);
Arkantos1695 13:dd2ac39ba5ba 300 pc.printf("LeftDist: %d\n", usensor2.getCurrentDistance());
Arkantos1695 13:dd2ac39ba5ba 301 wait(1);
Arkantos1695 13:dd2ac39ba5ba 302 pc.printf("RightDist: %d\n", usensor3.getCurrentDistance());
Arkantos1695 13:dd2ac39ba5ba 303 wait(1);
Arkantos1695 13:dd2ac39ba5ba 304 pc.printf("leftForwardDist: %d\n ", usensor4.getCurrentDistance());
Arkantos1695 13:dd2ac39ba5ba 305 wait (1);
Arkantos1695 13:dd2ac39ba5ba 306 pc.printf("rightForwardDist: %d\n", usensor5.getCurrentDistance());
Arkantos1695 13:dd2ac39ba5ba 307 wait(1);*/
Arkantos1695 13:dd2ac39ba5ba 308
Arkantos1695 13:dd2ac39ba5ba 309
Arkantos1695 13:dd2ac39ba5ba 310
Arkantos1695 13:dd2ac39ba5ba 311
Arkantos1695 9:ec0ceec8f5f5 312 }
uwang3 12:f57a51ec8399 313 // if switch flipped, stop robot
Arkantos1695 9:ec0ceec8f5f5 314 mctrl.stop();
Arkantos1695 13:dd2ac39ba5ba 315 }
Arkantos1695 13:dd2ac39ba5ba 316
cbrice6 0:9e014841f2b7 317 }