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@29:37dc63b57d6e, 2017-04-18 (annotated)
- Committer:
- tomras12
- Date:
- Tue Apr 18 21:13:06 2017 +0000
- Revision:
- 29:37dc63b57d6e
- Parent:
- 28:b813a8f685c3
Added button debounce and equilibrium angle setpoint on press
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 | 24:c7b3bac429c5 | 16 | Serial *pc; // 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 | 24:c7b3bac429c5 | 19 | BNO055 *imu; // IMU |
wchurch | 9:6a83e2777d24 | 20 | |
tomras12 | 24:c7b3bac429c5 | 21 | |
tomras12 | 24:c7b3bac429c5 | 22 | void detectOrientation() { |
tomras12 | 24:c7b3bac429c5 | 23 | |
tomras12 | 29:37dc63b57d6e | 24 | if (abs(euler_angles.r - balX.eqAngle) < TM |
tomras12 | 29:37dc63b57d6e | 25 | && abs(euler_angles.p) < TA) {currentConfig = &balX;} |
tomras12 | 29:37dc63b57d6e | 26 | else if (abs(euler_angles.p - balY.eqAngle) < TM |
tomras12 | 29:37dc63b57d6e | 27 | && abs(euler_angles.r) < TA) {currentConfig = &balY;} |
tomras12 | 29:37dc63b57d6e | 28 | else if (abs(euler_angles.p - balZ.eqAngle) < TM |
tomras12 | 29:37dc63b57d6e | 29 | && abs(euler_angles.r + 1.6) < TA) {currentConfig = &balZ;} |
tomras12 | 24:c7b3bac429c5 | 30 | else {currentConfig = &fall;} |
tomras12 | 24:c7b3bac429c5 | 31 | |
tomras12 | 24:c7b3bac429c5 | 32 | } |
tomras12 | 24:c7b3bac429c5 | 33 | |
tomras12 | 23:abe76b7c377a | 34 | void controlLoop() { |
tomras12 | 24:c7b3bac429c5 | 35 | // Update the motor torque |
tomras12 | 28:b813a8f685c3 | 36 | if(currentConfig->Kbt == 0) { // detect fallen state |
tomras12 | 28:b813a8f685c3 | 37 | pwmint.detach(); |
tomras12 | 28:b813a8f685c3 | 38 | //currentConfig->pwm->write(0.5); |
tomras12 | 28:b813a8f685c3 | 39 | //bt = 0.0; |
tomras12 | 28:b813a8f685c3 | 40 | myled = 0; |
tomras12 | 28:b813a8f685c3 | 41 | EN1 = 0; |
tomras12 | 28:b813a8f685c3 | 42 | //P2 = 0; |
tomras12 | 28:b813a8f685c3 | 43 | //P3 = 0; |
tomras12 | 28:b813a8f685c3 | 44 | isActive = false; |
tomras12 | 28:b813a8f685c3 | 45 | pc->printf("Loop now inactive\r\n"); |
tomras12 | 28:b813a8f685c3 | 46 | } else { |
tomras12 | 28:b813a8f685c3 | 47 | updatePWM(currentConfig); |
tomras12 | 28:b813a8f685c3 | 48 | } |
wchurch | 18:6f120b374991 | 49 | } |
wchurch | 18:6f120b374991 | 50 | |
tomras12 | 23:abe76b7c377a | 51 | void onButtonPress() |
wchurch | 18:6f120b374991 | 52 | { |
tomras12 | 29:37dc63b57d6e | 53 | wait_ms(100); // Debounce button |
tomras12 | 29:37dc63b57d6e | 54 | |
tomras12 | 23:abe76b7c377a | 55 | // Activate control loop if not active |
tomras12 | 23:abe76b7c377a | 56 | if(!isActive) { |
tomras12 | 29:37dc63b57d6e | 57 | // Reset equilibrium angle to current angle |
tomras12 | 29:37dc63b57d6e | 58 | currentConfig->eqAngle = *(currentConfig->angle); |
tomras12 | 28:b813a8f685c3 | 59 | pwmint.attach(&controlLoop, .01); |
wchurch | 18:6f120b374991 | 60 | EN1 = 1; |
wchurch | 19:3118e8e60182 | 61 | myled = 1; |
tomras12 | 23:abe76b7c377a | 62 | isActive = true; |
tomras12 | 26:f2bb916262c9 | 63 | pc->printf("Loop now active\r\n"); |
wchurch | 22:9f3022fe9084 | 64 | } |
wchurch | 22:9f3022fe9084 | 65 | |
wchurch | 22:9f3022fe9084 | 66 | else { |
wchurch | 6:f2c930a90873 | 67 | pwmint.detach(); |
tomras12 | 26:f2bb916262c9 | 68 | //currentConfig->pwm->write(0.5); |
tomras12 | 23:abe76b7c377a | 69 | //bt = 0.0; |
wchurch | 8:1011786787a4 | 70 | myled = 0; |
wchurch | 9:6a83e2777d24 | 71 | EN1 = 0; |
wchurch | 7:1be7e6735fe2 | 72 | //P2 = 0; |
wchurch | 7:1be7e6735fe2 | 73 | //P3 = 0; |
tomras12 | 23:abe76b7c377a | 74 | isActive = false; |
tomras12 | 26:f2bb916262c9 | 75 | pc->printf("Loop now inactive\r\n"); |
wchurch | 6:f2c930a90873 | 76 | } |
wchurch | 6:f2c930a90873 | 77 | } |
wchurch | 6:f2c930a90873 | 78 | |
tomras12 | 28:b813a8f685c3 | 79 | void checkCalib(BNO055 *imu, Serial *pc) { |
tomras12 | 28:b813a8f685c3 | 80 | pc->printf("Checking calibration status...\r\n"); |
tomras12 | 28:b813a8f685c3 | 81 | int stat = imu->read_calib_status(); |
tomras12 | 28:b813a8f685c3 | 82 | while(stat < 192) { |
tomras12 | 28:b813a8f685c3 | 83 | pc->printf("Sys:%d Gyr:%d Acc:%d Mag:%d\r\n", |
tomras12 | 28:b813a8f685c3 | 84 | (stat >> 6) & 0x03, |
tomras12 | 28:b813a8f685c3 | 85 | (stat >> 4) & 0x03, |
tomras12 | 28:b813a8f685c3 | 86 | (stat >> 2) & 0x03, |
tomras12 | 28:b813a8f685c3 | 87 | stat & 0x03); |
tomras12 | 28:b813a8f685c3 | 88 | wait_ms(250); |
tomras12 | 28:b813a8f685c3 | 89 | stat = imu->read_calib_status(); |
tomras12 | 28:b813a8f685c3 | 90 | } |
tomras12 | 28:b813a8f685c3 | 91 | pc->printf("Looks good buddy, put the cube down now.\r\n"); |
tomras12 | 28:b813a8f685c3 | 92 | wait(2); |
tomras12 | 28:b813a8f685c3 | 93 | } |
tomras12 | 28:b813a8f685c3 | 94 | |
tomras12 | 28:b813a8f685c3 | 95 | /* |
tomras12 | 28:b813a8f685c3 | 96 | * Returns PWM duty cycle based on: |
tomras12 | 28:b813a8f685c3 | 97 | * - wv wheel velocity |
tomras12 | 28:b813a8f685c3 | 98 | * - ae angle error |
tomras12 | 28:b813a8f685c3 | 99 | * - bv body velocity |
tomras12 | 28:b813a8f685c3 | 100 | * - Kwv wheel vel gain |
tomras12 | 28:b813a8f685c3 | 101 | * - Kbt angle gain |
tomras12 | 28:b813a8f685c3 | 102 | * - Kbv body vel gain |
tomras12 | 28:b813a8f685c3 | 103 | */ |
tomras12 | 28:b813a8f685c3 | 104 | double calcPWM(config *c) |
tomras12 | 28:b813a8f685c3 | 105 | { |
tomras12 | 28:b813a8f685c3 | 106 | // Converts and read the analog input value (value from 0.0 to 1.0): |
tomras12 | 28:b813a8f685c3 | 107 | double wv = c->hall->read(); |
tomras12 | 28:b813a8f685c3 | 108 | wv = (wv - 1.65) * 5000.0 / 1.65; // Scale the velocity to rad/s |
tomras12 | 28:b813a8f685c3 | 109 | |
tomras12 | 28:b813a8f685c3 | 110 | double bt = (*(c->angle) - c->eqAngle); |
tomras12 | 28:b813a8f685c3 | 111 | |
tomras12 | 28:b813a8f685c3 | 112 | double r1 = -(c->Kbt * bt + c->Kbv * -1.0 * (*(c->vel)) + c->Kwv * wv); |
tomras12 | 28:b813a8f685c3 | 113 | |
tomras12 | 28:b813a8f685c3 | 114 | //Limit PWM range |
tomras12 | 28:b813a8f685c3 | 115 | if (r1 > 6.0) {r1 = 6.0;} |
tomras12 | 28:b813a8f685c3 | 116 | else if (r1 < -6.0) {r1 = -6.0;} |
tomras12 | 28:b813a8f685c3 | 117 | |
tomras12 | 28:b813a8f685c3 | 118 | // Normalize for PWM output |
tomras12 | 28:b813a8f685c3 | 119 | r1 = ((.4*(r1/6.0)) + 0.5); |
tomras12 | 28:b813a8f685c3 | 120 | |
tomras12 | 28:b813a8f685c3 | 121 | // Check if cube is too far tilted and send 0 torque |
tomras12 | 28:b813a8f685c3 | 122 | // May be redundant, check outer program |
tomras12 | 28:b813a8f685c3 | 123 | /*if (bt > (pi/8) || bt < -(pi/8)){ |
tomras12 | 28:b813a8f685c3 | 124 | return .5; |
tomras12 | 28:b813a8f685c3 | 125 | }*/ |
tomras12 | 28:b813a8f685c3 | 126 | return r1; |
tomras12 | 28:b813a8f685c3 | 127 | } |
tomras12 | 28:b813a8f685c3 | 128 | |
tomras12 | 28:b813a8f685c3 | 129 | void updatePWM(config *c) { |
tomras12 | 28:b813a8f685c3 | 130 | |
tomras12 | 28:b813a8f685c3 | 131 | c->pwm->write(calcPWM(c)); |
tomras12 | 28:b813a8f685c3 | 132 | //pc->printf("%6.4f\r\n", calcPWM(c)); |
tomras12 | 28:b813a8f685c3 | 133 | } |
tomras12 | 28:b813a8f685c3 | 134 | |
tomras12 | 23:abe76b7c377a | 135 | /* |
tomras12 | 23:abe76b7c377a | 136 | * TODO: Documentation |
tomras12 | 23:abe76b7c377a | 137 | */ |
wchurch | 6:f2c930a90873 | 138 | int main() |
wchurch | 6:f2c930a90873 | 139 | { |
tomras12 | 24:c7b3bac429c5 | 140 | // Initialize serial |
tomras12 | 24:c7b3bac429c5 | 141 | pc = new Serial(SERIAL_TX, SERIAL_RX); |
tomras12 | 24:c7b3bac429c5 | 142 | // Initialize IMU |
tomras12 | 24:c7b3bac429c5 | 143 | pc->printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n"); |
tomras12 | 24:c7b3bac429c5 | 144 | wait_ms(1000); // Allows IMU time to power up |
tomras12 | 24:c7b3bac429c5 | 145 | pc->printf("Initializing IMU...\r\n"); |
tomras12 | 24:c7b3bac429c5 | 146 | imu = new BNO055(i2c, PA_8); // Init IMU to i2c and reset pin |
tomras12 | 24:c7b3bac429c5 | 147 | while (imu->chip_ready() == 0) { |
tomras12 | 24:c7b3bac429c5 | 148 | pc->printf("Bosch BNO055 is NOT available!!\r\n"); |
wchurch | 0:604ceafb7bb3 | 149 | } |
tomras12 | 23:abe76b7c377a | 150 | |
tomras12 | 23:abe76b7c377a | 151 | // Initialize pwm to 0 torque |
tomras12 | 24:c7b3bac429c5 | 152 | balZ.pwm->write(0.5); //Stops ESCON studio from throwing out-of-range |
tomras12 | 23:abe76b7c377a | 153 | // error |
tomras12 | 23:abe76b7c377a | 154 | |
tomras12 | 23:abe76b7c377a | 155 | // Set frequency of PWMs |
tomras12 | 24:c7b3bac429c5 | 156 | balZ.pwm->period(0.0002); // set pwm frequency |
tomras12 | 23:abe76b7c377a | 157 | |
tomras12 | 23:abe76b7c377a | 158 | // Attach the button interrupt to the callback function |
tomras12 | 23:abe76b7c377a | 159 | // This button toggles if the loop is enabled |
tomras12 | 23:abe76b7c377a | 160 | isActive= false; |
tomras12 | 23:abe76b7c377a | 161 | button.rise(&onButtonPress); |
tomras12 | 24:c7b3bac429c5 | 162 | |
tomras12 | 24:c7b3bac429c5 | 163 | // Calibrate until sys calib is at 3 |
tomras12 | 26:f2bb916262c9 | 164 | //checkCalib(imu, pc); |
tomras12 | 24:c7b3bac429c5 | 165 | |
tomras12 | 24:c7b3bac429c5 | 166 | // Initialize config |
tomras12 | 28:b813a8f685c3 | 167 | detectOrientation(); |
tomras12 | 24:c7b3bac429c5 | 168 | |
tomras12 | 24:c7b3bac429c5 | 169 | pc->printf("Push button to begin loop"); |
wchurch | 18:6f120b374991 | 170 | |
wchurch | 0:604ceafb7bb3 | 171 | while(1) { |
tomras12 | 24:c7b3bac429c5 | 172 | |
tomras12 | 28:b813a8f685c3 | 173 | imu->get_Euler_Angles(&euler_angles); |
tomras12 | 28:b813a8f685c3 | 174 | imu->get_velocities(&velocity); |
tomras12 | 28:b813a8f685c3 | 175 | detectOrientation(); |
tomras12 | 28:b813a8f685c3 | 176 | |
tomras12 | 25:41abce345a12 | 177 | //pc->printf("P:%6.4f, R:%6.4f\r\n", euler_angles.p, euler_angles.r); |
tomras12 | 24:c7b3bac429c5 | 178 | //pc->printf("X:%6.4f, Y:%6.4f, Z:%6.4f\r\n", velocity.x, velocity.y, velocity.z); |
tomras12 | 28:b813a8f685c3 | 179 | //pc->printf("%s\r\n", (currentConfig->descr)); |
tomras12 | 28:b813a8f685c3 | 180 | pc->printf("%6.4f\r\n", *(currentConfig->angle)); |
tomras12 | 25:41abce345a12 | 181 | // 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); |
tomras12 | 28:b813a8f685c3 | 182 | /*pc->printf("%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f\r\n", |
tomras12 | 25:41abce345a12 | 183 | euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi, |
tomras12 | 28:b813a8f685c3 | 184 | euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi);*/ |
tomras12 | 25:41abce345a12 | 185 | |
wchurch | 6:f2c930a90873 | 186 | |
tomras12 | 24:c7b3bac429c5 | 187 | // 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 | 188 | // euler_angles.h, euler_angles.p, euler_angles.r, r1, velocity.z, wv); |
wchurch | 18:6f120b374991 | 189 | |
tomras12 | 24:c7b3bac429c5 | 190 | //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 | 191 | // bt, euler_angles.p, r1, velocity.z, wv); |
wchurch | 18:6f120b374991 | 192 | |
wchurch | 8:1011786787a4 | 193 | //wait(0.2); |
wchurch | 0:604ceafb7bb3 | 194 | } |
wchurch | 0:604ceafb7bb3 | 195 | } |