something
Dependencies: mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library
Diff: main.cpp
- Revision:
- 1:92f6242c0196
- Parent:
- 0:9e014841f2b7
- Child:
- 3:c07ea8bf242e
--- a/main.cpp Mon Nov 19 01:08:21 2018 +0000 +++ b/main.cpp Mon Nov 19 18:41:30 2018 +0000 @@ -4,6 +4,11 @@ #include "Motor.h" // Motor Drivers +// Global Variables: +float start; +float heading; +bool turnflag = false; + //---------------------| // PIN INITIALIZATIONS | //---------------------| @@ -12,8 +17,8 @@ // Setup Motor Driver pins: Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5... -Motor Lback(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7... -Motor Rfront(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9... +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... Timer t; @@ -23,90 +28,98 @@ //-----------------------| class mctrl { public: - void stop(void); - void fwd(float s); - void rev(float s); - void turnLeft(float s); - void turnRight(float s); -private: - float tcurr; - float turntime -}; + int LeftCount; + int RightCount; + + // Stop all motors: + void stop(void) { + Lfront.speed(0); + Lback.speed(0); + Rfront.speed(0); + Rback.speed(0); + wait(0.02); + }; + // Go forward at constant speed: + void fwd(void){ + stop(); + + Lfront.speed(1); + Lback.speed(1); + Rfront.speed(1); + Rback.speed(1); + wait(0.02); + }; + // Reverse at constant speed: + void rev(void){ + stop(); + + Lfront.speed(-1); + Lback.speed(-1); + Rfront.speed(-1); + Rback.speed(-1); + wait(0.02); + }; + // Turn left 90 degrees: + void turnLeft(void){ + stop(); + + while ((heading-start) < 90) { + Lfront.speed(-1); + Lback.speed(-1); + Rfront.speed(1); + Rback.speed(1); + } + + LeftCount++; + + stop(); + wait(0.02); + }; + // Turn right 90 degrees: + void turnRight(void){ + stop(); + + while ((heading-start) < 90) { + Lfront.speed(1); + Lback.speed(1); + Rfront.speed(-1); + Rback.speed(-1); + } + + RightCount++; + + stop(); + wait(0.02); + }; +} mctrl; //------------------| // HELPER FUNCTIONS | //------------------| -void mctrl::stop(void) -{ - Lfront.speed(0); - Lback.speed(0); - Rfront.speed(0); - Rback.speed(0); - wait(0.02); -} - -void mctrl::fwd(float s) -{ - mctrl.stop(); - - Lfront.speed(s); - Lback.speed(s); - Rfront.speed(s); - Rback.speed(s); - wait(0.02); -} - -void mctrl::rev(float s) -{ - mctrl.stop(); - - Lfront.speed(-s); - Lback.speed(-s); - Rfront.speed(-s); - Rback.speed(-s); - wait(0.02); -} - -void mctrl::turnLeft(float s) -{ - mctrl.stop(); - - tcurr = t.read(); - while ((t.read() - tcurr) < turntime) { - Lfront.speed(-s); - Lback.speed(-s); - Rfront.speed(s); - Rback.speed(s); - } - - mctrl.stop(); - wait(0.02); -} - -void mctrl::turnRight(float s) -{ - mctrl.stop(); - - tcurr = t.read(); - while ((t.read() - tcurr) < turntime) { - Lfront.speed(s); - Lback.speed(s); - Rfront.speed(-s); - Rback.speed(-s); - } - - mctrl.stop(); - wait(0.02); -} - void dist(int distance) // NOTE: by default "distance" is in mm... { - if (distance < 150) { + if (distance < 150 || turnflag == true) { + turnflag = true; + + mctrl.stop(); // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm // 1) Turn right. // 2) Go forward __ seconds. // 3) Turn right. // 4) Continue forward until wall. + mctrl.turnRight(); + float currt = t.read(); + while ((t.read()-currt) < 3) { + mctrl.fwd(); + } + mctrl.turnRight(); + if (mctrl.RightCount == 2) { + turnflag = false; + mctrl.RightCount = 0; + } + } + else { + mctrl.fwd(); } } @@ -123,31 +136,37 @@ //---------------| int main() { // Initialize and calibrate the IMU: - IMU.begin(); - if (!IMU.begin()) { - pc.printf("Failed to communicate with LSM9DS1.\n"); - } - IMU.calibrate(); + //IMU.begin(); + //if (!IMU.begin()) { + // Do something if it doesn't detect the IMU... + //} + //IMU.calibrate(); + + //IMU.readGyro(); + //start = IMU.calcGyro(IMU.gx); // Initialize the Ultrasonic Sensor: usensor.startUpdates(); + // Initialize the Motor turn counts: + mctrl.LeftCount = 0; + mctrl.RightCount = 0; + // Initialize the Timer: t.start(); while(1) { // Read IMU gyro: - while(!IMU.gyroAvailable()); - IMU.readGyro(); + //while(!IMU.gyroAvailable()); + //IMU.readGyro(); + //heading = IMU.calcGyro(IMU.gx); // X - IMU.calcGyro(IMU.gx) - // Y - IMU.calcGyro(IMU.gx) + // Y - IMU.calcGyro(IMU.gy) // Z - IMU.calcGyro(IMU.gz) // Read Ultrasonic Sensor: usensor.checkDistance(); - // ... - wait(0.1); } }