Tom Rasmussen / Mbed 2 deprecated NucleoCube1

Dependencies:   BNO055_fusion_tom FastPWM mbed

Fork of NucleoCube1 by Will Church

main.cpp@23:abe76b7c377a, 2017-04-12 (annotated)

Committer:
tomras12
Date:
Wed Apr 12 00:29:45 2017 +0000
Revision:
23:abe76b7c377a
Parent:
22:9f3022fe9084
Child:
24:c7b3bac429c5
Refactor to make code more adaptable

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 23:abe76b7c377a 16 Serial pc(SERIAL_TX, SERIAL_RX); // 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 23:abe76b7c377a 19 BNO055 imu(i2c, PA_8); // Reset
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_ID_INF_TypeDef bno055_id_inf;
wchurch 18:6f120b374991 25 BNO055_EULER_TypeDef euler_angles;
tomras12 23:abe76b7c377a 26 BNO055_VEL_TypeDef velocity;
wchurch 18:6f120b374991 27
tomras12 23:abe76b7c377a 28 config *currentConfig; // Stores current config
tomras12 23:abe76b7c377a 29 bool isActive; // Control loop bool
wchurch 18:6f120b374991 30
tomras12 23:abe76b7c377a 31 /* Define our parameters here for each balancing configuration */
tomras12 23:abe76b7c377a 32 struct config justZ = {-89.9276, //Kbt
tomras12 23:abe76b7c377a 33 -14.9398, //Kbv
tomras12 23:abe76b7c377a 34 -0.001, //Kwv
tomras12 23:abe76b7c377a 35 pi/4.0, //eqAngle
tomras12 23:abe76b7c377a 36 &(euler_angles.p), //angle
tomras12 23:abe76b7c377a 37 &(velocity.z), //vel
tomras12 23:abe76b7c377a 38 new PwmOut(PE_9), //pwm
tomras12 23:abe76b7c377a 39 new AnalogIn(A0)}; //hall
wchurch 18:6f120b374991 40
tomras12 23:abe76b7c377a 41 void controlLoop() {
tomras12 23:abe76b7c377a 42 // Detect the current orientation
tomras12 23:abe76b7c377a 43 // For now just hardcode it to 'justZ'
tomras12 23:abe76b7c377a 44 currentConfig = &justZ;
tomras12 23:abe76b7c377a 45 updatePWM(currentConfig);
wchurch 18:6f120b374991 46 }
wchurch 18:6f120b374991 47
tomras12 23:abe76b7c377a 48 void onButtonPress()
wchurch 18:6f120b374991 49 {
tomras12 23:abe76b7c377a 50 // Activate control loop if not active
tomras12 23:abe76b7c377a 51 if(!isActive) {
tomras12 23:abe76b7c377a 52 pwmint.attach(&controlLoop, .005);
wchurch 18:6f120b374991 53 EN1 = 1;
wchurch 19:3118e8e60182 54 myled = 1;
tomras12 23:abe76b7c377a 55 isActive = true;
wchurch 22:9f3022fe9084 56 }
wchurch 22:9f3022fe9084 57
wchurch 22:9f3022fe9084 58 else {
wchurch 6:f2c930a90873 59 pwmint.detach();
tomras12 23:abe76b7c377a 60 currentConfig->pwm->write(0.5);
tomras12 23:abe76b7c377a 61 //bt = 0.0;
wchurch 8:1011786787a4 62 myled = 0;
wchurch 9:6a83e2777d24 63 EN1 = 0;
wchurch 7:1be7e6735fe2 64 //P2 = 0;
wchurch 7:1be7e6735fe2 65 //P3 = 0;
tomras12 23:abe76b7c377a 66 isActive = false;
wchurch 6:f2c930a90873 67 }
wchurch 6:f2c930a90873 68 }
wchurch 6:f2c930a90873 69
tomras12 23:abe76b7c377a 70 /*
tomras12 23:abe76b7c377a 71 * TODO: Documentation
tomras12 23:abe76b7c377a 72 */
wchurch 6:f2c930a90873 73 int main()
wchurch 6:f2c930a90873 74 {
tomras12 23:abe76b7c377a 75 pc.printf("Cube balance program on " __DATE__ "/" __TIME__ "\r\n");
wchurch 18:6f120b374991 76 if (imu.chip_ready() == 0) {
wchurch 4:ae9e664301dd 77 pc.printf("Bosch BNO055 is NOT available!!\r\n");
wchurch 0:604ceafb7bb3 78 }
wchurch 0:604ceafb7bb3 79 imu.read_id_inf(&bno055_id_inf);
tomras12 23:abe76b7c377a 80
tomras12 23:abe76b7c377a 81 // Initialize pwm to 0 torque
tomras12 23:abe76b7c377a 82 justZ.pwm->write(0.5); //Stops ESCON studio from throwing out-of-range
tomras12 23:abe76b7c377a 83 // error
tomras12 23:abe76b7c377a 84
tomras12 23:abe76b7c377a 85 // Set frequency of PWMs
tomras12 23:abe76b7c377a 86 justZ.pwm->period(0.0002); // set pwm frequency
wchurch 21:4f60d60b2e5a 87
wchurch 7:1be7e6735fe2 88 //pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n",
wchurch 7:1be7e6735fe2 89 // bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id,
wchurch 7:1be7e6735fe2 90 // bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
wchurch 18:6f120b374991 91
wchurch 18:6f120b374991 92
tomras12 23:abe76b7c377a 93
tomras12 23:abe76b7c377a 94 // Attach the button interrupt to the callback function
tomras12 23:abe76b7c377a 95 // This button toggles if the loop is enabled
tomras12 23:abe76b7c377a 96 isActive= false;
tomras12 23:abe76b7c377a 97 button.rise(&onButtonPress);
wchurch 18:6f120b374991 98
wchurch 0:604ceafb7bb3 99 while(1) {
wchurch 6:f2c930a90873 100
wchurch 18:6f120b374991 101 // 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 102 // euler_angles.h, euler_angles.p, euler_angles.r, r1, velocity.z, wv);
wchurch 18:6f120b374991 103
wchurch 21:4f60d60b2e5a 104 //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 105 // bt, euler_angles.p, r1, velocity.z, wv);
wchurch 18:6f120b374991 106
wchurch 6:f2c930a90873 107 imu.get_Euler_Angles(&euler_angles);
wchurch 15:1d21cf90cd47 108 imu.get_velocities(&velocity);
wchurch 18:6f120b374991 109
wchurch 8:1011786787a4 110 //wait(0.2);
wchurch 0:604ceafb7bb3 111 }
wchurch 0:604ceafb7bb3 112 }