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@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?

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 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 }