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:
- 24:c7b3bac429c5
- Parent:
- 23:abe76b7c377a
- Child:
- 25:41abce345a12
diff -r abe76b7c377a -r c7b3bac429c5 main.cpp --- a/main.cpp Wed Apr 12 00:29:45 2017 +0000 +++ b/main.cpp Wed Apr 12 20:54:52 2017 +0000 @@ -13,15 +13,14 @@ /* Initialize general I/O */ DigitalOut myled(LED1); InterruptIn button(USER_BUTTON); -Serial pc(SERIAL_TX, SERIAL_RX); // Serial connection to pc +Serial *pc; // Serial connection to pc DigitalOut EN1(D0); // Interrupt I2C i2c(PB_9, PB_8); // SDA, SCL -BNO055 imu(i2c, PA_8); // Reset +BNO055 *imu; // IMU /* -- GLOBALS -- */ Ticker pwmint; // Button interrupt -BNO055_ID_INF_TypeDef bno055_id_inf; BNO055_EULER_TypeDef euler_angles; BNO055_VEL_TypeDef velocity; @@ -29,20 +28,67 @@ bool isActive; // Control loop bool /* Define our parameters here for each balancing configuration */ -struct config justZ = {-89.9276, //Kbt +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 + 0.8300, //eqAngle &(euler_angles.p), //angle &(velocity.z), //vel new PwmOut(PE_9), //pwm - new AnalogIn(A0)}; //hall + 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 - // For now just hardcode it to 'justZ' - currentConfig = &justZ; - updatePWM(currentConfig); + detectOrientation(); + pc->printf("%s\r\n", currentConfig->descr); + + // Update the motor torque + //updatePWM(currentConfig); } void onButtonPress() @@ -64,6 +110,7 @@ //P2 = 0; //P3 = 0; isActive = false; + pc->printf("Push putton to begin loop"); } } @@ -72,40 +119,51 @@ */ int main() { - pc.printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n"); - if (imu.chip_ready() == 0) { - pc.printf("Bosch BNO055 is NOT available!!\r\n"); + // 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"); } - imu.read_id_inf(&bno055_id_inf); // Initialize pwm to 0 torque - justZ.pwm->write(0.5); //Stops ESCON studio from throwing out-of-range + balZ.pwm->write(0.5); //Stops ESCON studio from throwing out-of-range // error // Set frequency of PWMs - justZ.pwm->period(0.0002); // set pwm frequency - - //pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n", - // 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); - - + 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", 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)); - // 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", + // 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", + //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); + imu->get_Euler_Angles(&euler_angles); + imu->get_velocities(&velocity); //wait(0.2); }