cube
Dependencies: BNO055_fusion_tom FastPWM mbed
main.cpp
- Committer:
- wchurch
- Date:
- 2017-04-22
- Revision:
- 32:e77ad4aff1fd
- Parent:
- 31:e159b0caa2ea
- Child:
- 33:2434fd576d34
File content as of revision 32:e77ad4aff1fd:
#include "mbed.h" #include "BNO055.h" //------------------------------------ // Hyperterminal configuration // 9600 bauds, 8-bit data, no parity //------------------------------------ Serial pc(SERIAL_TX, SERIAL_RX); Ticker pwmint; DigitalOut myled(LED1); InterruptIn button(USER_BUTTON); PwmOut P1(PE_9); DigitalOut EN1(D0); AnalogIn I1(A0); //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; double Kbt = -67.5155; double Kbv = -9.3610; double Kwv = -0.0082; double speed; double wv; double bt; double bv; double r1; double pi = 3.14159265; int isPressed; void pwmupdate() { 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)); //Read body angle bv = -1.0*velocity.z; //Read boady angle velocity r1 = -1.0*(Kbt*bt + Kbv*bv + Kwv*wv); //Calculate current setpoint //Limit PWM range if (r1 > 6.0) { r1 = 6.0; } if (r1 < -6.0) { r1 = -6.0; } r1 = ((.4*(r1/6.0)) + 0.5) ; //Normalize for PWM output P1 = r1; // //Limit angle to prevent output railing if (bt > (pi/6)){ EN1 = 0; pwmint.detach(); } else if (bt< -(pi/6)){ EN1 = 0; pwmint.detach(); } } void eventFunction() { //Button press routine if(!isPressed) { //Enable closed loop mode, enable motor drivers pwmint.attach(&pwmupdate, .002); EN1 = 1; myled = 1; isPressed=1; } else { //Disable closed loop mode pwmint.detach(); P1 = 0.5; bt = 0.0; myled = 0; EN1 = 0; isPressed=0; } } int main() { 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); 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, 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", // bt, euler_angles.p, r1, bv, wv); imu.get_Euler_Angles(&euler_angles); imu.get_velocities(&velocity); //bt = ((euler_angles.p) + (pi/4)); //bv = -1.0*velocity.z; //wait(0.2); } }