Diff: main.cpp

Revision:

9:6a83e2777d24

Parent:

8:1011786787a4

Child:

10:69eb22acaea3

--- a/main.cpp	Sun Apr 09 19:28:56 2017 +0000
+++ b/main.cpp	Sun Apr 09 19:52:02 2017 +0000
@@ -14,6 +14,8 @@
 InterruptIn button(USER_BUTTON);
 
 PwmOut P1(PE_9);
+DigitalOut EN1(D0);
+
 //PwmOut P2(PE_11);     1D FOCUS FOR NOW
 //PwmOut P3(PE_13);
 
@@ -26,7 +28,7 @@
     
     //BNO055_EULER_TypeDef  velocity;  IN PROGRESS
 
-double Kbt = 0.003;
+double Kbt = .54;
 double Kbv = 0.23;
 double Kwv = 0.124;
 double r1;
@@ -37,7 +39,7 @@
     
     myled = !myled;
     
-    r1 = (Kbt*euler_angles.h);
+    r1 = (Kbt*euler_angles.h*3.14/180.0);
     r1 = (r1 + 6.0)/12.0 ;              //Normalize for PWM output
     P1 = r1; 
     //P2 = (euler_angles.r/360.0);
@@ -48,13 +50,15 @@
     void eventFunction() {
         
     if(!isPressed) {
-        pwmint.attach(&pwmupdate, .005);   
+        pwmint.attach(&pwmupdate, .005);    
+        EN1 = 1;   
         isPressed=1;
         
     } else {
         pwmint.detach();
         P1 = 0;
         myled = 0;
+        EN1 = 0;
         //P2 = 0;
         //P3 = 0;
         isPressed=0;   
@@ -75,18 +79,18 @@
     //           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);
                
-    P1.period(0.0002);      
+    P1.period(0.0002);      //Set PWM frequency
         
     isPressed=0;
-    button.rise(&eventFunction);
+    button.rise(&eventFunction);        //Enable Closed Loop
     
     
     
     while(1) {
 
         
-        pc.printf("Heading:%+6.4f [deg], Roll:%+6.4f [deg], Pitch:%+6.4f [deg]\r\n",
-                    euler_angles.h, euler_angles.r, euler_angles.p);
+        pc.printf("Heading:%+6.4f [deg], R1%+6.4f [PWM] \r\n", //, Pitch:%+6.4f [deg]\r\n",
+                    euler_angles.h, r1, euler_angles.p);
         
         imu.get_Euler_Angles(&euler_angles);