Diff: main.cpp

Revision:

31:e159b0caa2ea

Parent:

30:8916c88fe676

Child:

32:e77ad4aff1fd

--- a/main.cpp	Fri Apr 21 16:52:08 2017 +0000
+++ b/main.cpp	Sat Apr 22 05:33:17 2017 +0000
@@ -26,9 +26,9 @@
 BNO055_ID_INF_TypeDef   bno055_id_inf;
 BNO055_EULER_TypeDef    euler_angles;
 BNO055_VEL_TypeDef      velocity;  
-double Kbt =  -67.5155;//-65.9945; //-72.4921;//-67.9056;//-92.4921;//-89.9276;
-double Kbv = -9.3610;//-9.2202; //-9.9672;//-11.4004;//-13.8353;//-9.9672; //-14.9398;
-double Kwv = -0.0082; //-0.1164; //-0.00025; //-0.02;//
+double Kbt =  -67.5155;
+double Kbv = -9.3610;
+double Kwv = -0.0082; 
 double speed;
 double wv;
 double bt;
@@ -42,14 +42,14 @@
 void pwmupdate()
 {
 
-    //myled = !myled;
-    speed = I1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
-    wv = (speed-0.5)*(5000/0.5); // Scale the velocity to rad/s
+   
+    speed = I1.read();              // Converts and read the analog input value (value from 0.0 to 1.0)
+    wv = (speed-0.5)*(5000/0.5);    // Scale the velocity to rad/s
 
-    bt = ((euler_angles.p) + (pi/4));
-    bv = -1.0*velocity.z;
+    bt = ((euler_angles.p) + (pi/4));       //Read body angle
+    bv = -1.0*velocity.z;                   //Read boady angle velocity
     
-    r1 = -1.0*(Kbt*bt + Kbv*bv + Kwv*wv);
+    r1 = -1.0*(Kbt*bt + Kbv*bv + Kwv*wv);   //Calculate current setpoint
 
     
     //Limit PWM range
@@ -61,9 +61,12 @@
         r1 = -6.0;
     }
         
-    r1 = ((.4*(r1/6.0)) + 0.5) ;              //Normalize for PWM output
+    r1 = ((.4*(r1/6.0)) + 0.5) ;                //Normalize for PWM output
     
-    P1 = r1;
+    P1 = r1;                                    //
+    
+    
+    //Limit angle to prevent output railing
     
     if (bt > (pi/6)){
         EN1 = 0;
@@ -79,8 +82,11 @@
 
 void eventFunction()
 {
-
+    //Button press routine
+    
+    
     if(!isPressed) {
+        //Enable closed loop mode, enable motor drivers
         pwmint.attach(&pwmupdate, .005);
         EN1 = 1;
         myled = 1;
@@ -89,13 +95,13 @@
     } 
     
     else {
+        //Disable closed loop mode
         pwmint.detach();
         P1 = 0.5;
         bt = 0.0;
         myled = 0;
         EN1 = 0;
-        //P2 = 0;
-        //P3 = 0;
+
         isPressed=0;
     }
 }
@@ -104,13 +110,16 @@
 {
     wait_us(2000000);
     pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
+    pc.printf("Kbt: %+6.4f, Kbv: %+6.4f, Kwv: %+6.4f\r\n", Kbt,Kbv,Kwv);
     if (imu.chip_ready() == 0) {
         pc.printf("Bosch BNO055 is NOT available!!\r\n");
     }
 
     imu.read_id_inf(&bno055_id_inf);
+    
     P1 = .5;        //Stops ESCON studio from throwing out-of-range error
     
+    
     //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);
@@ -125,7 +134,7 @@
     while(1) {
 
 
-       // pc.printf("H:%+6.4f [rad], P:%+6.4f, R:%+6.4f, THETA%+6.4f [PWM], Theta_dot:%+6.4f [rad/s] , WV:%+6.4f [rad/s] \r\n",
+        // pc.printf("H:%+6.4f [rad], P:%+6.4f, R:%+6.4f, THETA%+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, bt, 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",
@@ -134,7 +143,7 @@
         imu.get_Euler_Angles(&euler_angles);
         imu.get_velocities(&velocity);
         
-        bt = ((euler_angles.p) + (pi/4));
+        //bt = ((euler_angles.p) + (pi/4));
         //bv = -1.0*velocity.z;
         //wait(0.2);
     }