![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Dependencies: BNO055_fusion_tom FastPWM mbed
Fork of NucleoCube1 by
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); } }