Route Fix
Dependencies: mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library
Diff: main.cpp
- Revision:
- 3:c07ea8bf242e
- Parent:
- 1:92f6242c0196
- Child:
- 4:6546c17ac0a6
diff -r 0f1b79bd395d -r c07ea8bf242e main.cpp --- a/main.cpp Mon Nov 19 21:44:30 2018 +0000 +++ b/main.cpp Wed Nov 21 19:30:56 2018 +0000 @@ -2,18 +2,25 @@ #include "LSM9DS1.h" // IMU #include "ultrasonic.h" // Ultrasonic Sensor #include "Motor.h" // Motor Drivers +#include "PinDetect.h" // Pin Detect (for switch) // Global Variables: -float start; -float heading; +bool tr = true; bool turnflag = false; //---------------------| // PIN INITIALIZATIONS | //---------------------| -// Setup IMU pins: -LSM9DS1 IMU(p28, p27, 0xD6, 0x3C); // SDA to p28, SCL to p27.. +// Debug LED pin: +//DigitalOut led(p25); + +// Motor Power Control pins: +/* +DigitalOut Ctrl1(p29); +DigitalOut Ctrl2(p30); +PinDetect sw(p20); +*/ // Setup Motor Driver pins: Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5... @@ -28,9 +35,6 @@ //-----------------------| class mctrl { public: - int LeftCount; - int RightCount; - // Stop all motors: void stop(void) { Lfront.speed(0); @@ -39,6 +43,7 @@ Rback.speed(0); wait(0.02); }; + // Go forward at constant speed: void fwd(void){ stop(); @@ -49,45 +54,39 @@ 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) { + while ((t.read()-currt) < 2) { 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) { + while ((t.read()-currt) < 2) { Lfront.speed(1); Lback.speed(1); Rfront.speed(-1); Rback.speed(-1); } - - RightCount++; - stop(); wait(0.02); }; @@ -98,25 +97,23 @@ //------------------| void dist(int distance) // NOTE: by default "distance" is in mm... { - 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(); + if (distance < 150) { + // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm + // 1) Turn 90 degrees (hardcoded => 2 seconds). + // 2) Go forward 2 seconds. + // 3) Turn 90 degrees (hardcoded => 2 seconds). + // 4) Continue forward until wall. + // [Step 1] float currt = t.read(); - while ((t.read()-currt) < 3) { - mctrl.fwd(); - } - mctrl.turnRight(); - if (mctrl.RightCount == 2) { - turnflag = false; - mctrl.RightCount = 0; - } + if (tr) {mctrl.turnRight(currt)} else {mctrl.turnLeft(currt)} + // [Step 2] + currt = t.read(); + while ((t.read()-currt) < 2) {mctrl.fwd()} + // [Step 3] + currt = t.read(); + if (tr) {mctrl.turnRight(currt)} else {mctrl.turnLeft(currt)} + // [End] + tr = !tr; } else { mctrl.fwd(); @@ -135,34 +132,53 @@ // MAIN FUNCTION | //---------------| int main() { + // Use internal pullup for the switch: + //sw.mode(PullUp); + + // Delay for initial pullups to take effect: + //wait(0.01); + // Initialize and calibrate the IMU: - //IMU.begin(); - //if (!IMU.begin()) { - // Do something if it doesn't detect the IMU... - //} - //IMU.calibrate(); + /* + LSM9DS1 imu(p28, p27, 0xD6, 0x3C); // SDA to p28, SCL to p27... + imu.begin(); + imu.calibrate(1); + imu.calibrateMag(0); - //IMU.readGyro(); - //start = IMU.calcGyro(IMU.gx); + imu.readGyro(); + start = imu.gz; + */ // Initialize the Ultrasonic Sensor: usensor.startUpdates(); - // Initialize the Motor turn counts: - mctrl.LeftCount = 0; - mctrl.RightCount = 0; - // Initialize the Timer: t.start(); while(1) { + // Toggle motor battery banks: + /* + if (!sw == 1) { + Ctrl1 = 1; + Ctrl2 = 1; + wait(0.2); + } + else { + Ctrl1 = 0; + Ctrl2 = 0; + wait(0.2); + } + */ + // Read IMU gyro: - //while(!IMU.gyroAvailable()); - //IMU.readGyro(); - //heading = IMU.calcGyro(IMU.gx); - // X - IMU.calcGyro(IMU.gx) - // Y - IMU.calcGyro(IMU.gy) - // Z - IMU.calcGyro(IMU.gz) + //while(!imu.gyroAvailable()); + //imu.readGyro(); + //heading = imu.calcGyro(imu.gz); + // X - imu.calcGyro(imu.gx) + // Y - imu.calcGyro(imu.gy) + // Z - imu.calcGyro(imu.gz) + + //if (heading > 90) {led=1;} else {led=0;} // Read Ultrasonic Sensor: usensor.checkDistance(); @@ -170,3 +186,4 @@ wait(0.1); } } +