something
Dependencies: mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library
Diff: main.cpp
- Revision:
- 11:f95294698901
- Parent:
- 10:2a1c8ce9d76c
- Child:
- 12:7598d38e7d44
diff -r 2a1c8ce9d76c -r f95294698901 main.cpp --- a/main.cpp Wed Dec 05 03:59:52 2018 +0000 +++ b/main.cpp Sat Dec 08 19:59:14 2018 +0000 @@ -18,8 +18,8 @@ DigitalIn sw(p20); Serial pc(USBTX,USBRX); // Setup Motor Driver pins: -Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5... -Motor Rfront(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7... +Motor Lfront(p21, p6, p5); // PWM to p21, forward to p5, reverse to p6... +Motor Rfront(p22, p8, p7); // PWM to p22, forward to p7, reverse to p8... 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); @@ -44,10 +44,10 @@ void fwd(void){ //stop(); - Lfront.speed(1); - Lback.speed(1); - Rfront.speed(1); - Rback.speed(1); + Lfront.speed(0.85); + Lback.speed(0.85); + Rfront.speed(0.85); + Rback.speed(0.85); wait(0.02); }; @@ -62,16 +62,25 @@ }; // Turn left 90 degrees: - void turnLeft(float currt){ + void turnLeft(){ stop(); - while ((t.read()-currt) < 1.53) { + float degree = 0.0, angularV = 0.0; + float currt = t.read(); + 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(.45); } stop(); - //wait(0.02); + wait(0.02); }; // Turn right 90 degrees: @@ -79,12 +88,6 @@ stop(); 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(); @@ -96,47 +99,71 @@ if(angularV > 50.0 || angularV <-50.0) { degree += (abs(angularV))/100.00; } - wait(.4); + wait(.45); } stop(); wait(0.02); }; - - + // Turn L or R, "mag" of turn prop. to dist. from wall + void turnLeftScaled(double scale) { + stop(); + Lfront.speed(-1 * scale); + Lback.speed(-1 * scale); + Rfront.speed(1 * scale); + Rback.speed(1 * scale); + //stop(); + wait(0.02); + }; + void turnRightScaled(double scale) { + stop(); + Lfront.speed(1 * scale); + Lback.speed(1 * scale); + Rfront.speed(-1 * scale); + Rback.speed(-1 * scale); + //stop(); + wait(0.02); + }; } mctrl; //------------------| // HELPER FUNCTIONS | //------------------| +// PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm void dist(int distance) // NOTE: by default "distance" is in mm... { if (distance < 150) { mctrl.stop(); - // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm - // 1) Turn 90 degrees (hardcoded => 1.4 seconds). - // 2) Go forward 1.5 seconds. - // 3) Turn 90 degrees (hardcoded => 1.4 seconds). - // 4) Continue forward until wall. - - - // [Step 1] + + // Turn 90 degrees 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] + if (tr) { + mctrl.turnRight(); + } else { + mctrl.turnLeft(); + } + + // Go forward X (1?) number of seconds. + // change dstance???? globals?? currt = t.read(); - if (tr) {mctrl.turnRight();} else {mctrl.turnLeft(currt);} - // [End] + while ((t.read() - currt) < 1.25 && distance > 150) { + mctrl.fwd(); + } + + // Turn 90 degrees + currt = t.read(); + if (tr) { + mctrl.turnRight(); + } else { + mctrl.turnLeft(); + } + tr = !tr; - } - else { - mctrl.fwd(); - + } else { + // Continue forward until wall. + mctrl.fwd(); } } @@ -160,7 +187,6 @@ //rightdistance = distance; } - //------------------------------| // PIN INITIALIZATIONS (cont'd) | //------------------------------| @@ -178,30 +204,91 @@ int main() { // Use internal pullup for the switch: sw.mode(PullUp); + // Initialize the Ultrasonic Sensor: usensor.startUpdates(); usensor2.startUpdates(); usensor3.startUpdates(); wait(0.5); + + // obtain and print starting distances + int forwardDist = usensor.getCurrentDistance(); int leftdistancestarting = usensor2.getCurrentDistance(); + int rightDist = usensor3.getCurrentDistance(); wait(0.5); - pc.printf("Current Starting Distance %d mm\r\n", leftdistancestarting); + + pc.printf("Current Forward Starting Distance %d mm\r\n", forwardDist); + pc.printf("Current Left Starting Distance %d mm\r\n", leftdistancestarting); + pc.printf("Current Right Starting Distance %d mm\r\n", rightDist); // Initialize the Timer: t.start(); - while(1) { - while (!sw) - { - - usensor.checkDistance(); - usensor2.checkDistance(); - usensor3.checkDistance(); - pc.printf("Current Ending Distance %d mm\r\n", leftdistanceending); - + //while(1) { + while (!sw) { + usensor.checkDistance(); + + //determine which sensor is closest to wall + int currLeftDist = usensor2.getCurrentDistance(); + int currRightDist = usensor3.getCurrentDistance(); + + // may also have to scale the difference (40, etc.) based on - + // iteration of the 90deg turn sequence (loss of accuracy) + // or decrease numbers of turns + + //if left sensor closest to wall + if (currLeftDist > 100 && currRightDist > 100) { + //mctrl.rev(); + mctrl.fwd(); + } else { + if (currLeftDist < currRightDist) { + //if robot has drifted toward the wall by a lot, scale R turn prop. + if ((leftdistancestarting - currLeftDist) > 40) { + mctrl.turnRightScaled(1); + mctrl.fwd(); + //if drifted toward wall a little, scale accordingly + } else if ((leftdistancestarting - currLeftDist) > 5) { + mctrl.turnRightScaled(0.75); + mctrl.fwd(); + //if drift away a lot, scale L turn accordingly + } else if ((currLeftDist - leftdistancestarting) > 40) { + mctrl.turnLeftScaled(1); + mctrl.fwd(); + } else if ((currLeftDist - leftdistancestarting) > 5) { + mctrl.turnLeftScaled(0.75); + mctrl.fwd(); + } //else { +// mctrl.fwd(); +// } + } else if (currLeftDist > currRightDist) { + //if robot has drifted toward the wall by a lot, scale L turn by a lot + if ((rightDist - currRightDist) > 40) { + mctrl.turnLeftScaled(1); + mctrl.fwd(); + //if drifted toward wall a little, scale accordingly + } else if ((rightDist - currRightDist) > 5) { + mctrl.turnLeftScaled(0.75); + mctrl.fwd(); + //if drift away a lot, scale R turn accordingly + } else if ((currRightDist - rightDist) > 40) { + mctrl.turnRightScaled(1); + mctrl.fwd(); + } else if ((currRightDist - rightDist) > 5) { + mctrl.turnRightScaled(0.75); + mctrl.fwd(); + } //else { +// mctrl.fwd(); +// } + } + // may be weird when updating after turning? maybe wait/etc. + leftdistancestarting = currLeftDist; + rightDist = currRightDist; + } + //usensor2.checkDistance(); + //usensor3.checkDistance(); + //pc.printf("Current Ending Distance %d mm\r\n", leftdistanceending); } mctrl.stop(); - - } + //} }