mohammad karim / Mbed 2 deprecated 4180_FINAL_PROJECTUricascode

something

Dependencies:   mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library

main.cpp@12:7598d38e7d44, 2018-12-10 (annotated)

Committer:
jahed
Date:
Mon Dec 10 23:21:06 2018 +0000
Revision:
12:7598d38e7d44
Parent:
11:f95294698901 
i changed urica's old code here

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