Dependencies: BNO055_fusion_tom FastPWM mbed
Fork of NucleoCube1 by
main.cpp@27:c3ca97f1db60, 2017-04-18 (annotated)
- Committer:
- tomras12
- Date:
- Tue Apr 18 03:15:07 2017 +0000
- Revision:
- 27:c3ca97f1db60
- Parent:
- 26:f2bb916262c9
Fixed hall sensor analog read
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 | 23:abe76b7c377a | 21 | /* -- GLOBALS -- */ |
tomras12 | 23:abe76b7c377a | 22 | Ticker pwmint; // Button interrupt |
wchurch | 18:6f120b374991 | 23 | |
wchurch | 18:6f120b374991 | 24 | BNO055_EULER_TypeDef euler_angles; |
tomras12 | 23:abe76b7c377a | 25 | BNO055_VEL_TypeDef velocity; |
wchurch | 18:6f120b374991 | 26 | |
tomras12 | 23:abe76b7c377a | 27 | config *currentConfig; // Stores current config |
tomras12 | 23:abe76b7c377a | 28 | bool isActive; // Control loop bool |
wchurch | 18:6f120b374991 | 29 | |
tomras12 | 23:abe76b7c377a | 30 | /* Define our parameters here for each balancing configuration */ |
tomras12 | 24:c7b3bac429c5 | 31 | struct config balX = {-89.9276, //Kbt |
tomras12 | 23:abe76b7c377a | 32 | -14.9398, //Kbv |
tomras12 | 23:abe76b7c377a | 33 | -0.001, //Kwv |
tomras12 | 23:abe76b7c377a | 34 | pi/4.0, //eqAngle |
tomras12 | 24:c7b3bac429c5 | 35 | &(euler_angles.r), //angle |
tomras12 | 24:c7b3bac429c5 | 36 | &(velocity.x), //vel |
tomras12 | 24:c7b3bac429c5 | 37 | new PwmOut(PE_9), //pwm |
tomras12 | 24:c7b3bac429c5 | 38 | new AnalogIn(A0), //hall |
tomras12 | 24:c7b3bac429c5 | 39 | "Balancing on X edge"}; //descr |
tomras12 | 24:c7b3bac429c5 | 40 | |
tomras12 | 24:c7b3bac429c5 | 41 | struct config balY = {0, //Kbt |
tomras12 | 24:c7b3bac429c5 | 42 | 0, //Kbv |
tomras12 | 24:c7b3bac429c5 | 43 | 0, //Kwv |
tomras12 | 25:41abce345a12 | 44 | -pi/4.0, //eqAngle |
tomras12 | 24:c7b3bac429c5 | 45 | &(euler_angles.p), //angle |
tomras12 | 24:c7b3bac429c5 | 46 | &(velocity.y), //vel |
tomras12 | 24:c7b3bac429c5 | 47 | new PwmOut(PE_9), //pwm |
tomras12 | 24:c7b3bac429c5 | 48 | new AnalogIn(A0), //hall |
tomras12 | 24:c7b3bac429c5 | 49 | "Balancing on Y edge"}; //descr |
tomras12 | 24:c7b3bac429c5 | 50 | |
tomras12 | 24:c7b3bac429c5 | 51 | struct config balZ = {-89.9276, //Kbt |
tomras12 | 24:c7b3bac429c5 | 52 | -14.9398, //Kbv |
tomras12 | 24:c7b3bac429c5 | 53 | -0.001, //Kwv |
tomras12 | 25:41abce345a12 | 54 | -pi/4, //eqAngle |
tomras12 | 23:abe76b7c377a | 55 | &(euler_angles.p), //angle |
tomras12 | 23:abe76b7c377a | 56 | &(velocity.z), //vel |
tomras12 | 23:abe76b7c377a | 57 | new PwmOut(PE_9), //pwm |
tomras12 | 24:c7b3bac429c5 | 58 | new AnalogIn(A0), //hall |
tomras12 | 24:c7b3bac429c5 | 59 | "Balancing on Z edge"}; //descr |
wchurch | 18:6f120b374991 | 60 | |
tomras12 | 24:c7b3bac429c5 | 61 | struct config fall = {0, //Kbt |
tomras12 | 24:c7b3bac429c5 | 62 | 0, //Kbv |
tomras12 | 24:c7b3bac429c5 | 63 | 0, //Kwv |
tomras12 | 24:c7b3bac429c5 | 64 | 0, //eqAngle |
tomras12 | 24:c7b3bac429c5 | 65 | &(euler_angles.p), //angle |
tomras12 | 24:c7b3bac429c5 | 66 | &(velocity.z), //vel |
tomras12 | 24:c7b3bac429c5 | 67 | NULL, //pwm |
tomras12 | 24:c7b3bac429c5 | 68 | NULL, //hall |
tomras12 | 24:c7b3bac429c5 | 69 | "Fallen"}; //descr |
tomras12 | 24:c7b3bac429c5 | 70 | |
tomras12 | 24:c7b3bac429c5 | 71 | void detectOrientation() { |
tomras12 | 24:c7b3bac429c5 | 72 | float tm = .25; //threshold for main axis |
tomras12 | 24:c7b3bac429c5 | 73 | float ta = .2; //threshold for aux axis |
tomras12 | 24:c7b3bac429c5 | 74 | |
tomras12 | 24:c7b3bac429c5 | 75 | if (abs(euler_angles.r - balX.eqAngle) < tm |
tomras12 | 24:c7b3bac429c5 | 76 | && abs(euler_angles.p) < ta) {currentConfig = &balX;} |
tomras12 | 24:c7b3bac429c5 | 77 | else if (abs(euler_angles.p - balY.eqAngle) < tm |
tomras12 | 24:c7b3bac429c5 | 78 | && abs(euler_angles.r) < ta) {currentConfig = &balY;} |
tomras12 | 24:c7b3bac429c5 | 79 | else if (abs(euler_angles.p - balZ.eqAngle) < tm |
tomras12 | 25:41abce345a12 | 80 | && abs(euler_angles.r + 1.6) < ta) {currentConfig = &balZ;} |
tomras12 | 24:c7b3bac429c5 | 81 | else {currentConfig = &fall;} |
tomras12 | 24:c7b3bac429c5 | 82 | |
tomras12 | 24:c7b3bac429c5 | 83 | } |
tomras12 | 24:c7b3bac429c5 | 84 | |
tomras12 | 23:abe76b7c377a | 85 | void controlLoop() { |
tomras12 | 23:abe76b7c377a | 86 | // Detect the current orientation |
tomras12 | 25:41abce345a12 | 87 | |
tomras12 | 26:f2bb916262c9 | 88 | detectOrientation(); |
tomras12 | 26:f2bb916262c9 | 89 | //pc->printf("%s\r\n", currentConfig->descr); |
tomras12 | 24:c7b3bac429c5 | 90 | // Update the motor torque |
tomras12 | 24:c7b3bac429c5 | 91 | //updatePWM(currentConfig); |
wchurch | 18:6f120b374991 | 92 | } |
wchurch | 18:6f120b374991 | 93 | |
tomras12 | 23:abe76b7c377a | 94 | void onButtonPress() |
wchurch | 18:6f120b374991 | 95 | { |
tomras12 | 25:41abce345a12 | 96 | wait_ms(10); |
tomras12 | 23:abe76b7c377a | 97 | // Activate control loop if not active |
tomras12 | 23:abe76b7c377a | 98 | if(!isActive) { |
tomras12 | 26:f2bb916262c9 | 99 | pwmint.attach(&controlLoop, .001); |
wchurch | 18:6f120b374991 | 100 | EN1 = 1; |
wchurch | 19:3118e8e60182 | 101 | myled = 1; |
tomras12 | 23:abe76b7c377a | 102 | isActive = true; |
tomras12 | 26:f2bb916262c9 | 103 | pc->printf("Loop now active\r\n"); |
wchurch | 22:9f3022fe9084 | 104 | } |
wchurch | 22:9f3022fe9084 | 105 | |
wchurch | 22:9f3022fe9084 | 106 | else { |
wchurch | 6:f2c930a90873 | 107 | pwmint.detach(); |
tomras12 | 26:f2bb916262c9 | 108 | //currentConfig->pwm->write(0.5); |
tomras12 | 23:abe76b7c377a | 109 | //bt = 0.0; |
wchurch | 8:1011786787a4 | 110 | myled = 0; |
wchurch | 9:6a83e2777d24 | 111 | EN1 = 0; |
wchurch | 7:1be7e6735fe2 | 112 | //P2 = 0; |
wchurch | 7:1be7e6735fe2 | 113 | //P3 = 0; |
tomras12 | 23:abe76b7c377a | 114 | isActive = false; |
tomras12 | 26:f2bb916262c9 | 115 | pc->printf("Loop now inactive\r\n"); |
wchurch | 6:f2c930a90873 | 116 | } |
wchurch | 6:f2c930a90873 | 117 | } |
wchurch | 6:f2c930a90873 | 118 | |
tomras12 | 23:abe76b7c377a | 119 | /* |
tomras12 | 23:abe76b7c377a | 120 | * TODO: Documentation |
tomras12 | 23:abe76b7c377a | 121 | */ |
wchurch | 6:f2c930a90873 | 122 | int main() |
wchurch | 6:f2c930a90873 | 123 | { |
tomras12 | 24:c7b3bac429c5 | 124 | // Initialize serial |
tomras12 | 24:c7b3bac429c5 | 125 | pc = new Serial(SERIAL_TX, SERIAL_RX); |
tomras12 | 24:c7b3bac429c5 | 126 | // Initialize IMU |
tomras12 | 24:c7b3bac429c5 | 127 | pc->printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n"); |
tomras12 | 24:c7b3bac429c5 | 128 | wait_ms(1000); // Allows IMU time to power up |
tomras12 | 24:c7b3bac429c5 | 129 | pc->printf("Initializing IMU...\r\n"); |
tomras12 | 24:c7b3bac429c5 | 130 | imu = new BNO055(i2c, PA_8); // Init IMU to i2c and reset pin |
tomras12 | 24:c7b3bac429c5 | 131 | while (imu->chip_ready() == 0) { |
tomras12 | 24:c7b3bac429c5 | 132 | pc->printf("Bosch BNO055 is NOT available!!\r\n"); |
wchurch | 0:604ceafb7bb3 | 133 | } |
tomras12 | 23:abe76b7c377a | 134 | |
tomras12 | 23:abe76b7c377a | 135 | // Initialize pwm to 0 torque |
tomras12 | 24:c7b3bac429c5 | 136 | balZ.pwm->write(0.5); //Stops ESCON studio from throwing out-of-range |
tomras12 | 23:abe76b7c377a | 137 | // error |
tomras12 | 23:abe76b7c377a | 138 | |
tomras12 | 23:abe76b7c377a | 139 | // Set frequency of PWMs |
tomras12 | 24:c7b3bac429c5 | 140 | balZ.pwm->period(0.0002); // set pwm frequency |
tomras12 | 23:abe76b7c377a | 141 | |
tomras12 | 23:abe76b7c377a | 142 | // Attach the button interrupt to the callback function |
tomras12 | 23:abe76b7c377a | 143 | // This button toggles if the loop is enabled |
tomras12 | 23:abe76b7c377a | 144 | isActive= false; |
tomras12 | 23:abe76b7c377a | 145 | button.rise(&onButtonPress); |
tomras12 | 24:c7b3bac429c5 | 146 | |
tomras12 | 24:c7b3bac429c5 | 147 | // Calibrate until sys calib is at 3 |
tomras12 | 26:f2bb916262c9 | 148 | //checkCalib(imu, pc); |
tomras12 | 24:c7b3bac429c5 | 149 | |
tomras12 | 24:c7b3bac429c5 | 150 | // Initialize config |
tomras12 | 26:f2bb916262c9 | 151 | //detectOrientation(); |
tomras12 | 24:c7b3bac429c5 | 152 | |
tomras12 | 24:c7b3bac429c5 | 153 | pc->printf("Push button to begin loop"); |
wchurch | 18:6f120b374991 | 154 | |
wchurch | 0:604ceafb7bb3 | 155 | while(1) { |
tomras12 | 24:c7b3bac429c5 | 156 | |
tomras12 | 25:41abce345a12 | 157 | //pc->printf("P:%6.4f, R:%6.4f\r\n", euler_angles.p, euler_angles.r); |
tomras12 | 24:c7b3bac429c5 | 158 | //pc->printf("X:%6.4f, Y:%6.4f, Z:%6.4f\r\n", velocity.x, velocity.y, velocity.z); |
tomras12 | 24:c7b3bac429c5 | 159 | //pc->printf("%6.4f\r\n", *(currentConfig->angle)); |
tomras12 | 25:41abce345a12 | 160 | // 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 | 26:f2bb916262c9 | 161 | pc->printf("%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f\r\n", |
tomras12 | 25:41abce345a12 | 162 | euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi, |
tomras12 | 26:f2bb916262c9 | 163 | euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi); |
tomras12 | 25:41abce345a12 | 164 | |
wchurch | 6:f2c930a90873 | 165 | |
tomras12 | 24:c7b3bac429c5 | 166 | // 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 | 167 | // euler_angles.h, euler_angles.p, euler_angles.r, r1, velocity.z, wv); |
wchurch | 18:6f120b374991 | 168 | |
tomras12 | 24:c7b3bac429c5 | 169 | //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 | 170 | // bt, euler_angles.p, r1, velocity.z, wv); |
wchurch | 18:6f120b374991 | 171 | |
tomras12 | 24:c7b3bac429c5 | 172 | imu->get_Euler_Angles(&euler_angles); |
tomras12 | 24:c7b3bac429c5 | 173 | imu->get_velocities(&velocity); |
tomras12 | 25:41abce345a12 | 174 | |
tomras12 | 26:f2bb916262c9 | 175 | |
wchurch | 18:6f120b374991 | 176 | |
wchurch | 8:1011786787a4 | 177 | //wait(0.2); |
wchurch | 0:604ceafb7bb3 | 178 | } |
wchurch | 0:604ceafb7bb3 | 179 | } |