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);
     }
 }