Dependencies: BNO055_fusion_tom FastPWM mbed
Fork of NucleoCube1 by
main.cpp
- Committer:
- tomras12
- Date:
- 2017-04-13
- Revision:
- 25:41abce345a12
- Parent:
- 24:c7b3bac429c5
- Child:
- 26:f2bb916262c9
File content as of revision 25:41abce345a12:
/* * main.cpp * April 11, 2017 * * Control software for balancing cube senior design project * * Will Church * Tom Rasmussen */ #include "cube.h" /* Initialize general I/O */ DigitalOut myled(LED1); InterruptIn button(USER_BUTTON); Serial *pc; // Serial connection to pc DigitalOut EN1(D0); // Interrupt 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 = {-89.9276, //Kbt -14.9398, //Kbv -0.001, //Kwv -pi/4, //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;} else {currentConfig = &fall;} } void controlLoop() { // Detect the current orientation // Update the motor torque //updatePWM(currentConfig); } void onButtonPress() { wait_ms(10); // Activate control loop if not active if(!isActive) { pwmint.attach(&controlLoop, .005); EN1 = 1; myled = 1; isActive = true; } else { pwmint.detach(); currentConfig->pwm->write(0.5); //bt = 0.0; myled = 0; EN1 = 0; //P2 = 0; //P3 = 0; isActive = false; pc->printf("Push putton to begin loop"); } } /* * TODO: Documentation */ int main() { // Initialize serial pc = new Serial(SERIAL_TX, SERIAL_RX); // Initialize IMU pc->printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n"); wait_ms(1000); // Allows IMU time to power up pc->printf("Initializing IMU...\r\n"); imu = new BNO055(i2c, PA_8); // Init IMU to i2c and reset pin while (imu->chip_ready() == 0) { pc->printf("Bosch BNO055 is NOT available!!\r\n"); } // Initialize pwm to 0 torque balZ.pwm->write(0.5); //Stops ESCON studio from throwing out-of-range // error // Set frequency of PWMs balZ.pwm->period(0.0002); // set pwm frequency // Attach the button interrupt to the callback function // This button toggles if the loop is enabled isActive= false; button.rise(&onButtonPress); // Calibrate until sys calib is at 3 checkCalib(imu, pc); // Initialize config detectOrientation(); pc->printf("Push button to begin loop"); while(1) { //pc->printf("P:%6.4f, R:%6.4f\r\n", euler_angles.p, euler_angles.r); //pc->printf("X:%6.4f, Y:%6.4f, Z:%6.4f\r\n", velocity.x, velocity.y, velocity.z); //pc->printf("%6.4f\r\n", *(currentConfig->angle)); // cuberotate: pc->printf("Orientation: %6.4f %6.4f %6.4f\r\n", euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi); /*pc->printf("%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f\r\n", euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi, euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi);*/ // pc->printf("H:%+6.4f [rad], P:%+6.4f, R:%+6.4f, R1%+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, r1, 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, velocity.z, wv); imu->get_Euler_Angles(&euler_angles); imu->get_velocities(&velocity); detectOrientation(); pc->printf("%s\r\n", currentConfig->descr); //wait(0.2); } }