Will Church / Mbed 2 deprecated NucleoCube1

cube

Dependencies:   BNO055_fusion_tom FastPWM mbed

main.cpp@37:55cae2b7033e, 2017-05-03 (annotated)

Committer:
wchurch
Date:
Wed May 03 19:40:28 2017 +0000
Revision:
37:55cae2b7033e
Parent:
36:54b527b3a576 
Expo;

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 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 37:55cae2b7033e 34 double Kwv = -0.008575; //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 }