This is the one where I went back and un-did the cube.cpp file
Dependencies: BNO055_fusion_tom FastPWM mbed
Fork of NucleoCube1 by
Diff: main.cpp
- Revision:
- 29:37dc63b57d6e
- Parent:
- 28:b813a8f685c3
diff -r b813a8f685c3 -r 37dc63b57d6e main.cpp --- a/main.cpp Tue Apr 18 19:58:56 2017 +0000 +++ b/main.cpp Tue Apr 18 21:13:06 2017 +0000 @@ -18,66 +18,15 @@ I2C i2c(PB_9, PB_8); // SDA, SCL BNO055 *imu; // IMU -/* -- GLOBALS -- */ -Ticker pwmint; // Button interrupt - -BNO055_EULER_TypeDef euler_angles; -BNO055_VEL_TypeDef velocity; - -config *currentConfig; // Stores current config -bool isActive; // Control loop bool - -/* Define our parameters here for each balancing configuration */ -struct config balX = {-89.9276, //Kbt - -14.9398, //Kbv - -0.001, //Kwv - pi/4.0, //eqAngle - &(euler_angles.r), //angle - &(velocity.x), //vel - new PwmOut(PE_9), //pwm - new AnalogIn(A0), //hall - "Balancing on X edge"}; //descr - -struct config balY = {0, //Kbt - 0, //Kbv - 0, //Kwv - -pi/4.0, //eqAngle - &(euler_angles.p), //angle - &(velocity.y), //vel - new PwmOut(PE_9), //pwm - new AnalogIn(A0), //hall - "Balancing on Y edge"}; //descr - -struct config balZ = {-72.4921, //Kbt - -9.9672, //Kbv - -0.00025, //Kwv - -.7000, //eqAngle - &(euler_angles.p), //angle - &(velocity.z), //vel - new PwmOut(PE_9), //pwm - new AnalogIn(A0), //hall - "Balancing on Z edge"}; //descr - -struct config fall = {0, //Kbt - 0, //Kbv - 0, //Kwv - 0, //eqAngle - &(euler_angles.p), //angle - &(velocity.z), //vel - NULL, //pwm - NULL, //hall - "Fallen"}; //descr void detectOrientation() { - float tm = .25; //threshold for main axis - float ta = .2; //threshold for aux axis - if (abs(euler_angles.r - balX.eqAngle) < tm - && abs(euler_angles.p) < ta) {currentConfig = &balX;} - else if (abs(euler_angles.p - balY.eqAngle) < tm - && abs(euler_angles.r) < ta) {currentConfig = &balY;} - else if (abs(euler_angles.p - balZ.eqAngle) < tm - && abs(euler_angles.r + 1.6) < ta) {currentConfig = &balZ;} + if (abs(euler_angles.r - balX.eqAngle) < TM + && abs(euler_angles.p) < TA) {currentConfig = &balX;} + else if (abs(euler_angles.p - balY.eqAngle) < TM + && abs(euler_angles.r) < TA) {currentConfig = &balY;} + else if (abs(euler_angles.p - balZ.eqAngle) < TM + && abs(euler_angles.r + 1.6) < TA) {currentConfig = &balZ;} else {currentConfig = &fall;} } @@ -101,9 +50,12 @@ void onButtonPress() { - wait_ms(10); + wait_ms(100); // Debounce button + // Activate control loop if not active if(!isActive) { + // Reset equilibrium angle to current angle + currentConfig->eqAngle = *(currentConfig->angle); pwmint.attach(&controlLoop, .01); EN1 = 1; myled = 1;