cube
Dependencies: BNO055_fusion_tom FastPWM mbed
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);