Diff: main.cpp

Revision:

18:6f120b374991

Parent:

16:27069802baae

Child:

19:3118e8e60182

--- a/main.cpp	Mon Apr 10 21:59:07 2017 +0000
+++ b/main.cpp	Tue Apr 11 02:02:01 2017 +0000
@@ -9,7 +9,7 @@
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 
-Ticker pwmint; 
+Ticker pwmint;
 DigitalOut myled(LED1);
 InterruptIn button(USER_BUTTON);
 
@@ -20,53 +20,59 @@
 //PwmOut P2(PE_11);     1D FOCUS FOR NOW
 //PwmOut P3(PE_13);
 
-    I2C    i2c(PB_9, PB_8);         // SDA, SCL
-    BNO055 imu(i2c, PA_8);          // Reset
- 
-    BNO055_ID_INF_TypeDef   bno055_id_inf;
-    BNO055_EULER_TypeDef    euler_angles;
-    BNO055_VEL_TypeDef      velocity;  //IN PROGRESS
+I2C    i2c(PB_9, PB_8);         // SDA, SCL
+BNO055 imu(i2c, PA_8);          // Reset
+
+BNO055_ID_INF_TypeDef   bno055_id_inf;
+BNO055_EULER_TypeDef    euler_angles;
+BNO055_VEL_TypeDef      velocity;  //IN PROGRESS
 
 double Kbt = -89.9276;
 double Kbv = -14.9398;
-double Kwv = -0.0909;
+double Kwv = 0.0;//-0.0909;
 
 double wv;
+double bt;
 double r1;
 
+double pi = 3.14159265;
+
 int isPressed;
 
-void pwmupdate() {
-    
+void pwmupdate()
+{
+
     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
-    
-    
-    r1 = (Kbt*euler_angles.p + Kbv*velocity.z + Kwv*wv);
-    
+
+    bt = (euler_angles.p - (pi/4.0));
+
+    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){
+//    if (r1 > 1.0) {
+//        r1 = 1.0;
+//    }
+    if (r1 < 0.0) {
         r1 = 0.0;
-        }
-    P1 = r1; 
+    }
+    P1 = r1;
     //P2 = (euler_angles.r/360.0);
     //P3 = (euler_angles.p/360.0);
-    
-    }
-    
-    void eventFunction() {
-        
+
+}
+
+void eventFunction()
+{
+
     if(!isPressed) {
-        pwmint.attach(&pwmupdate, .005);    
-        EN1 = 1;   
+        pwmint.attach(&pwmupdate, .005);
+        EN1 = 1;
         isPressed=1;
-        
+
     } else {
         pwmint.detach();
         P1 = 0.0;
@@ -75,40 +81,43 @@
         EN1 = 0;
         //P2 = 0;
         //P3 = 0;
-        isPressed=0;   
+        isPressed=0;
     }
 }
 
 int main()
 {
-    
+
     pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
-    if (imu.chip_ready() == 0){
+    if (imu.chip_ready() == 0) {
         pc.printf("Bosch BNO055 is NOT available!!\r\n");
     }
-    
+
     imu.read_id_inf(&bno055_id_inf);
-    
+
     //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);
-               
+
     P1.period(0.0002);      //Set PWM frequency
-        
+
     isPressed=0;
     button.rise(&eventFunction);        //Enable Closed Loop
-    
-    
-    
+
+
+
     while(1) {
 
-        
-        pc.printf("H:%+6.4f [rad], P:%+6.4f, R:%+6.4f, R1%+6.4f [PWM], Theta_dot:%+6.4f [rad/s] , WV:%+6.4f [rad/s] \r\n",
-                    euler_angles.h, euler_angles.p, euler_angles.r, r1, velocity.z, wv);
-        
+
+        // pc.printf("H:%+6.4f [rad], P:%+6.4f, R:%+6.4f, R1%+6.4f [PWM], Theta_dot:%+6.4f [rad/s] , WV:%+6.4f [rad/s] \r\n",
+        //             euler_angles.h, euler_angles.p, euler_angles.r, r1, velocity.z, wv);
+
+        pc.printf("Theta:%+6.4f [rad], P:%+6.4f [rad], R1%+6.4f [PWM], Theta_dot:%+6.4f [rad/s], WV:%+6.4f [rad/s] \r\n",
+                  bt, euler_angles.p, r1, velocity.z, wv);
+
         imu.get_Euler_Angles(&euler_angles);
         imu.get_velocities(&velocity);
-        
+
         //wait(0.2);
     }
 }