cube
Dependencies: BNO055_fusion_tom FastPWM mbed
main.cpp@36:54b527b3a576, 2017-04-25 (annotated)
- Committer:
- wchurch
- Date:
- Tue Apr 25 19:11:41 2017 +0000
- Revision:
- 36:54b527b3a576
- Parent:
- 35:34a62f118eb0
- Child:
- 37:55cae2b7033e
hanging with stolfi
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
wchurch | 0:604ceafb7bb3 | 1 | #include "mbed.h" |
wchurch | 0:604ceafb7bb3 | 2 | #include "BNO055.h" |
wchurch | 0:604ceafb7bb3 | 3 | |
wchurch | 0:604ceafb7bb3 | 4 | |
wchurch | 0:604ceafb7bb3 | 5 | //------------------------------------ |
wchurch | 0:604ceafb7bb3 | 6 | // Hyperterminal configuration |
wchurch | 0:604ceafb7bb3 | 7 | // 9600 bauds, 8-bit data, no parity |
wchurch | 0:604ceafb7bb3 | 8 | //------------------------------------ |
wchurch | 0:604ceafb7bb3 | 9 | |
wchurch | 0:604ceafb7bb3 | 10 | Serial pc(SERIAL_TX, SERIAL_RX); |
wchurch | 0:604ceafb7bb3 | 11 | |
wchurch | 18:6f120b374991 | 12 | Ticker pwmint; |
wchurch | 0:604ceafb7bb3 | 13 | DigitalOut myled(LED1); |
wchurch | 6:f2c930a90873 | 14 | InterruptIn button(USER_BUTTON); |
wchurch | 6:f2c930a90873 | 15 | |
wchurch | 1:d81f93d3f819 | 16 | PwmOut P1(PE_9); |
wchurch | 9:6a83e2777d24 | 17 | DigitalOut EN1(D0); |
wchurch | 13:e41d32a48931 | 18 | AnalogIn I1(A0); |
wchurch | 9:6a83e2777d24 | 19 | |
wchurch | 7:1be7e6735fe2 | 20 | //PwmOut P2(PE_11); 1D FOCUS FOR NOW |
wchurch | 7:1be7e6735fe2 | 21 | //PwmOut P3(PE_13); |
wchurch | 0:604ceafb7bb3 | 22 | |
wchurch | 18:6f120b374991 | 23 | I2C i2c(PB_9, PB_8); // SDA, SCL |
wchurch | 18:6f120b374991 | 24 | BNO055 imu(i2c, PA_8); // Reset |
wchurch | 18:6f120b374991 | 25 | |
wchurch | 18:6f120b374991 | 26 | BNO055_ID_INF_TypeDef bno055_id_inf; |
wchurch | 18:6f120b374991 | 27 | BNO055_EULER_TypeDef euler_angles; |
wchurch | 23:ac8bbcfa1b24 | 28 | BNO055_VEL_TypeDef velocity; |
wchurch | 36:54b527b3a576 | 29 | |
wchurch | 36:54b527b3a576 | 30 | |
wchurch | 36:54b527b3a576 | 31 | //Pole P + f //Pole P //LQR 2000 //LQR 500 //LQR 1000 |
wchurch | 36:54b527b3a576 | 32 | double Kbt = -67.6352; //-67.5155; //-73.8411; //-83.4030; //-77.7950; |
wchurch | 36:54b527b3a576 | 33 | double Kbv = -9.3204; //-9.3610; //-9.3563; //-10.5874; //-9.8654; |
wchurch | 36:54b527b3a576 | 34 | double Kwv = -0.0082; //82;//587; //-0.0080; //-0.00244; //-0.00468; //-0.00337; |
wchurch | 36:54b527b3a576 | 35 | |
wchurch | 28:0b0b258eef75 | 36 | double speed; |
wchurch | 16:27069802baae | 37 | double wv; |
wchurch | 18:6f120b374991 | 38 | double bt; |
wchurch | 23:ac8bbcfa1b24 | 39 | double bv; |
wchurch | 8:1011786787a4 | 40 | double r1; |
wchurch | 8:1011786787a4 | 41 | |
wchurch | 18:6f120b374991 | 42 | double pi = 3.14159265; |
wchurch | 18:6f120b374991 | 43 | |
wchurch | 6:f2c930a90873 | 44 | int isPressed; |
wchurch | 6:f2c930a90873 | 45 | |
wchurch | 18:6f120b374991 | 46 | void pwmupdate() |
wchurch | 18:6f120b374991 | 47 | { |
wchurch | 18:6f120b374991 | 48 | |
wchurch | 31:e159b0caa2ea | 49 | |
wchurch | 31:e159b0caa2ea | 50 | speed = I1.read(); // Converts and read the analog input value (value from 0.0 to 1.0) |
wchurch | 31:e159b0caa2ea | 51 | wv = (speed-0.5)*(5000/0.5); // Scale the velocity to rad/s |
wchurch | 18:6f120b374991 | 52 | |
wchurch | 31:e159b0caa2ea | 53 | bt = ((euler_angles.p) + (pi/4)); //Read body angle |
wchurch | 31:e159b0caa2ea | 54 | bv = -1.0*velocity.z; //Read boady angle velocity |
wchurch | 23:ac8bbcfa1b24 | 55 | |
wchurch | 36:54b527b3a576 | 56 | |
wchurch | 36:54b527b3a576 | 57 | //THIS IS THE ONE LINE OF CODE THAT MATTERS |
wchurch | 31:e159b0caa2ea | 58 | r1 = -1.0*(Kbt*bt + Kbv*bv + Kwv*wv); //Calculate current setpoint |
wchurch | 18:6f120b374991 | 59 | |
wchurch | 19:3118e8e60182 | 60 | |
wchurch | 10:69eb22acaea3 | 61 | //Limit PWM range |
wchurch | 19:3118e8e60182 | 62 | if (r1 > 6.0) { |
wchurch | 19:3118e8e60182 | 63 | r1 = 6.0; |
wchurch | 18:6f120b374991 | 64 | } |
wchurch | 19:3118e8e60182 | 65 | |
wchurch | 36:54b527b3a576 | 66 | else if (r1 < -6.0) { |
wchurch | 19:3118e8e60182 | 67 | r1 = -6.0; |
wchurch | 19:3118e8e60182 | 68 | } |
wchurch | 20:79c99bbf6dd7 | 69 | |
wchurch | 31:e159b0caa2ea | 70 | r1 = ((.4*(r1/6.0)) + 0.5) ; //Normalize for PWM output |
wchurch | 19:3118e8e60182 | 71 | |
wchurch | 31:e159b0caa2ea | 72 | P1 = r1; // |
wchurch | 31:e159b0caa2ea | 73 | |
wchurch | 31:e159b0caa2ea | 74 | |
wchurch | 31:e159b0caa2ea | 75 | //Limit angle to prevent output railing |
wchurch | 23:ac8bbcfa1b24 | 76 | |
wchurch | 29:137559beea74 | 77 | if (bt > (pi/6)){ |
wchurch | 20:79c99bbf6dd7 | 78 | EN1 = 0; |
wchurch | 20:79c99bbf6dd7 | 79 | pwmint.detach(); |
wchurch | 20:79c99bbf6dd7 | 80 | } |
wchurch | 20:79c99bbf6dd7 | 81 | |
wchurch | 29:137559beea74 | 82 | else if (bt< -(pi/6)){ |
wchurch | 20:79c99bbf6dd7 | 83 | EN1 = 0; |
wchurch | 20:79c99bbf6dd7 | 84 | pwmint.detach(); |
wchurch | 20:79c99bbf6dd7 | 85 | } |
wchurch | 20:79c99bbf6dd7 | 86 | |
wchurch | 18:6f120b374991 | 87 | } |
wchurch | 18:6f120b374991 | 88 | |
wchurch | 18:6f120b374991 | 89 | void eventFunction() |
wchurch | 18:6f120b374991 | 90 | { |
wchurch | 31:e159b0caa2ea | 91 | //Button press routine |
wchurch | 31:e159b0caa2ea | 92 | |
wchurch | 31:e159b0caa2ea | 93 | |
wchurch | 6:f2c930a90873 | 94 | if(!isPressed) { |
wchurch | 31:e159b0caa2ea | 95 | //Enable closed loop mode, enable motor drivers |
wchurch | 32:e77ad4aff1fd | 96 | pwmint.attach(&pwmupdate, .002); |
wchurch | 18:6f120b374991 | 97 | EN1 = 1; |
wchurch | 19:3118e8e60182 | 98 | myled = 1; |
wchurch | 6:f2c930a90873 | 99 | isPressed=1; |
wchurch | 18:6f120b374991 | 100 | |
wchurch | 22:9f3022fe9084 | 101 | } |
wchurch | 22:9f3022fe9084 | 102 | |
wchurch | 22:9f3022fe9084 | 103 | else { |
wchurch | 31:e159b0caa2ea | 104 | //Disable closed loop mode |
wchurch | 6:f2c930a90873 | 105 | pwmint.detach(); |
wchurch | 20:79c99bbf6dd7 | 106 | P1 = 0.5; |
wchurch | 19:3118e8e60182 | 107 | bt = 0.0; |
wchurch | 8:1011786787a4 | 108 | myled = 0; |
wchurch | 9:6a83e2777d24 | 109 | EN1 = 0; |
wchurch | 31:e159b0caa2ea | 110 | |
wchurch | 18:6f120b374991 | 111 | isPressed=0; |
wchurch | 6:f2c930a90873 | 112 | } |
wchurch | 6:f2c930a90873 | 113 | } |
wchurch | 6:f2c930a90873 | 114 | |
wchurch | 6:f2c930a90873 | 115 | int main() |
wchurch | 6:f2c930a90873 | 116 | { |
wchurch | 23:ac8bbcfa1b24 | 117 | wait_us(2000000); |
wchurch | 32:e77ad4aff1fd | 118 | |
wchurch | 0:604ceafb7bb3 | 119 | pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n"); |
wchurch | 31:e159b0caa2ea | 120 | pc.printf("Kbt: %+6.4f, Kbv: %+6.4f, Kwv: %+6.4f\r\n", Kbt,Kbv,Kwv); |
wchurch | 32:e77ad4aff1fd | 121 | |
wchurch | 18:6f120b374991 | 122 | if (imu.chip_ready() == 0) { |
wchurch | 4:ae9e664301dd | 123 | pc.printf("Bosch BNO055 is NOT available!!\r\n"); |
wchurch | 0:604ceafb7bb3 | 124 | } |
wchurch | 18:6f120b374991 | 125 | |
wchurch | 0:604ceafb7bb3 | 126 | imu.read_id_inf(&bno055_id_inf); |
wchurch | 31:e159b0caa2ea | 127 | |
wchurch | 21:4f60d60b2e5a | 128 | P1 = .5; //Stops ESCON studio from throwing out-of-range error |
wchurch | 21:4f60d60b2e5a | 129 | |
wchurch | 31:e159b0caa2ea | 130 | |
wchurch | 7:1be7e6735fe2 | 131 | //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 | 132 | // bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, |
wchurch | 7:1be7e6735fe2 | 133 | // bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); |
wchurch | 18:6f120b374991 | 134 | |
wchurch | 9:6a83e2777d24 | 135 | P1.period(0.0002); //Set PWM frequency |
wchurch | 18:6f120b374991 | 136 | |
wchurch | 6:f2c930a90873 | 137 | isPressed=0; |
wchurch | 9:6a83e2777d24 | 138 | button.rise(&eventFunction); //Enable Closed Loop |
wchurch | 18:6f120b374991 | 139 | |
wchurch | 18:6f120b374991 | 140 | |
wchurch | 18:6f120b374991 | 141 | |
wchurch | 0:604ceafb7bb3 | 142 | while(1) { |
wchurch | 6:f2c930a90873 | 143 | |
wchurch | 18:6f120b374991 | 144 | |
wchurch | 31:e159b0caa2ea | 145 | // pc.printf("H:%+6.4f [rad], P:%+6.4f, R:%+6.4f, THETA%+6.4f [PWM], Theta_dot:%+6.4f [rad/s] , WV:%+6.4f [rad/s] \r\n", |
wchurch | 23:ac8bbcfa1b24 | 146 | // euler_angles.h, euler_angles.p, euler_angles.r, bt, velocity.z, wv); |
wchurch | 18:6f120b374991 | 147 | |
wchurch | 21:4f60d60b2e5a | 148 | //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 | 23:ac8bbcfa1b24 | 149 | // bt, euler_angles.p, r1, bv, wv); |
wchurch | 18:6f120b374991 | 150 | |
wchurch | 6:f2c930a90873 | 151 | imu.get_Euler_Angles(&euler_angles); |
wchurch | 15:1d21cf90cd47 | 152 | imu.get_velocities(&velocity); |
wchurch | 23:ac8bbcfa1b24 | 153 | |
wchurch | 31:e159b0caa2ea | 154 | //bt = ((euler_angles.p) + (pi/4)); |
wchurch | 23:ac8bbcfa1b24 | 155 | //bv = -1.0*velocity.z; |
wchurch | 8:1011786787a4 | 156 | //wait(0.2); |
wchurch | 0:604ceafb7bb3 | 157 | } |
wchurch | 0:604ceafb7bb3 | 158 | } |