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
main.cpp@23:abe76b7c377a, 2017-04-12 (annotated)
- Committer:
- tomras12
- Date:
- Wed Apr 12 00:29:45 2017 +0000
- Revision:
- 23:abe76b7c377a
- Parent:
- 22:9f3022fe9084
- Child:
- 24:c7b3bac429c5
Refactor to make code more adaptable
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tomras12 | 23:abe76b7c377a | 1 | /* |
tomras12 | 23:abe76b7c377a | 2 | * main.cpp |
tomras12 | 23:abe76b7c377a | 3 | * April 11, 2017 |
tomras12 | 23:abe76b7c377a | 4 | * |
tomras12 | 23:abe76b7c377a | 5 | * Control software for balancing cube senior design project |
tomras12 | 23:abe76b7c377a | 6 | * |
tomras12 | 23:abe76b7c377a | 7 | * Will Church |
tomras12 | 23:abe76b7c377a | 8 | * Tom Rasmussen |
tomras12 | 23:abe76b7c377a | 9 | */ |
wchurch | 0:604ceafb7bb3 | 10 | |
tomras12 | 23:abe76b7c377a | 11 | #include "cube.h" |
wchurch | 0:604ceafb7bb3 | 12 | |
tomras12 | 23:abe76b7c377a | 13 | /* Initialize general I/O */ |
wchurch | 0:604ceafb7bb3 | 14 | DigitalOut myled(LED1); |
wchurch | 6:f2c930a90873 | 15 | InterruptIn button(USER_BUTTON); |
tomras12 | 23:abe76b7c377a | 16 | Serial pc(SERIAL_TX, SERIAL_RX); // Serial connection to pc |
tomras12 | 23:abe76b7c377a | 17 | DigitalOut EN1(D0); // Interrupt |
tomras12 | 23:abe76b7c377a | 18 | I2C i2c(PB_9, PB_8); // SDA, SCL |
tomras12 | 23:abe76b7c377a | 19 | BNO055 imu(i2c, PA_8); // Reset |
wchurch | 9:6a83e2777d24 | 20 | |
tomras12 | 23:abe76b7c377a | 21 | /* -- GLOBALS -- */ |
tomras12 | 23:abe76b7c377a | 22 | Ticker pwmint; // Button interrupt |
wchurch | 18:6f120b374991 | 23 | |
wchurch | 18:6f120b374991 | 24 | BNO055_ID_INF_TypeDef bno055_id_inf; |
wchurch | 18:6f120b374991 | 25 | BNO055_EULER_TypeDef euler_angles; |
tomras12 | 23:abe76b7c377a | 26 | BNO055_VEL_TypeDef velocity; |
wchurch | 18:6f120b374991 | 27 | |
tomras12 | 23:abe76b7c377a | 28 | config *currentConfig; // Stores current config |
tomras12 | 23:abe76b7c377a | 29 | bool isActive; // Control loop bool |
wchurch | 18:6f120b374991 | 30 | |
tomras12 | 23:abe76b7c377a | 31 | /* Define our parameters here for each balancing configuration */ |
tomras12 | 23:abe76b7c377a | 32 | struct config justZ = {-89.9276, //Kbt |
tomras12 | 23:abe76b7c377a | 33 | -14.9398, //Kbv |
tomras12 | 23:abe76b7c377a | 34 | -0.001, //Kwv |
tomras12 | 23:abe76b7c377a | 35 | pi/4.0, //eqAngle |
tomras12 | 23:abe76b7c377a | 36 | &(euler_angles.p), //angle |
tomras12 | 23:abe76b7c377a | 37 | &(velocity.z), //vel |
tomras12 | 23:abe76b7c377a | 38 | new PwmOut(PE_9), //pwm |
tomras12 | 23:abe76b7c377a | 39 | new AnalogIn(A0)}; //hall |
wchurch | 18:6f120b374991 | 40 | |
tomras12 | 23:abe76b7c377a | 41 | void controlLoop() { |
tomras12 | 23:abe76b7c377a | 42 | // Detect the current orientation |
tomras12 | 23:abe76b7c377a | 43 | // For now just hardcode it to 'justZ' |
tomras12 | 23:abe76b7c377a | 44 | currentConfig = &justZ; |
tomras12 | 23:abe76b7c377a | 45 | updatePWM(currentConfig); |
wchurch | 18:6f120b374991 | 46 | } |
wchurch | 18:6f120b374991 | 47 | |
tomras12 | 23:abe76b7c377a | 48 | void onButtonPress() |
wchurch | 18:6f120b374991 | 49 | { |
tomras12 | 23:abe76b7c377a | 50 | // Activate control loop if not active |
tomras12 | 23:abe76b7c377a | 51 | if(!isActive) { |
tomras12 | 23:abe76b7c377a | 52 | pwmint.attach(&controlLoop, .005); |
wchurch | 18:6f120b374991 | 53 | EN1 = 1; |
wchurch | 19:3118e8e60182 | 54 | myled = 1; |
tomras12 | 23:abe76b7c377a | 55 | isActive = true; |
wchurch | 22:9f3022fe9084 | 56 | } |
wchurch | 22:9f3022fe9084 | 57 | |
wchurch | 22:9f3022fe9084 | 58 | else { |
wchurch | 6:f2c930a90873 | 59 | pwmint.detach(); |
tomras12 | 23:abe76b7c377a | 60 | currentConfig->pwm->write(0.5); |
tomras12 | 23:abe76b7c377a | 61 | //bt = 0.0; |
wchurch | 8:1011786787a4 | 62 | myled = 0; |
wchurch | 9:6a83e2777d24 | 63 | EN1 = 0; |
wchurch | 7:1be7e6735fe2 | 64 | //P2 = 0; |
wchurch | 7:1be7e6735fe2 | 65 | //P3 = 0; |
tomras12 | 23:abe76b7c377a | 66 | isActive = false; |
wchurch | 6:f2c930a90873 | 67 | } |
wchurch | 6:f2c930a90873 | 68 | } |
wchurch | 6:f2c930a90873 | 69 | |
tomras12 | 23:abe76b7c377a | 70 | /* |
tomras12 | 23:abe76b7c377a | 71 | * TODO: Documentation |
tomras12 | 23:abe76b7c377a | 72 | */ |
wchurch | 6:f2c930a90873 | 73 | int main() |
wchurch | 6:f2c930a90873 | 74 | { |
tomras12 | 23:abe76b7c377a | 75 | pc.printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n"); |
wchurch | 18:6f120b374991 | 76 | if (imu.chip_ready() == 0) { |
wchurch | 4:ae9e664301dd | 77 | pc.printf("Bosch BNO055 is NOT available!!\r\n"); |
wchurch | 0:604ceafb7bb3 | 78 | } |
wchurch | 0:604ceafb7bb3 | 79 | imu.read_id_inf(&bno055_id_inf); |
tomras12 | 23:abe76b7c377a | 80 | |
tomras12 | 23:abe76b7c377a | 81 | // Initialize pwm to 0 torque |
tomras12 | 23:abe76b7c377a | 82 | justZ.pwm->write(0.5); //Stops ESCON studio from throwing out-of-range |
tomras12 | 23:abe76b7c377a | 83 | // error |
tomras12 | 23:abe76b7c377a | 84 | |
tomras12 | 23:abe76b7c377a | 85 | // Set frequency of PWMs |
tomras12 | 23:abe76b7c377a | 86 | justZ.pwm->period(0.0002); // set pwm frequency |
wchurch | 21:4f60d60b2e5a | 87 | |
wchurch | 7:1be7e6735fe2 | 88 | //pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n", |
wchurch | 7:1be7e6735fe2 | 89 | // bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, |
wchurch | 7:1be7e6735fe2 | 90 | // bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); |
wchurch | 18:6f120b374991 | 91 | |
wchurch | 18:6f120b374991 | 92 | |
tomras12 | 23:abe76b7c377a | 93 | |
tomras12 | 23:abe76b7c377a | 94 | // Attach the button interrupt to the callback function |
tomras12 | 23:abe76b7c377a | 95 | // This button toggles if the loop is enabled |
tomras12 | 23:abe76b7c377a | 96 | isActive= false; |
tomras12 | 23:abe76b7c377a | 97 | button.rise(&onButtonPress); |
wchurch | 18:6f120b374991 | 98 | |
wchurch | 0:604ceafb7bb3 | 99 | while(1) { |
wchurch | 6:f2c930a90873 | 100 | |
wchurch | 18:6f120b374991 | 101 | // 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", |
wchurch | 18:6f120b374991 | 102 | // euler_angles.h, euler_angles.p, euler_angles.r, r1, velocity.z, wv); |
wchurch | 18:6f120b374991 | 103 | |
wchurch | 21:4f60d60b2e5a | 104 | //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", |
wchurch | 21:4f60d60b2e5a | 105 | // bt, euler_angles.p, r1, velocity.z, wv); |
wchurch | 18:6f120b374991 | 106 | |
wchurch | 6:f2c930a90873 | 107 | imu.get_Euler_Angles(&euler_angles); |
wchurch | 15:1d21cf90cd47 | 108 | imu.get_velocities(&velocity); |
wchurch | 18:6f120b374991 | 109 | |
wchurch | 8:1011786787a4 | 110 | //wait(0.2); |
wchurch | 0:604ceafb7bb3 | 111 | } |
wchurch | 0:604ceafb7bb3 | 112 | } |