Diff: main.cpp

Revision:

8:1011786787a4

Parent:

7:1be7e6735fe2

Child:

9:6a83e2777d24

diff -r 1be7e6735fe2 -r 1011786787a4 main.cpp
--- a/main.cpp	Sun Apr 09 18:37:31 2017 +0000
+++ b/main.cpp	Sun Apr 09 19:28:56 2017 +0000
@@ -24,17 +24,22 @@
     BNO055_EULER_TypeDef  euler_angles;
     
     
-    //BNO055_EULER_TypeDef  velocity;  IN PROGESS
+    //BNO055_EULER_TypeDef  velocity;  IN PROGRESS
 
-int i = 180;
+double Kbt = 0.003;
+double Kbv = 0.23;
+double Kwv = 0.124;
+double r1;
+
 int isPressed;
 
 void pwmupdate() {
     
     myled = !myled;
     
-    
-    P1 = (euler_angles.h/360.0);
+    r1 = (Kbt*euler_angles.h);
+    r1 = (r1 + 6.0)/12.0 ;              //Normalize for PWM output
+    P1 = r1; 
     //P2 = (euler_angles.r/360.0);
     //P3 = (euler_angles.p/360.0);
     
@@ -43,12 +48,13 @@
     void eventFunction() {
         
     if(!isPressed) {
-        pwmint.attach(&pwmupdate, .5);   
+        pwmint.attach(&pwmupdate, .005);   
         isPressed=1;
         
     } else {
         pwmint.detach();
         P1 = 0;
+        myled = 0;
         //P2 = 0;
         //P3 = 0;
         isPressed=0;   
@@ -85,6 +91,6 @@
         imu.get_Euler_Angles(&euler_angles);
 
         
-        wait(0.2);
+        //wait(0.2);
     }
 }