Will Church / Mbed 2 deprecated NucleoCube1

cube

Dependencies:   BNO055_fusion_tom FastPWM mbed

main.cpp@35:34a62f118eb0, 2017-04-22 (annotated)

Committer:
wchurch
Date:
Sat Apr 22 21:09:29 2017 +0000
Revision:
35:34a62f118eb0
Parent:
34:2cdc880e4c29
Child:
36:54b527b3a576
Fuck LQR

Who changed what in which revision?

UserRevisionLine numberNew 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 35:34a62f118eb0 29 //Pole P + f//Pole P //LQR 2000 //LQR 500 //LQR 1000
wchurch 35:34a62f118eb0 30 double Kbt = -67.6352; //-67.5155; //-73.8411; //-83.4030; //-77.7950;
wchurch 35:34a62f118eb0 31 double Kbv = -9.3204; //-9.3610; //-9.3563; //-10.5874; //-9.8654;
wchurch 35:34a62f118eb0 32 double Kwv = -0.0094;//82;//587; //-0.0080; //-0.00244; //-0.00468; //-0.00337;
wchurch 28:0b0b258eef75 33 double speed;
wchurch 16:27069802baae 34 double wv;
wchurch 18:6f120b374991 35 double bt;
wchurch 23:ac8bbcfa1b24 36 double bv;
wchurch 8:1011786787a4 37 double r1;
wchurch 8:1011786787a4 38
wchurch 18:6f120b374991 39 double pi = 3.14159265;
wchurch 18:6f120b374991 40
wchurch 6:f2c930a90873 41 int isPressed;
wchurch 6:f2c930a90873 42
wchurch 18:6f120b374991 43 void pwmupdate()
wchurch 18:6f120b374991 44 {
wchurch 18:6f120b374991 45
wchurch 31:e159b0caa2ea 46
wchurch 31:e159b0caa2ea 47 speed = I1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
wchurch 31:e159b0caa2ea 48 wv = (speed-0.5)*(5000/0.5); // Scale the velocity to rad/s
wchurch 18:6f120b374991 49
wchurch 31:e159b0caa2ea 50 bt = ((euler_angles.p) + (pi/4)); //Read body angle
wchurch 31:e159b0caa2ea 51 bv = -1.0*velocity.z; //Read boady angle velocity
wchurch 23:ac8bbcfa1b24 52
wchurch 31:e159b0caa2ea 53 r1 = -1.0*(Kbt*bt + Kbv*bv + Kwv*wv); //Calculate current setpoint
wchurch 18:6f120b374991 54
wchurch 19:3118e8e60182 55
wchurch 10:69eb22acaea3 56 //Limit PWM range
wchurch 19:3118e8e60182 57 if (r1 > 6.0) {
wchurch 19:3118e8e60182 58 r1 = 6.0;
wchurch 18:6f120b374991 59 }
wchurch 19:3118e8e60182 60
wchurch 19:3118e8e60182 61 if (r1 < -6.0) {
wchurch 19:3118e8e60182 62 r1 = -6.0;
wchurch 19:3118e8e60182 63 }
wchurch 20:79c99bbf6dd7 64
wchurch 31:e159b0caa2ea 65 r1 = ((.4*(r1/6.0)) + 0.5) ; //Normalize for PWM output
wchurch 19:3118e8e60182 66
wchurch 31:e159b0caa2ea 67 P1 = r1; //
wchurch 31:e159b0caa2ea 68
wchurch 31:e159b0caa2ea 69
wchurch 31:e159b0caa2ea 70 //Limit angle to prevent output railing
wchurch 23:ac8bbcfa1b24 71
wchurch 29:137559beea74 72 if (bt > (pi/6)){
wchurch 20:79c99bbf6dd7 73 EN1 = 0;
wchurch 20:79c99bbf6dd7 74 pwmint.detach();
wchurch 20:79c99bbf6dd7 75 }
wchurch 20:79c99bbf6dd7 76
wchurch 29:137559beea74 77 else 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 18:6f120b374991 82 }
wchurch 18:6f120b374991 83
wchurch 18:6f120b374991 84 void eventFunction()
wchurch 18:6f120b374991 85 {
wchurch 31:e159b0caa2ea 86 //Button press routine
wchurch 31:e159b0caa2ea 87
wchurch 31:e159b0caa2ea 88
wchurch 6:f2c930a90873 89 if(!isPressed) {
wchurch 31:e159b0caa2ea 90 //Enable closed loop mode, enable motor drivers
wchurch 32:e77ad4aff1fd 91 pwmint.attach(&pwmupdate, .002);
wchurch 18:6f120b374991 92 EN1 = 1;
wchurch 19:3118e8e60182 93 myled = 1;
wchurch 6:f2c930a90873 94 isPressed=1;
wchurch 18:6f120b374991 95
wchurch 22:9f3022fe9084 96 }
wchurch 22:9f3022fe9084 97
wchurch 22:9f3022fe9084 98 else {
wchurch 31:e159b0caa2ea 99 //Disable closed loop mode
wchurch 6:f2c930a90873 100 pwmint.detach();
wchurch 20:79c99bbf6dd7 101 P1 = 0.5;
wchurch 19:3118e8e60182 102 bt = 0.0;
wchurch 8:1011786787a4 103 myled = 0;
wchurch 9:6a83e2777d24 104 EN1 = 0;
wchurch 31:e159b0caa2ea 105
wchurch 18:6f120b374991 106 isPressed=0;
wchurch 6:f2c930a90873 107 }
wchurch 6:f2c930a90873 108 }
wchurch 6:f2c930a90873 109
wchurch 6:f2c930a90873 110 int main()
wchurch 6:f2c930a90873 111 {
wchurch 23:ac8bbcfa1b24 112 wait_us(2000000);
wchurch 32:e77ad4aff1fd 113
wchurch 0:604ceafb7bb3 114 pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
wchurch 31:e159b0caa2ea 115 pc.printf("Kbt: %+6.4f, Kbv: %+6.4f, Kwv: %+6.4f\r\n", Kbt,Kbv,Kwv);
wchurch 32:e77ad4aff1fd 116
wchurch 18:6f120b374991 117 if (imu.chip_ready() == 0) {
wchurch 4:ae9e664301dd 118 pc.printf("Bosch BNO055 is NOT available!!\r\n");
wchurch 0:604ceafb7bb3 119 }
wchurch 18:6f120b374991 120
wchurch 0:604ceafb7bb3 121 imu.read_id_inf(&bno055_id_inf);
wchurch 31:e159b0caa2ea 122
wchurch 21:4f60d60b2e5a 123 P1 = .5; //Stops ESCON studio from throwing out-of-range error
wchurch 21:4f60d60b2e5a 124
wchurch 31:e159b0caa2ea 125
wchurch 7:1be7e6735fe2 126 //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 127 // bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id,
wchurch 7:1be7e6735fe2 128 // bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
wchurch 18:6f120b374991 129
wchurch 9:6a83e2777d24 130 P1.period(0.0002); //Set PWM frequency
wchurch 18:6f120b374991 131
wchurch 6:f2c930a90873 132 isPressed=0;
wchurch 9:6a83e2777d24 133 button.rise(&eventFunction); //Enable Closed Loop
wchurch 18:6f120b374991 134
wchurch 18:6f120b374991 135
wchurch 18:6f120b374991 136
wchurch 0:604ceafb7bb3 137 while(1) {
wchurch 6:f2c930a90873 138
wchurch 18:6f120b374991 139
wchurch 31:e159b0caa2ea 140 // 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 141 // euler_angles.h, euler_angles.p, euler_angles.r, bt, velocity.z, wv);
wchurch 18:6f120b374991 142
wchurch 21:4f60d60b2e5a 143 //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 144 // bt, euler_angles.p, r1, bv, wv);
wchurch 18:6f120b374991 145
wchurch 6:f2c930a90873 146 imu.get_Euler_Angles(&euler_angles);
wchurch 15:1d21cf90cd47 147 imu.get_velocities(&velocity);
wchurch 23:ac8bbcfa1b24 148
wchurch 31:e159b0caa2ea 149 //bt = ((euler_angles.p) + (pi/4));
wchurch 23:ac8bbcfa1b24 150 //bv = -1.0*velocity.z;
wchurch 8:1011786787a4 151 //wait(0.2);
wchurch 0:604ceafb7bb3 152 }
wchurch 0:604ceafb7bb3 153 }