Will Church / Mbed 2 deprecated NucleoCube1

cube

Dependencies:   BNO055_fusion_tom FastPWM mbed

main.cpp@27:240ce092615c, 2017-04-19 (annotated)

Committer:
wchurch
Date:
Wed Apr 19 05:31:19 2017 +0000
Revision:
27:240ce092615c
Parent:
26:6ffb048f45ae
Child:
28:0b0b258eef75
gains foolin' (moved real parts back to 2.5) spins slowly

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