Route Fix
Dependencies: mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library
Diff: main.cpp
- Revision:
- 10:2a1c8ce9d76c
- Parent:
- 9:ec0ceec8f5f5
--- a/main.cpp Mon Dec 03 17:01:09 2018 +0000 +++ b/main.cpp Wed Dec 05 03:59:52 2018 +0000 @@ -22,6 +22,7 @@ Motor Rfront(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7... Motor Lback(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9... Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11... +LSM9DS1 imu(p28, p27, 0xD6, 0x3C); Timer t; @@ -63,7 +64,7 @@ // Turn left 90 degrees: void turnLeft(float currt){ stop(); - while ((t.read()-currt) < 1.30) { + while ((t.read()-currt) < 1.53) { Lfront.speed(-1); Lback.speed(-1); Rfront.speed(1); @@ -74,16 +75,31 @@ }; // Turn right 90 degrees: - void turnRight(float currt){ + void turnRight(){ stop(); - while ((t.read()-currt) < 1.30) { + float degree = 0.0, angularV = 0.0; + float currt = t.read(); + /*while ((t.read()-currt) < 1.56) { Lfront.speed(1); Lback.speed(1); Rfront.speed(-1); Rback.speed(-1); + }*/ + while(degree < 90) { + while(imu.gyroAvailable()); + imu.readGyro(); + angularV = imu.gz; + Lfront.speed(1); + Lback.speed(1); + Rfront.speed(-1); + Rback.speed(-1); + if(angularV > 50.0 || angularV <-50.0) { + degree += (abs(angularV))/100.00; + } + wait(.4); } stop(); - //wait(0.02); + wait(0.02); }; @@ -107,16 +123,16 @@ // [Step 1] - /*float currt = t.read(); - if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);} + float currt = t.read(); + if (tr) {mctrl.turnRight();} else {mctrl.turnLeft(currt);} // [Step 2] currt = t.read(); while ((t.read()-currt) < 1) {mctrl.fwd();} // [Step 3] currt = t.read(); - if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);} + if (tr) {mctrl.turnRight();} else {mctrl.turnLeft(currt);} // [End] - tr = !tr;*/ + tr = !tr; } else { mctrl.fwd(); @@ -149,11 +165,12 @@ // PIN INITIALIZATIONS (cont'd) | //------------------------------| // Setup Ultrasonic Sensor pins: -ultrasonic usensor(p13, p14, .07, 1, &dist); // trigger to p13, echo to p14... +ultrasonic usensor(p15, p16, .07, 1, &dist); // trigger to p13, echo to p14... // update every .07 secs w/ timeout after 1 sec... // call "dist" when the distance changes... -ultrasonic usensor2(p13, p26, .07, 1, &dist2); // left sensor trigger -ultrasonic usensor3(p13, p25, .07, 1, &dist3); //right sensor trigger +ultrasonic usensor2(p15, p26, .07, 1, &dist2); // left sensor trigger +ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger + //---------------| // MAIN FUNCTION |