Tom Rasmussen
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 
Tom Rasmussen
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
--- 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
--- 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 -- */
//------------------------------------
--- 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;