Tom Rasmussen / Mbed 2 deprecated NucleoCube1

Dependencies:   BNO055_fusion_tom FastPWM mbed

Fork of NucleoCube1 by Will Church

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?

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