Will Church / Mbed 2 deprecated NucleoCube1

cube

Dependencies:   BNO055_fusion_tom FastPWM mbed

main.cpp@32:e77ad4aff1fd, 2017-04-22 (annotated)

Committer:
wchurch
Date:
Sat Apr 22 19:39:21 2017 +0000
Revision:
32:e77ad4aff1fd
Parent:
31:e159b0caa2ea
Child:
33:2434fd576d34
fix loop speed;

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