mohammad karim / Mbed 2 deprecated 4180_FINAL_PROJECTUricascode

something

Dependencies:   mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library

main.cpp@11:f95294698901, 2018-12-08 (annotated)

Committer:
uwang3
Date:
Sat Dec 08 19:59:14 2018 +0000
Revision:
11:f95294698901
Parent:
10:2a1c8ce9d76c
Child:
12:7598d38e7d44
Wall Path Correction; TODO:; adjust dist(int) function so that you will stop going forward within a certain range of the wall instead of solely relying on time

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 11:f95294698901 109 void turnLeftScaled(double scale) {
uwang3 11:f95294698901 110 stop();
uwang3 11:f95294698901 111 Lfront.speed(-1 * scale);
uwang3 11:f95294698901 112 Lback.speed(-1 * scale);
uwang3 11:f95294698901 113 Rfront.speed(1 * scale);
uwang3 11:f95294698901 114 Rback.speed(1 * scale);
uwang3 11:f95294698901 115 //stop();
uwang3 11:f95294698901 116 wait(0.02);
uwang3 11:f95294698901 117 };
Arkantos1695 9:ec0ceec8f5f5 118
uwang3 11:f95294698901 119 void turnRightScaled(double scale) {
uwang3 11:f95294698901 120 stop();
uwang3 11:f95294698901 121 Lfront.speed(1 * scale);
uwang3 11:f95294698901 122 Lback.speed(1 * scale);
uwang3 11:f95294698901 123 Rfront.speed(-1 * scale);
uwang3 11:f95294698901 124 Rback.speed(-1 * scale);
uwang3 11:f95294698901 125 //stop();
uwang3 11:f95294698901 126 wait(0.02);
uwang3 11:f95294698901 127 };
cbrice6 1:92f6242c0196 128 } mctrl;
cbrice6 0:9e014841f2b7 129
cbrice6 0:9e014841f2b7 130 //------------------|
cbrice6 0:9e014841f2b7 131 // HELPER FUNCTIONS |
cbrice6 0:9e014841f2b7 132 //------------------|
uwang3 11:f95294698901 133 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
cbrice6 0:9e014841f2b7 134 void dist(int distance) // NOTE: by default "distance" is in mm...
cbrice6 0:9e014841f2b7 135 {
cbrice6 3:c07ea8bf242e 136 if (distance < 150) {
Arkantos1695 9:ec0ceec8f5f5 137
Arkantos1695 9:ec0ceec8f5f5 138 mctrl.stop();
uwang3 11:f95294698901 139
uwang3 11:f95294698901 140 // Turn 90 degrees
Arkantos1695 10:2a1c8ce9d76c 141 float currt = t.read();
uwang3 11:f95294698901 142 if (tr) {
uwang3 11:f95294698901 143 mctrl.turnRight();
uwang3 11:f95294698901 144 } else {
uwang3 11:f95294698901 145 mctrl.turnLeft();
uwang3 11:f95294698901 146 }
uwang3 11:f95294698901 147
uwang3 11:f95294698901 148 // Go forward X (1?) number of seconds.
uwang3 11:f95294698901 149 // change dstance???? globals??
cbrice6 3:c07ea8bf242e 150 currt = t.read();
uwang3 11:f95294698901 151 while ((t.read() - currt) < 1.25 && distance > 150) {
uwang3 11:f95294698901 152 mctrl.fwd();
uwang3 11:f95294698901 153 }
uwang3 11:f95294698901 154
uwang3 11:f95294698901 155 // Turn 90 degrees
uwang3 11:f95294698901 156 currt = t.read();
uwang3 11:f95294698901 157 if (tr) {
uwang3 11:f95294698901 158 mctrl.turnRight();
uwang3 11:f95294698901 159 } else {
uwang3 11:f95294698901 160 mctrl.turnLeft();
uwang3 11:f95294698901 161 }
uwang3 11:f95294698901 162
Arkantos1695 10:2a1c8ce9d76c 163 tr = !tr;
uwang3 11:f95294698901 164 } else {
uwang3 11:f95294698901 165 // Continue forward until wall.
uwang3 11:f95294698901 166 mctrl.fwd();
cbrice6 0:9e014841f2b7 167 }
cbrice6 0:9e014841f2b7 168 }
cbrice6 0:9e014841f2b7 169
Arkantos1695 8:2d0fc244cc65 170 void dist2(int distance) // left sensor interrupt
Arkantos1695 8:2d0fc244cc65 171 {
Arkantos1695 9:ec0ceec8f5f5 172
Arkantos1695 9:ec0ceec8f5f5 173 leftdistanceending = distance;
Arkantos1695 9:ec0ceec8f5f5 174 //pc.printf(" Distance %d mm\r\n", distance);
Arkantos1695 9:ec0ceec8f5f5 175
Arkantos1695 9:ec0ceec8f5f5 176
Arkantos1695 8:2d0fc244cc65 177 }
Arkantos1695 8:2d0fc244cc65 178
Arkantos1695 8:2d0fc244cc65 179 void dist3(int distance) // right sensor interrupt
Arkantos1695 8:2d0fc244cc65 180 {
Arkantos1695 8:2d0fc244cc65 181 /*if (distance < 150) {
Arkantos1695 8:2d0fc244cc65 182 pc.printf("Right Sensor trigg");
Arkantos1695 8:2d0fc244cc65 183 }
Arkantos1695 8:2d0fc244cc65 184 else {
Arkantos1695 8:2d0fc244cc65 185 pc.printf("Right Sensor echo");
Arkantos1695 8:2d0fc244cc65 186 }*/
Arkantos1695 9:ec0ceec8f5f5 187 //rightdistance = distance;
Arkantos1695 8:2d0fc244cc65 188 }
Arkantos1695 8:2d0fc244cc65 189
cbrice6 0:9e014841f2b7 190 //------------------------------|
cbrice6 0:9e014841f2b7 191 // PIN INITIALIZATIONS (cont'd) |
cbrice6 0:9e014841f2b7 192 //------------------------------|
cbrice6 0:9e014841f2b7 193 // Setup Ultrasonic Sensor pins:
Arkantos1695 10:2a1c8ce9d76c 194 ultrasonic usensor(p15, p16, .07, 1, &dist); // trigger to p13, echo to p14...
Arkantos1695 8:2d0fc244cc65 195 // update every .07 secs w/ timeout after 1 sec...
cbrice6 0:9e014841f2b7 196 // call "dist" when the distance changes...
Arkantos1695 10:2a1c8ce9d76c 197 ultrasonic usensor2(p15, p26, .07, 1, &dist2); // left sensor trigger
Arkantos1695 10:2a1c8ce9d76c 198 ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
Arkantos1695 10:2a1c8ce9d76c 199
cbrice6 0:9e014841f2b7 200
cbrice6 0:9e014841f2b7 201 //---------------|
cbrice6 0:9e014841f2b7 202 // MAIN FUNCTION |
cbrice6 0:9e014841f2b7 203 //---------------|
cbrice6 0:9e014841f2b7 204 int main() {
cbrice6 3:c07ea8bf242e 205 // Use internal pullup for the switch:
cbrice6 6:d25157f50f14 206 sw.mode(PullUp);
uwang3 11:f95294698901 207
cbrice6 0:9e014841f2b7 208 // Initialize the Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 209 usensor.startUpdates();
Arkantos1695 8:2d0fc244cc65 210 usensor2.startUpdates();
Arkantos1695 8:2d0fc244cc65 211 usensor3.startUpdates();
Arkantos1695 9:ec0ceec8f5f5 212 wait(0.5);
uwang3 11:f95294698901 213
uwang3 11:f95294698901 214 // obtain and print starting distances
uwang3 11:f95294698901 215 int forwardDist = usensor.getCurrentDistance();
Arkantos1695 9:ec0ceec8f5f5 216 int leftdistancestarting = usensor2.getCurrentDistance();
uwang3 11:f95294698901 217 int rightDist = usensor3.getCurrentDistance();
Arkantos1695 9:ec0ceec8f5f5 218 wait(0.5);
uwang3 11:f95294698901 219
uwang3 11:f95294698901 220 pc.printf("Current Forward Starting Distance %d mm\r\n", forwardDist);
uwang3 11:f95294698901 221 pc.printf("Current Left Starting Distance %d mm\r\n", leftdistancestarting);
uwang3 11:f95294698901 222 pc.printf("Current Right Starting Distance %d mm\r\n", rightDist);
Arkantos1695 9:ec0ceec8f5f5 223
cbrice6 0:9e014841f2b7 224 // Initialize the Timer:
cbrice6 0:9e014841f2b7 225 t.start();
Arkantos1695 9:ec0ceec8f5f5 226
uwang3 11:f95294698901 227 //while(1) {
uwang3 11:f95294698901 228 while (!sw) {
uwang3 11:f95294698901 229 usensor.checkDistance();
uwang3 11:f95294698901 230
uwang3 11:f95294698901 231 //determine which sensor is closest to wall
uwang3 11:f95294698901 232 int currLeftDist = usensor2.getCurrentDistance();
uwang3 11:f95294698901 233 int currRightDist = usensor3.getCurrentDistance();
uwang3 11:f95294698901 234
uwang3 11:f95294698901 235 // may also have to scale the difference (40, etc.) based on -
uwang3 11:f95294698901 236 // iteration of the 90deg turn sequence (loss of accuracy)
uwang3 11:f95294698901 237 // or decrease numbers of turns
uwang3 11:f95294698901 238
uwang3 11:f95294698901 239 //if left sensor closest to wall
uwang3 11:f95294698901 240 if (currLeftDist > 100 && currRightDist > 100) {
uwang3 11:f95294698901 241 //mctrl.rev();
uwang3 11:f95294698901 242 mctrl.fwd();
uwang3 11:f95294698901 243 } else {
uwang3 11:f95294698901 244 if (currLeftDist < currRightDist) {
uwang3 11:f95294698901 245 //if robot has drifted toward the wall by a lot, scale R turn prop.
uwang3 11:f95294698901 246 if ((leftdistancestarting - currLeftDist) > 40) {
uwang3 11:f95294698901 247 mctrl.turnRightScaled(1);
uwang3 11:f95294698901 248 mctrl.fwd();
uwang3 11:f95294698901 249 //if drifted toward wall a little, scale accordingly
uwang3 11:f95294698901 250 } else if ((leftdistancestarting - currLeftDist) > 5) {
uwang3 11:f95294698901 251 mctrl.turnRightScaled(0.75);
uwang3 11:f95294698901 252 mctrl.fwd();
uwang3 11:f95294698901 253 //if drift away a lot, scale L turn accordingly
uwang3 11:f95294698901 254 } else if ((currLeftDist - leftdistancestarting) > 40) {
uwang3 11:f95294698901 255 mctrl.turnLeftScaled(1);
uwang3 11:f95294698901 256 mctrl.fwd();
uwang3 11:f95294698901 257 } else if ((currLeftDist - leftdistancestarting) > 5) {
uwang3 11:f95294698901 258 mctrl.turnLeftScaled(0.75);
uwang3 11:f95294698901 259 mctrl.fwd();
uwang3 11:f95294698901 260 } //else {
uwang3 11:f95294698901 261 // mctrl.fwd();
uwang3 11:f95294698901 262 // }
uwang3 11:f95294698901 263 } else if (currLeftDist > currRightDist) {
uwang3 11:f95294698901 264 //if robot has drifted toward the wall by a lot, scale L turn by a lot
uwang3 11:f95294698901 265 if ((rightDist - currRightDist) > 40) {
uwang3 11:f95294698901 266 mctrl.turnLeftScaled(1);
uwang3 11:f95294698901 267 mctrl.fwd();
uwang3 11:f95294698901 268 //if drifted toward wall a little, scale accordingly
uwang3 11:f95294698901 269 } else if ((rightDist - currRightDist) > 5) {
uwang3 11:f95294698901 270 mctrl.turnLeftScaled(0.75);
uwang3 11:f95294698901 271 mctrl.fwd();
uwang3 11:f95294698901 272 //if drift away a lot, scale R turn accordingly
uwang3 11:f95294698901 273 } else if ((currRightDist - rightDist) > 40) {
uwang3 11:f95294698901 274 mctrl.turnRightScaled(1);
uwang3 11:f95294698901 275 mctrl.fwd();
uwang3 11:f95294698901 276 } else if ((currRightDist - rightDist) > 5) {
uwang3 11:f95294698901 277 mctrl.turnRightScaled(0.75);
uwang3 11:f95294698901 278 mctrl.fwd();
uwang3 11:f95294698901 279 } //else {
uwang3 11:f95294698901 280 // mctrl.fwd();
uwang3 11:f95294698901 281 // }
uwang3 11:f95294698901 282 }
uwang3 11:f95294698901 283 // may be weird when updating after turning? maybe wait/etc.
uwang3 11:f95294698901 284 leftdistancestarting = currLeftDist;
uwang3 11:f95294698901 285 rightDist = currRightDist;
uwang3 11:f95294698901 286 }
uwang3 11:f95294698901 287 //usensor2.checkDistance();
uwang3 11:f95294698901 288 //usensor3.checkDistance();
uwang3 11:f95294698901 289 //pc.printf("Current Ending Distance %d mm\r\n", leftdistanceending);
Arkantos1695 9:ec0ceec8f5f5 290 }
Arkantos1695 9:ec0ceec8f5f5 291 mctrl.stop();
uwang3 11:f95294698901 292 //}
cbrice6 0:9e014841f2b7 293 }
cbrice6 3:c07ea8bf242e 294