Diff: main.cpp

Revision:

20:79c99bbf6dd7

Parent:

19:3118e8e60182

Child:

21:4f60d60b2e5a

diff -r 3118e8e60182 -r 79c99bbf6dd7 main.cpp
--- a/main.cpp	Tue Apr 11 02:23:17 2017 +0000
+++ b/main.cpp	Tue Apr 11 05:28:54 2017 +0000
@@ -60,13 +60,22 @@
     if (r1 < -6.0) {
         r1 = -6.0;
     }
-    
-    r1 = (r1 + 6.0)/12.0 ;              //Normalize for PWM output
+        
+    r1 = ((.4*(r1/6.0)) + 0.5) ;              //Normalize for PWM output
     
     P1 = r1;
     //P2 = (euler_angles.r/360.0);
     //P3 = (euler_angles.p/360.0);
-
+    if (bt > (pi/8)){
+        EN1 = 0;
+        pwmint.detach();
+        }
+        
+    else if (bt< -(pi/8)){
+        EN1 = 0; 
+        pwmint.detach();
+        }
+        
 }
 
 void eventFunction()
@@ -80,7 +89,7 @@
 
     } else {
         pwmint.detach();
-        P1 = 0.0;
+        P1 = 0.5;
         bt = 0.0;
         myled = 0;
         EN1 = 0;
@@ -99,7 +108,7 @@
     }
 
     imu.read_id_inf(&bno055_id_inf);
-
+    P1 = .5;
     //pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n",
     //           bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id,
     //           bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);