Diff: main.cpp

Revision:

3:c07ea8bf242e

Parent:

1:92f6242c0196

Child:

4:6546c17ac0a6

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