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@25:41abce345a12, 2017-04-13 (annotated)

Committer:
tomras12
Date:
Thu Apr 13 00:40:51 2017 +0000
Revision:
25:41abce345a12
Parent:
24:c7b3bac429c5
Child:
26:f2bb916262c9
Detect orientation doesnt work if i put it in the control loop and i dont get why

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 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 24:c7b3bac429c5 88
tomras12 24:c7b3bac429c5 89 // Update the motor torque
tomras12 24:c7b3bac429c5 90 //updatePWM(currentConfig);
wchurch 18:6f120b374991 91 }
wchurch 18:6f120b374991 92
tomras12 23:abe76b7c377a 93 void onButtonPress()
wchurch 18:6f120b374991 94 {
tomras12 25:41abce345a12 95 wait_ms(10);
tomras12 23:abe76b7c377a 96 // Activate control loop if not active
tomras12 23:abe76b7c377a 97 if(!isActive) {
tomras12 23:abe76b7c377a 98 pwmint.attach(&controlLoop, .005);
wchurch 18:6f120b374991 99 EN1 = 1;
wchurch 19:3118e8e60182 100 myled = 1;
tomras12 23:abe76b7c377a 101 isActive = true;
wchurch 22:9f3022fe9084 102 }
wchurch 22:9f3022fe9084 103
wchurch 22:9f3022fe9084 104 else {
wchurch 6:f2c930a90873 105 pwmint.detach();
tomras12 23:abe76b7c377a 106 currentConfig->pwm->write(0.5);
tomras12 23:abe76b7c377a 107 //bt = 0.0;
wchurch 8:1011786787a4 108 myled = 0;
wchurch 9:6a83e2777d24 109 EN1 = 0;
wchurch 7:1be7e6735fe2 110 //P2 = 0;
wchurch 7:1be7e6735fe2 111 //P3 = 0;
tomras12 23:abe76b7c377a 112 isActive = false;
tomras12 24:c7b3bac429c5 113 pc->printf("Push putton to begin loop");
wchurch 6:f2c930a90873 114 }
wchurch 6:f2c930a90873 115 }
wchurch 6:f2c930a90873 116
tomras12 23:abe76b7c377a 117 /*
tomras12 23:abe76b7c377a 118 * TODO: Documentation
tomras12 23:abe76b7c377a 119 */
wchurch 6:f2c930a90873 120 int main()
wchurch 6:f2c930a90873 121 {
tomras12 24:c7b3bac429c5 122 // Initialize serial
tomras12 24:c7b3bac429c5 123 pc = new Serial(SERIAL_TX, SERIAL_RX);
tomras12 24:c7b3bac429c5 124 // Initialize IMU
tomras12 24:c7b3bac429c5 125 pc->printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n");
tomras12 24:c7b3bac429c5 126 wait_ms(1000); // Allows IMU time to power up
tomras12 24:c7b3bac429c5 127 pc->printf("Initializing IMU...\r\n");
tomras12 24:c7b3bac429c5 128 imu = new BNO055(i2c, PA_8); // Init IMU to i2c and reset pin
tomras12 24:c7b3bac429c5 129 while (imu->chip_ready() == 0) {
tomras12 24:c7b3bac429c5 130 pc->printf("Bosch BNO055 is NOT available!!\r\n");
wchurch 0:604ceafb7bb3 131 }
tomras12 23:abe76b7c377a 132
tomras12 23:abe76b7c377a 133 // Initialize pwm to 0 torque
tomras12 24:c7b3bac429c5 134 balZ.pwm->write(0.5); //Stops ESCON studio from throwing out-of-range
tomras12 23:abe76b7c377a 135 // error
tomras12 23:abe76b7c377a 136
tomras12 23:abe76b7c377a 137 // Set frequency of PWMs
tomras12 24:c7b3bac429c5 138 balZ.pwm->period(0.0002); // set pwm frequency
tomras12 23:abe76b7c377a 139
tomras12 23:abe76b7c377a 140 // Attach the button interrupt to the callback function
tomras12 23:abe76b7c377a 141 // This button toggles if the loop is enabled
tomras12 23:abe76b7c377a 142 isActive= false;
tomras12 23:abe76b7c377a 143 button.rise(&onButtonPress);
tomras12 24:c7b3bac429c5 144
tomras12 24:c7b3bac429c5 145 // Calibrate until sys calib is at 3
tomras12 25:41abce345a12 146 checkCalib(imu, pc);
tomras12 24:c7b3bac429c5 147
tomras12 24:c7b3bac429c5 148 // Initialize config
tomras12 24:c7b3bac429c5 149 detectOrientation();
tomras12 24:c7b3bac429c5 150
tomras12 24:c7b3bac429c5 151 pc->printf("Push button to begin loop");
wchurch 18:6f120b374991 152
wchurch 0:604ceafb7bb3 153 while(1) {
tomras12 24:c7b3bac429c5 154
tomras12 25:41abce345a12 155 //pc->printf("P:%6.4f, R:%6.4f\r\n", euler_angles.p, euler_angles.r);
tomras12 24:c7b3bac429c5 156 //pc->printf("X:%6.4f, Y:%6.4f, Z:%6.4f\r\n", velocity.x, velocity.y, velocity.z);
tomras12 24:c7b3bac429c5 157 //pc->printf("%6.4f\r\n", *(currentConfig->angle));
tomras12 25:41abce345a12 158 // 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 25:41abce345a12 159 /*pc->printf("%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f\r\n",
tomras12 25:41abce345a12 160 euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi,
tomras12 25:41abce345a12 161 euler_angles.h * 180 / pi, euler_angles.p * 180 / pi, euler_angles.r * 180 / pi);*/
tomras12 25:41abce345a12 162
wchurch 6:f2c930a90873 163
tomras12 24:c7b3bac429c5 164 // 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 165 // euler_angles.h, euler_angles.p, euler_angles.r, r1, velocity.z, wv);
wchurch 18:6f120b374991 166
tomras12 24:c7b3bac429c5 167 //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 168 // bt, euler_angles.p, r1, velocity.z, wv);
wchurch 18:6f120b374991 169
tomras12 24:c7b3bac429c5 170 imu->get_Euler_Angles(&euler_angles);
tomras12 24:c7b3bac429c5 171 imu->get_velocities(&velocity);
tomras12 25:41abce345a12 172
tomras12 25:41abce345a12 173 detectOrientation();
tomras12 25:41abce345a12 174 pc->printf("%s\r\n", currentConfig->descr);
wchurch 18:6f120b374991 175
wchurch 8:1011786787a4 176 //wait(0.2);
wchurch 0:604ceafb7bb3 177 }
wchurch 0:604ceafb7bb3 178 }