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
Revision 29:37dc63b57d6e, committed 2017-04-18
- Comitter:
- tomras12
- Date:
- Tue Apr 18 21:13:06 2017 +0000
- Parent:
- 28:b813a8f685c3
- Commit message:
- Added button debounce and equilibrium angle setpoint on press
Changed in this revision
diff -r b813a8f685c3 -r 37dc63b57d6e cube.cpp --- a/cube.cpp Tue Apr 18 19:58:56 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,14 +0,0 @@ -/* - * cube.h - * April 11, 2017 - * - * Control software for balancing cube senior design project - * - * Will Church - * Tom Rasmussen - */ - - #include "cube.h" - - - \ No newline at end of file
diff -r b813a8f685c3 -r 37dc63b57d6e cube.h --- a/cube.h Tue Apr 18 19:58:56 2017 +0000 +++ b/cube.h Tue Apr 18 21:13:06 2017 +0000 @@ -28,27 +28,62 @@ char *descr; // Description of config }; +/* -- GLOBALS -- */ +Ticker pwmint; // Button interrupt +BNO055_EULER_TypeDef euler_angles; +BNO055_VEL_TypeDef velocity; +config *currentConfig; // Stores current config +bool isActive; // Control loop bool + /* -- CONSTANTS -- */ -// 1D gains, other wheels detached -/*const double Kbt = -89.9276; -const double Kbv = -14.9398; -const double Kwv = -0.001; //-0.0909; +const double pi = 3.14159265; +const float TM = .25; //threshold for main axis +const float TA = .2; //threshold for aux axis -1d gains, others wheels attached -const double Kbt_x = 0; -const double Kbv_x = 0; -const double Kwv_x = 0; +/* -- CONFIGS -- */ +/* 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 -const double Kbt_y = 0; -const double Kbv_y = 0; -const double Kwv_y = 0; +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 -const double Kbt_z = 0; -const double Kbv_z = 0; -const double Kwv_z = 0;*/ +struct config fall = {0, //Kbt + 0, //Kbv + 0, //Kwv + 0, //eqAngle + &(euler_angles.p), //angle + &(velocity.z), //vel + NULL, //pwm + NULL, //hall + "Fallen"}; //descr // Other constants -const double pi = 3.14159265; + /* -- NOTES -- */ //------------------------------------
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;