Tom Rasmussen / Mbed 2 deprecated NucleoCubeFork1

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 Tom Rasmussen

main.cpp@28:b813a8f685c3, 2017-04-18 (annotated)

Committer:
tomras12
Date:
Tue Apr 18 19:58:56 2017 +0000
Revision:
28:b813a8f685c3
Parent:
26:f2bb916262c9
Child:
29:37dc63b57d6e
Fixed equilibrium angle

Who changed what in which revision?

UserRevisionLine numberNew 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 28:b813a8f685c3 51 struct config balZ = {-72.4921, //Kbt
tomras12 28:b813a8f685c3 52 -9.9672, //Kbv
tomras12 28:b813a8f685c3 53 -0.00025, //Kwv
tomras12 28:b813a8f685c3 54 -.7000, //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 24:c7b3bac429c5 86 // Update the motor torque
tomras12 28:b813a8f685c3 87 if(currentConfig->Kbt == 0) { // detect fallen state
tomras12 28:b813a8f685c3 88 pwmint.detach();
tomras12 28:b813a8f685c3 89 //currentConfig->pwm->write(0.5);
tomras12 28:b813a8f685c3 90 //bt = 0.0;
tomras12 28:b813a8f685c3 91 myled = 0;
tomras12 28:b813a8f685c3 92 EN1 = 0;
tomras12 28:b813a8f685c3 93 //P2 = 0;
tomras12 28:b813a8f685c3 94 //P3 = 0;
tomras12 28:b813a8f685c3 95 isActive = false;
tomras12 28:b813a8f685c3 96 pc->printf("Loop now inactive\r\n");
tomras12 28:b813a8f685c3 97 } else {
tomras12 28:b813a8f685c3 98 updatePWM(currentConfig);
tomras12 28:b813a8f685c3 99 }
wchurch 18:6f120b374991 100 }
wchurch 18:6f120b374991 101
tomras12 23:abe76b7c377a 102 void onButtonPress()
wchurch 18:6f120b374991 103 {
tomras12 25:41abce345a12 104 wait_ms(10);
tomras12 23:abe76b7c377a 105 // Activate control loop if not active
tomras12 23:abe76b7c377a 106 if(!isActive) {
tomras12 28:b813a8f685c3 107 pwmint.attach(&controlLoop, .01);
wchurch 18:6f120b374991 108 EN1 = 1;
wchurch 19:3118e8e60182 109 myled = 1;
tomras12 23:abe76b7c377a 110 isActive = true;
tomras12 26:f2bb916262c9 111 pc->printf("Loop now active\r\n");
wchurch 22:9f3022fe9084 112 }
wchurch 22:9f3022fe9084 113
wchurch 22:9f3022fe9084 114 else {
wchurch 6:f2c930a90873 115 pwmint.detach();
tomras12 26:f2bb916262c9 116 //currentConfig->pwm->write(0.5);
tomras12 23:abe76b7c377a 117 //bt = 0.0;
wchurch 8:1011786787a4 118 myled = 0;
wchurch 9:6a83e2777d24 119 EN1 = 0;
wchurch 7:1be7e6735fe2 120 //P2 = 0;
wchurch 7:1be7e6735fe2 121 //P3 = 0;
tomras12 23:abe76b7c377a 122 isActive = false;
tomras12 26:f2bb916262c9 123 pc->printf("Loop now inactive\r\n");
wchurch 6:f2c930a90873 124 }
wchurch 6:f2c930a90873 125 }
wchurch 6:f2c930a90873 126
tomras12 28:b813a8f685c3 127 void checkCalib(BNO055 *imu, Serial *pc) {
tomras12 28:b813a8f685c3 128 pc->printf("Checking calibration status...\r\n");
tomras12 28:b813a8f685c3 129 int stat = imu->read_calib_status();
tomras12 28:b813a8f685c3 130 while(stat < 192) {
tomras12 28:b813a8f685c3 131 pc->printf("Sys:%d Gyr:%d Acc:%d Mag:%d\r\n",
tomras12 28:b813a8f685c3 132 (stat >> 6) & 0x03,
tomras12 28:b813a8f685c3 133 (stat >> 4) & 0x03,
tomras12 28:b813a8f685c3 134 (stat >> 2) & 0x03,
tomras12 28:b813a8f685c3 135 stat & 0x03);
tomras12 28:b813a8f685c3 136 wait_ms(250);
tomras12 28:b813a8f685c3 137 stat = imu->read_calib_status();
tomras12 28:b813a8f685c3 138 }
tomras12 28:b813a8f685c3 139 pc->printf("Looks good buddy, put the cube down now.\r\n");
tomras12 28:b813a8f685c3 140 wait(2);
tomras12 28:b813a8f685c3 141 }
tomras12 28:b813a8f685c3 142
tomras12 28:b813a8f685c3 143 /*
tomras12 28:b813a8f685c3 144 * Returns PWM duty cycle based on:
tomras12 28:b813a8f685c3 145 * - wv wheel velocity
tomras12 28:b813a8f685c3 146 * - ae angle error
tomras12 28:b813a8f685c3 147 * - bv body velocity
tomras12 28:b813a8f685c3 148 * - Kwv wheel vel gain
tomras12 28:b813a8f685c3 149 * - Kbt angle gain
tomras12 28:b813a8f685c3 150 * - Kbv body vel gain
tomras12 28:b813a8f685c3 151 */
tomras12 28:b813a8f685c3 152 double calcPWM(config *c)
tomras12 28:b813a8f685c3 153 {
tomras12 28:b813a8f685c3 154 // Converts and read the analog input value (value from 0.0 to 1.0):
tomras12 28:b813a8f685c3 155 double wv = c->hall->read();
tomras12 28:b813a8f685c3 156 wv = (wv - 1.65) * 5000.0 / 1.65; // Scale the velocity to rad/s
tomras12 28:b813a8f685c3 157
tomras12 28:b813a8f685c3 158 double bt = (*(c->angle) - c->eqAngle);
tomras12 28:b813a8f685c3 159
tomras12 28:b813a8f685c3 160 double r1 = -(c->Kbt * bt + c->Kbv * -1.0 * (*(c->vel)) + c->Kwv * wv);
tomras12 28:b813a8f685c3 161
tomras12 28:b813a8f685c3 162 //Limit PWM range
tomras12 28:b813a8f685c3 163 if (r1 > 6.0) {r1 = 6.0;}
tomras12 28:b813a8f685c3 164 else if (r1 < -6.0) {r1 = -6.0;}
tomras12 28:b813a8f685c3 165
tomras12 28:b813a8f685c3 166 // Normalize for PWM output
tomras12 28:b813a8f685c3 167 r1 = ((.4*(r1/6.0)) + 0.5);
tomras12 28:b813a8f685c3 168
tomras12 28:b813a8f685c3 169 // Check if cube is too far tilted and send 0 torque
tomras12 28:b813a8f685c3 170 // May be redundant, check outer program
tomras12 28:b813a8f685c3 171 /*if (bt > (pi/8) || bt < -(pi/8)){
tomras12 28:b813a8f685c3 172 return .5;
tomras12 28:b813a8f685c3 173 }*/
tomras12 28:b813a8f685c3 174 return r1;
tomras12 28:b813a8f685c3 175 }
tomras12 28:b813a8f685c3 176
tomras12 28:b813a8f685c3 177 void updatePWM(config *c) {
tomras12 28:b813a8f685c3 178
tomras12 28:b813a8f685c3 179 c->pwm->write(calcPWM(c));
tomras12 28:b813a8f685c3 180 //pc->printf("%6.4f\r\n", calcPWM(c));
tomras12 28:b813a8f685c3 181 }
tomras12 28:b813a8f685c3 182
tomras12 23:abe76b7c377a 183 /*
tomras12 23:abe76b7c377a 184 * TODO: Documentation
tomras12 23:abe76b7c377a 185 */
wchurch 6:f2c930a90873 186 int main()
wchurch 6:f2c930a90873 187 {
tomras12 24:c7b3bac429c5 188 // Initialize serial
tomras12 24:c7b3bac429c5 189 pc = new Serial(SERIAL_TX, SERIAL_RX);
tomras12 24:c7b3bac429c5 190 // Initialize IMU
tomras12 24:c7b3bac429c5 191 pc->printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n");
tomras12 24:c7b3bac429c5 192 wait_ms(1000); // Allows IMU time to power up
tomras12 24:c7b3bac429c5 193 pc->printf("Initializing IMU...\r\n");
tomras12 24:c7b3bac429c5 194 imu = new BNO055(i2c, PA_8); // Init IMU to i2c and reset pin
tomras12 24:c7b3bac429c5 195 while (imu->chip_ready() == 0) {
tomras12 24:c7b3bac429c5 196 pc->printf("Bosch BNO055 is NOT available!!\r\n");
wchurch 0:604ceafb7bb3 197 }
tomras12 23:abe76b7c377a 198
tomras12 23:abe76b7c377a 199 // Initialize pwm to 0 torque
tomras12 24:c7b3bac429c5 200 balZ.pwm->write(0.5); //Stops ESCON studio from throwing out-of-range
tomras12 23:abe76b7c377a 201 // error
tomras12 23:abe76b7c377a 202
tomras12 23:abe76b7c377a 203 // Set frequency of PWMs
tomras12 24:c7b3bac429c5 204 balZ.pwm->period(0.0002); // set pwm frequency
tomras12 23:abe76b7c377a 205
tomras12 23:abe76b7c377a 206 // Attach the button interrupt to the callback function
tomras12 23:abe76b7c377a 207 // This button toggles if the loop is enabled
tomras12 23:abe76b7c377a 208 isActive= false;
tomras12 23:abe76b7c377a 209 button.rise(&onButtonPress);
tomras12 24:c7b3bac429c5 210
tomras12 24:c7b3bac429c5 211 // Calibrate until sys calib is at 3
tomras12 26:f2bb916262c9 212 //checkCalib(imu, pc);
tomras12 24:c7b3bac429c5 213
tomras12 24:c7b3bac429c5 214 // Initialize config
tomras12 28:b813a8f685c3 215 detectOrientation();
tomras12 24:c7b3bac429c5 216
tomras12 24:c7b3bac429c5 217 pc->printf("Push button to begin loop");
wchurch 18:6f120b374991 218
wchurch 0:604ceafb7bb3 219 while(1) {
tomras12 24:c7b3bac429c5 220
tomras12 28:b813a8f685c3 221 imu->get_Euler_Angles(&euler_angles);
tomras12 28:b813a8f685c3 222 imu->get_velocities(&velocity);
tomras12 28:b813a8f685c3 223 detectOrientation();
tomras12 28:b813a8f685c3 224
tomras12 25:41abce345a12 225 //pc->printf("P:%6.4f, R:%6.4f\r\n", euler_angles.p, euler_angles.r);
tomras12 24:c7b3bac429c5 226 //pc->printf("X:%6.4f, Y:%6.4f, Z:%6.4f\r\n", velocity.x, velocity.y, velocity.z);
tomras12 28:b813a8f685c3 227 //pc->printf("%s\r\n", (currentConfig->descr));
tomras12 28:b813a8f685c3 228 pc->printf("%6.4f\r\n", *(currentConfig->angle));
tomras12 25:41abce345a12 229 // 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 230 /*pc->printf("%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f\r\n",
tomras12 25:41abce345a12 231 euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi,
tomras12 28:b813a8f685c3 232 euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi);*/
tomras12 25:41abce345a12 233
wchurch 6:f2c930a90873 234
tomras12 24:c7b3bac429c5 235 // 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 236 // euler_angles.h, euler_angles.p, euler_angles.r, r1, velocity.z, wv);
wchurch 18:6f120b374991 237
tomras12 24:c7b3bac429c5 238 //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 239 // bt, euler_angles.p, r1, velocity.z, wv);
wchurch 18:6f120b374991 240
wchurch 8:1011786787a4 241 //wait(0.2);
wchurch 0:604ceafb7bb3 242 }
wchurch 0:604ceafb7bb3 243 }