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:
- 23:abe76b7c377a
- Parent:
- 22:9f3022fe9084
- Child:
- 24:c7b3bac429c5
--- a/main.cpp Tue Apr 11 17:48:48 2017 +0000 +++ b/main.cpp Wed Apr 12 00:29:45 2017 +0000 @@ -1,130 +1,103 @@ -#include "mbed.h" -#include "BNO055.h" - +/* + * main.cpp + * April 11, 2017 + * + * Control software for balancing cube senior design project + * + * Will Church + * Tom Rasmussen + */ -//------------------------------------ -// Hyperterminal configuration -// 9600 bauds, 8-bit data, no parity -//------------------------------------ +#include "cube.h" -Serial pc(SERIAL_TX, SERIAL_RX); - -Ticker pwmint; +/* Initialize general I/O */ DigitalOut myled(LED1); InterruptIn button(USER_BUTTON); - -PwmOut P1(PE_9); -DigitalOut EN1(D0); -AnalogIn I1(A0); +Serial pc(SERIAL_TX, SERIAL_RX); // Serial connection to pc +DigitalOut EN1(D0); // Interrupt +I2C i2c(PB_9, PB_8); // SDA, SCL +BNO055 imu(i2c, PA_8); // Reset -//PwmOut P2(PE_11); 1D FOCUS FOR NOW -//PwmOut P3(PE_13); - -I2C i2c(PB_9, PB_8); // SDA, SCL -BNO055 imu(i2c, PA_8); // Reset +/* -- GLOBALS -- */ +Ticker pwmint; // Button interrupt BNO055_ID_INF_TypeDef bno055_id_inf; BNO055_EULER_TypeDef euler_angles; -BNO055_VEL_TypeDef velocity; //IN PROGRESS - -double Kbt = -89.9276; -double Kbv = -14.9398; -double Kwv = -0.001; //-0.0909; -double wv; -double bt; -double r1; - -double pi = 3.14159265; +BNO055_VEL_TypeDef velocity; -int isPressed; - -void pwmupdate() -{ - - //myled = !myled; - wv = I1.read(); // Converts and read the analog input value (value from 0.0 to 1.0) - wv = (wv-2.0)*5000.0; // Scale the velocity to rad/s - - bt = (euler_angles.p - (pi/4.0)); - - r1 = (Kbt*bt + Kbv*velocity.z + Kwv*wv); +config *currentConfig; // Stores current config +bool isActive; // Control loop bool - +/* Define our parameters here for each balancing configuration */ +struct config justZ = {-89.9276, //Kbt + -14.9398, //Kbv + -0.001, //Kwv + pi/4.0, //eqAngle + &(euler_angles.p), //angle + &(velocity.z), //vel + new PwmOut(PE_9), //pwm + new AnalogIn(A0)}; //hall - //Limit PWM range - if (r1 > 6.0) { - r1 = 6.0; - } - - if (r1 < -6.0) { - r1 = -6.0; - } - - r1 = ((.4*(r1/6.0)) + 0.5) ; //Normalize for PWM output - - P1 = r1; - //P2 = (euler_angles.r/360.0); - //P3 = (euler_angles.p/360.0); - if (bt > (pi/8)){ - EN1 = 0; - pwmint.detach(); - } - - else if (bt< -(pi/8)){ - EN1 = 0; - pwmint.detach(); - } - +void controlLoop() { + // Detect the current orientation + // For now just hardcode it to 'justZ' + currentConfig = &justZ; + updatePWM(currentConfig); } -void eventFunction() +void onButtonPress() { - - if(!isPressed) { - pwmint.attach(&pwmupdate, .005); + // Activate control loop if not active + if(!isActive) { + pwmint.attach(&controlLoop, .005); EN1 = 1; myled = 1; - isPressed=1; - + isActive = true; } else { pwmint.detach(); - P1 = 0.5; - bt = 0.0; + currentConfig->pwm->write(0.5); + //bt = 0.0; myled = 0; EN1 = 0; //P2 = 0; //P3 = 0; - isPressed=0; + isActive = false; } } +/* + * TODO: Documentation + */ int main() { - - pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n"); + pc.printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n"); if (imu.chip_ready() == 0) { pc.printf("Bosch BNO055 is NOT available!!\r\n"); } - imu.read_id_inf(&bno055_id_inf); - P1 = .5; //Stops ESCON studio from throwing out-of-range error + + // Initialize pwm to 0 torque + justZ.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); - P1.period(0.0002); //Set PWM frequency - isPressed=0; - button.rise(&eventFunction); //Enable Closed Loop - - + + // Attach the button interrupt to the callback function + // This button toggles if the loop is enabled + isActive= false; + button.rise(&onButtonPress); while(1) { - // 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);