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@12:f57a51ec8399, 2018-12-10 (annotated)

Committer:
uwang3
Date:
Mon Dec 10 17:57:37 2018 +0000
Revision:
12:f57a51ec8399
Parent:
11:f95294698901
Child:
13:dd2ac39ba5ba
Added more in detail comments; Added some additional logic in path correction sections;

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 9:ec0ceec8f5f5 11 int leftdistancestarting= 0;
Arkantos1695 9:ec0ceec8f5f5 12 int leftdistanceending= 0;
cbrice6 0:9e014841f2b7 13 //---------------------|
cbrice6 0:9e014841f2b7 14 // PIN INITIALIZATIONS |
cbrice6 0:9e014841f2b7 15 //---------------------|
cbrice6 3:c07ea8bf242e 16 // Debug LED pin:
cbrice6 3:c07ea8bf242e 17 //DigitalOut led(p25);
cbrice6 6:d25157f50f14 18 DigitalIn sw(p20);
cbrice6 6:d25157f50f14 19 Serial pc(USBTX,USBRX);
cbrice6 0:9e014841f2b7 20 // Setup Motor Driver pins:
uwang3 11:f95294698901 21 Motor Lfront(p21, p6, p5); // PWM to p21, forward to p5, reverse to p6...
uwang3 11:f95294698901 22 Motor Rfront(p22, p8, p7); // PWM to p22, forward to p7, reverse to p8...
cbrice6 1:92f6242c0196 23 Motor Lback(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9...
cbrice6 0:9e014841f2b7 24 Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11...
Arkantos1695 10:2a1c8ce9d76c 25 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
cbrice6 0:9e014841f2b7 26
cbrice6 0:9e014841f2b7 27 Timer t;
cbrice6 0:9e014841f2b7 28
cbrice6 0:9e014841f2b7 29 //-----------------------|
cbrice6 0:9e014841f2b7 30 // CLASS INITIALIZATIONS |
cbrice6 0:9e014841f2b7 31 //-----------------------|
cbrice6 0:9e014841f2b7 32 class mctrl {
cbrice6 0:9e014841f2b7 33 public:
cbrice6 1:92f6242c0196 34 // Stop all motors:
cbrice6 1:92f6242c0196 35 void stop(void) {
cbrice6 1:92f6242c0196 36 Lfront.speed(0);
cbrice6 1:92f6242c0196 37 Lback.speed(0);
cbrice6 1:92f6242c0196 38 Rfront.speed(0);
cbrice6 1:92f6242c0196 39 Rback.speed(0);
Arkantos1695 7:5667ce16d526 40 wait(0.55);
cbrice6 1:92f6242c0196 41 };
cbrice6 3:c07ea8bf242e 42
cbrice6 1:92f6242c0196 43 // Go forward at constant speed:
cbrice6 1:92f6242c0196 44 void fwd(void){
Arkantos1695 7:5667ce16d526 45 //stop();
cbrice6 1:92f6242c0196 46
uwang3 11:f95294698901 47 Lfront.speed(0.85);
uwang3 11:f95294698901 48 Lback.speed(0.85);
uwang3 11:f95294698901 49 Rfront.speed(0.85);
uwang3 11:f95294698901 50 Rback.speed(0.85);
cbrice6 1:92f6242c0196 51 wait(0.02);
cbrice6 1:92f6242c0196 52 };
cbrice6 3:c07ea8bf242e 53
cbrice6 1:92f6242c0196 54 // Reverse at constant speed:
cbrice6 1:92f6242c0196 55 void rev(void){
cbrice6 1:92f6242c0196 56 stop();
cbrice6 1:92f6242c0196 57 Lfront.speed(-1);
cbrice6 1:92f6242c0196 58 Lback.speed(-1);
cbrice6 1:92f6242c0196 59 Rfront.speed(-1);
cbrice6 1:92f6242c0196 60 Rback.speed(-1);
cbrice6 1:92f6242c0196 61 wait(0.02);
cbrice6 1:92f6242c0196 62 };
cbrice6 3:c07ea8bf242e 63
cbrice6 1:92f6242c0196 64 // Turn left 90 degrees:
uwang3 11:f95294698901 65 void turnLeft(){
cbrice6 1:92f6242c0196 66 stop();
uwang3 11:f95294698901 67 float degree = 0.0, angularV = 0.0;
uwang3 11:f95294698901 68 float currt = t.read();
uwang3 11:f95294698901 69 while(degree < 90) {
uwang3 11:f95294698901 70 while(imu.gyroAvailable());
uwang3 11:f95294698901 71 imu.readGyro();
uwang3 11:f95294698901 72 angularV = imu.gz;
cbrice6 1:92f6242c0196 73 Lfront.speed(-1);
cbrice6 1:92f6242c0196 74 Lback.speed(-1);
cbrice6 1:92f6242c0196 75 Rfront.speed(1);
cbrice6 1:92f6242c0196 76 Rback.speed(1);
uwang3 11:f95294698901 77 if(angularV > 50.0 || angularV <-50.0) {
uwang3 11:f95294698901 78 degree += (abs(angularV))/100.00;
uwang3 11:f95294698901 79 }
uwang3 11:f95294698901 80 wait(.45);
cbrice6 1:92f6242c0196 81 }
cbrice6 1:92f6242c0196 82 stop();
uwang3 11:f95294698901 83 wait(0.02);
cbrice6 1:92f6242c0196 84 };
cbrice6 3:c07ea8bf242e 85
cbrice6 1:92f6242c0196 86 // Turn right 90 degrees:
Arkantos1695 10:2a1c8ce9d76c 87 void turnRight(){
cbrice6 1:92f6242c0196 88 stop();
Arkantos1695 10:2a1c8ce9d76c 89 float degree = 0.0, angularV = 0.0;
Arkantos1695 10:2a1c8ce9d76c 90 float currt = t.read();
Arkantos1695 10:2a1c8ce9d76c 91 while(degree < 90) {
Arkantos1695 10:2a1c8ce9d76c 92 while(imu.gyroAvailable());
Arkantos1695 10:2a1c8ce9d76c 93 imu.readGyro();
Arkantos1695 10:2a1c8ce9d76c 94 angularV = imu.gz;
Arkantos1695 10:2a1c8ce9d76c 95 Lfront.speed(1);
Arkantos1695 10:2a1c8ce9d76c 96 Lback.speed(1);
Arkantos1695 10:2a1c8ce9d76c 97 Rfront.speed(-1);
Arkantos1695 10:2a1c8ce9d76c 98 Rback.speed(-1);
Arkantos1695 10:2a1c8ce9d76c 99 if(angularV > 50.0 || angularV <-50.0) {
Arkantos1695 10:2a1c8ce9d76c 100 degree += (abs(angularV))/100.00;
Arkantos1695 10:2a1c8ce9d76c 101 }
uwang3 11:f95294698901 102 wait(.45);
cbrice6 1:92f6242c0196 103 }
cbrice6 1:92f6242c0196 104 stop();
Arkantos1695 10:2a1c8ce9d76c 105 wait(0.02);
cbrice6 1:92f6242c0196 106 };
Arkantos1695 9:ec0ceec8f5f5 107
uwang3 11:f95294698901 108 // Turn L or R, "mag" of turn prop. to dist. from wall
uwang3 12:f57a51ec8399 109 // motor speed values are normalized to -1 to 1 (aka no 1.25)
uwang3 11:f95294698901 110 void turnLeftScaled(double scale) {
uwang3 12:f57a51ec8399 111 // try commenting out this stop otherwise robot will stop before correcting
uwang3 11:f95294698901 112 stop();
uwang3 11:f95294698901 113 Lfront.speed(-1 * scale);
uwang3 11:f95294698901 114 Lback.speed(-1 * scale);
uwang3 11:f95294698901 115 Rfront.speed(1 * scale);
uwang3 11:f95294698901 116 Rback.speed(1 * scale);
uwang3 11:f95294698901 117 //stop();
uwang3 11:f95294698901 118 wait(0.02);
uwang3 11:f95294698901 119 };
Arkantos1695 9:ec0ceec8f5f5 120
uwang3 11:f95294698901 121 void turnRightScaled(double scale) {
uwang3 11:f95294698901 122 stop();
uwang3 11:f95294698901 123 Lfront.speed(1 * scale);
uwang3 11:f95294698901 124 Lback.speed(1 * scale);
uwang3 11:f95294698901 125 Rfront.speed(-1 * scale);
uwang3 11:f95294698901 126 Rback.speed(-1 * scale);
uwang3 11:f95294698901 127 //stop();
uwang3 11:f95294698901 128 wait(0.02);
uwang3 11:f95294698901 129 };
cbrice6 1:92f6242c0196 130 } mctrl;
cbrice6 0:9e014841f2b7 131
cbrice6 0:9e014841f2b7 132 //------------------|
cbrice6 0:9e014841f2b7 133 // HELPER FUNCTIONS |
cbrice6 0:9e014841f2b7 134 //------------------|
uwang3 11:f95294698901 135 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
cbrice6 0:9e014841f2b7 136 void dist(int distance) // NOTE: by default "distance" is in mm...
cbrice6 0:9e014841f2b7 137 {
cbrice6 3:c07ea8bf242e 138 if (distance < 150) {
Arkantos1695 9:ec0ceec8f5f5 139
Arkantos1695 9:ec0ceec8f5f5 140 mctrl.stop();
uwang3 11:f95294698901 141
uwang3 11:f95294698901 142 // Turn 90 degrees
Arkantos1695 10:2a1c8ce9d76c 143 float currt = t.read();
uwang3 11:f95294698901 144 if (tr) {
uwang3 11:f95294698901 145 mctrl.turnRight();
uwang3 11:f95294698901 146 } else {
uwang3 11:f95294698901 147 mctrl.turnLeft();
uwang3 11:f95294698901 148 }
uwang3 11:f95294698901 149
uwang3 12:f57a51ec8399 150 // Go forward 1.25 seconds... is it though???
uwang3 12:f57a51ec8399 151 // change dstance???? globals?? to make sure it doesnt hit wall later on
cbrice6 3:c07ea8bf242e 152 currt = t.read();
uwang3 12:f57a51ec8399 153 while ((t.read() - currt) < 1.25) {
uwang3 11:f95294698901 154 mctrl.fwd();
uwang3 11:f95294698901 155 }
uwang3 11:f95294698901 156
uwang3 11:f95294698901 157 // Turn 90 degrees
uwang3 11:f95294698901 158 currt = t.read();
uwang3 11:f95294698901 159 if (tr) {
uwang3 11:f95294698901 160 mctrl.turnRight();
uwang3 11:f95294698901 161 } else {
uwang3 11:f95294698901 162 mctrl.turnLeft();
uwang3 11:f95294698901 163 }
uwang3 11:f95294698901 164
Arkantos1695 10:2a1c8ce9d76c 165 tr = !tr;
uwang3 11:f95294698901 166 } else {
uwang3 11:f95294698901 167 // Continue forward until wall.
uwang3 11:f95294698901 168 mctrl.fwd();
cbrice6 0:9e014841f2b7 169 }
cbrice6 0:9e014841f2b7 170 }
cbrice6 0:9e014841f2b7 171
uwang3 12:f57a51ec8399 172 // nonsense so far
Arkantos1695 8:2d0fc244cc65 173 void dist2(int distance) // left sensor interrupt
Arkantos1695 8:2d0fc244cc65 174 {
Arkantos1695 9:ec0ceec8f5f5 175
Arkantos1695 9:ec0ceec8f5f5 176 leftdistanceending = distance;
Arkantos1695 9:ec0ceec8f5f5 177 //pc.printf(" Distance %d mm\r\n", distance);
Arkantos1695 9:ec0ceec8f5f5 178
Arkantos1695 9:ec0ceec8f5f5 179
Arkantos1695 8:2d0fc244cc65 180 }
Arkantos1695 8:2d0fc244cc65 181
uwang3 12:f57a51ec8399 182 // nonsense so far
Arkantos1695 8:2d0fc244cc65 183 void dist3(int distance) // right sensor interrupt
Arkantos1695 8:2d0fc244cc65 184 {
Arkantos1695 8:2d0fc244cc65 185 /*if (distance < 150) {
Arkantos1695 8:2d0fc244cc65 186 pc.printf("Right Sensor trigg");
Arkantos1695 8:2d0fc244cc65 187 }
Arkantos1695 8:2d0fc244cc65 188 else {
Arkantos1695 8:2d0fc244cc65 189 pc.printf("Right Sensor echo");
Arkantos1695 8:2d0fc244cc65 190 }*/
Arkantos1695 9:ec0ceec8f5f5 191 //rightdistance = distance;
Arkantos1695 8:2d0fc244cc65 192 }
Arkantos1695 8:2d0fc244cc65 193
cbrice6 0:9e014841f2b7 194 //------------------------------|
cbrice6 0:9e014841f2b7 195 // PIN INITIALIZATIONS (cont'd) |
cbrice6 0:9e014841f2b7 196 //------------------------------|
cbrice6 0:9e014841f2b7 197 // Setup Ultrasonic Sensor pins:
Arkantos1695 10:2a1c8ce9d76c 198 ultrasonic usensor(p15, p16, .07, 1, &dist); // trigger to p13, echo to p14...
Arkantos1695 8:2d0fc244cc65 199 // update every .07 secs w/ timeout after 1 sec...
cbrice6 0:9e014841f2b7 200 // call "dist" when the distance changes...
Arkantos1695 10:2a1c8ce9d76c 201 ultrasonic usensor2(p15, p26, .07, 1, &dist2); // left sensor trigger
Arkantos1695 10:2a1c8ce9d76c 202 ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
Arkantos1695 10:2a1c8ce9d76c 203
cbrice6 0:9e014841f2b7 204
cbrice6 0:9e014841f2b7 205 //---------------|
cbrice6 0:9e014841f2b7 206 // MAIN FUNCTION |
cbrice6 0:9e014841f2b7 207 //---------------|
cbrice6 0:9e014841f2b7 208 int main() {
cbrice6 3:c07ea8bf242e 209 // Use internal pullup for the switch:
cbrice6 6:d25157f50f14 210 sw.mode(PullUp);
uwang3 11:f95294698901 211
cbrice6 0:9e014841f2b7 212 // Initialize the Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 213 usensor.startUpdates();
Arkantos1695 8:2d0fc244cc65 214 usensor2.startUpdates();
Arkantos1695 8:2d0fc244cc65 215 usensor3.startUpdates();
Arkantos1695 9:ec0ceec8f5f5 216 wait(0.5);
uwang3 11:f95294698901 217
uwang3 11:f95294698901 218 // obtain and print starting distances
uwang3 11:f95294698901 219 int forwardDist = usensor.getCurrentDistance();
uwang3 12:f57a51ec8399 220 int leftDist = usensor2.getCurrentDistance();
uwang3 11:f95294698901 221 int rightDist = usensor3.getCurrentDistance();
Arkantos1695 9:ec0ceec8f5f5 222 wait(0.5);
uwang3 11:f95294698901 223
uwang3 11:f95294698901 224 pc.printf("Current Forward Starting Distance %d mm\r\n", forwardDist);
uwang3 12:f57a51ec8399 225 pc.printf("Current Left Starting Distance %d mm\r\n", leftDist);
uwang3 11:f95294698901 226 pc.printf("Current Right Starting Distance %d mm\r\n", rightDist);
Arkantos1695 9:ec0ceec8f5f5 227
cbrice6 0:9e014841f2b7 228 // Initialize the Timer:
cbrice6 0:9e014841f2b7 229 t.start();
Arkantos1695 9:ec0ceec8f5f5 230
uwang3 11:f95294698901 231 //while(1) {
uwang3 11:f95294698901 232 while (!sw) {
uwang3 12:f57a51ec8399 233 // if front sonar detects a change in the distace measured, dist is called
uwang3 11:f95294698901 234 usensor.checkDistance();
uwang3 11:f95294698901 235
uwang3 12:f57a51ec8399 236 // obtain the distance measurements for the left and right sonars
uwang3 11:f95294698901 237 int currLeftDist = usensor2.getCurrentDistance();
uwang3 11:f95294698901 238 int currRightDist = usensor3.getCurrentDistance();
uwang3 11:f95294698901 239
uwang3 12:f57a51ec8399 240 // if the left and right sonars detect distances greater than 100mm
uwang3 12:f57a51ec8399 241 // (10 cm or 3.93 inches), just go forward aka ignore path corr.
uwang3 11:f95294698901 242 if (currLeftDist > 100 && currRightDist > 100) {
uwang3 11:f95294698901 243 mctrl.fwd();
uwang3 11:f95294698901 244 } else {
uwang3 12:f57a51ec8399 245 // if left side of robot is nearer to the wall...
uwang3 12:f57a51ec8399 246 if (currLeftDist < currRightDist) {
uwang3 12:f57a51ec8399 247 //if robot has drifted toward the wall by a lot, scale R turn proportionally
uwang3 12:f57a51ec8399 248 if ((leftDist - currLeftDist) > 40) {
uwang3 12:f57a51ec8399 249 // take a right turn with a greater speed
uwang3 12:f57a51ec8399 250 // (trying to emulate a "big" turn, may need to be changed)
uwang3 12:f57a51ec8399 251 mctrl.turnRightScaled(1);
uwang3 12:f57a51ec8399 252 // this is an attempt to go forward (may need to change)
uwang3 12:f57a51ec8399 253 mctrl.fwd();
uwang3 12:f57a51ec8399 254 //if drifted toward wall a little (less than 40 but greater than 5 mm), scale accordingly
uwang3 12:f57a51ec8399 255 } else if ((leftDist - currLeftDist) > 5) {
uwang3 12:f57a51ec8399 256 mctrl.turnRightScaled(0.75);
uwang3 12:f57a51ec8399 257 mctrl.fwd();
uwang3 12:f57a51ec8399 258 //if drift away from wall a lot, scale L turn accordingly
uwang3 12:f57a51ec8399 259 } else if ((currLeftDist - leftDist) > 40) {
uwang3 12:f57a51ec8399 260 mctrl.turnLeftScaled(1);
uwang3 12:f57a51ec8399 261 mctrl.fwd();
uwang3 12:f57a51ec8399 262 } else if ((currLeftDist - leftDist) > 5) {
uwang3 12:f57a51ec8399 263 mctrl.turnLeftScaled(0.75);
uwang3 12:f57a51ec8399 264 mctrl.fwd();
uwang3 12:f57a51ec8399 265 // if drift if less than 5 mm in either direction (away or toward wall), go forward normally...
uwang3 12:f57a51ec8399 266 // last meeting i had this commented out, see how the robot behaves this time!!
uwang3 12:f57a51ec8399 267 } else {
uwang3 12:f57a51ec8399 268 mctrl.fwd();
uwang3 12:f57a51ec8399 269 }
uwang3 12:f57a51ec8399 270 } else if (currLeftDist > currRightDist) {
uwang3 12:f57a51ec8399 271 //if robot has drifted toward the wall by a lot, scale L turn by a lot
uwang3 12:f57a51ec8399 272 if ((rightDist - currRightDist) > 40) {
uwang3 12:f57a51ec8399 273 mctrl.turnLeftScaled(1);
uwang3 12:f57a51ec8399 274 mctrl.fwd();
uwang3 12:f57a51ec8399 275 //if drifted toward wall a little, scale accordingly
uwang3 12:f57a51ec8399 276 } else if ((rightDist - currRightDist) > 5) {
uwang3 12:f57a51ec8399 277 mctrl.turnLeftScaled(0.75);
uwang3 12:f57a51ec8399 278 mctrl.fwd();
uwang3 12:f57a51ec8399 279 //if drift away a lot, scale R turn accordingly
uwang3 12:f57a51ec8399 280 } else if ((currRightDist - rightDist) > 40) {
uwang3 12:f57a51ec8399 281 mctrl.turnRightScaled(1);
uwang3 12:f57a51ec8399 282 mctrl.fwd();
uwang3 12:f57a51ec8399 283 } else if ((currRightDist - rightDist) > 5) {
uwang3 12:f57a51ec8399 284 mctrl.turnRightScaled(0.75);
uwang3 12:f57a51ec8399 285 mctrl.fwd();
uwang3 12:f57a51ec8399 286 } else {
uwang3 12:f57a51ec8399 287 mctrl.fwd();
uwang3 12:f57a51ec8399 288 }
uwang3 12:f57a51ec8399 289 }
uwang3 12:f57a51ec8399 290
uwang3 12:f57a51ec8399 291 // update the left and right distances that will the next values will be compared to. may be weird so maybe wait/etc.
uwang3 12:f57a51ec8399 292 leftDist = currLeftDist;
uwang3 12:f57a51ec8399 293 rightDist = currRightDist;
uwang3 11:f95294698901 294 }
Arkantos1695 9:ec0ceec8f5f5 295 }
uwang3 12:f57a51ec8399 296 // if switch flipped, stop robot
Arkantos1695 9:ec0ceec8f5f5 297 mctrl.stop();
cbrice6 0:9e014841f2b7 298 }
cbrice6 3:c07ea8bf242e 299