Diff: main.cpp

Revision:

11:f95294698901

Parent:

10:2a1c8ce9d76c

Child:

12:7598d38e7d44

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