Diff: main.cpp

Revision:

19:3118e8e60182

Parent:

18:6f120b374991

Child:

20:79c99bbf6dd7

diff -r 6f120b374991 -r 3118e8e60182 main.cpp
--- a/main.cpp	Tue Apr 11 02:02:01 2017 +0000
+++ b/main.cpp	Tue Apr 11 02:23:17 2017 +0000
@@ -42,7 +42,7 @@
 void pwmupdate()
 {
 
-    myled = !myled;
+    //myled = !myled;
     wv = I1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
     wv = (wv-2.0)*5000.0; // Scale the velocity to rad/s
 
@@ -50,15 +50,19 @@
 
     r1 = (Kbt*bt + Kbv*velocity.z + Kwv*wv);
 
-    r1 = (r1 + 6.0)/12.0 ;              //Normalize for PWM output
+    
 
     //Limit PWM range
-//    if (r1 > 1.0) {
-//        r1 = 1.0;
-//    }
-    if (r1 < 0.0) {
-        r1 = 0.0;
+    if (r1 > 6.0) {
+        r1 = 6.0;
     }
+    
+    if (r1 < -6.0) {
+        r1 = -6.0;
+    }
+    
+    r1 = (r1 + 6.0)/12.0 ;              //Normalize for PWM output
+    
     P1 = r1;
     //P2 = (euler_angles.r/360.0);
     //P3 = (euler_angles.p/360.0);
@@ -71,12 +75,13 @@
     if(!isPressed) {
         pwmint.attach(&pwmupdate, .005);
         EN1 = 1;
+        myled = 1;
         isPressed=1;
 
     } else {
         pwmint.detach();
         P1 = 0.0;
-        r1 = -1.0;
+        bt = 0.0;
         myled = 0;
         EN1 = 0;
         //P2 = 0;