Diff: main.cpp

Revision:

10:2a1c8ce9d76c

Parent:

9:ec0ceec8f5f5

Child:

11:f95294698901

--- a/main.cpp	Mon Dec 03 17:01:09 2018 +0000
+++ b/main.cpp	Wed Dec 05 03:59:52 2018 +0000
@@ -22,6 +22,7 @@
 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...
+LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
 
 Timer t;
 
@@ -63,7 +64,7 @@
     // Turn left 90 degrees:
     void turnLeft(float currt){
         stop();
-        while ((t.read()-currt) < 1.30) {
+        while ((t.read()-currt) < 1.53) {
             Lfront.speed(-1);
             Lback.speed(-1);
             Rfront.speed(1);
@@ -74,16 +75,31 @@
     };
     
     // Turn right 90 degrees:
-    void turnRight(float currt){
+    void turnRight(){
         stop();
-        while ((t.read()-currt) < 1.30) {
+        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();
+            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(.4);
         }
         stop();
-        //wait(0.02);
+        wait(0.02);
     };
     
     
@@ -107,16 +123,16 @@
     
             
         // [Step 1]
-        /*float currt = t.read();
-        if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
+        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]
         currt = t.read();
-        if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
+        if (tr) {mctrl.turnRight();} else {mctrl.turnLeft(currt);}
         // [End]
-        tr = !tr;*/
+        tr = !tr;
     }
     else {
         mctrl.fwd();
@@ -149,11 +165,12 @@
 // PIN INITIALIZATIONS (cont'd) |
 //------------------------------|
 // Setup Ultrasonic Sensor pins:
-ultrasonic usensor(p13, p14, .07, 1, &dist);  // trigger to p13, echo to p14...
+ultrasonic usensor(p15, p16, .07, 1, &dist);  // trigger to p13, echo to p14...
                                              // update every .07 secs w/ timeout after 1 sec...
                                               // call "dist" when the distance changes...
-ultrasonic usensor2(p13, p26, .07, 1, &dist2);  // left sensor trigger
-ultrasonic usensor3(p13, p25, .07, 1, &dist3); //right sensor trigger
+ultrasonic usensor2(p15, p26, .07, 1, &dist2);  // left sensor trigger
+ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
+
 
 //---------------|
 // MAIN FUNCTION |