Tom Rasmussen / Mbed 2 deprecated NucleoCubeFork1

This is the one where I went back and un-did the cube.cpp file

Dependencies:   BNO055_fusion_tom FastPWM mbed

Fork of NucleoCube1 by Tom Rasmussen

main.cpp@15:1d21cf90cd47, 2017-04-10 (annotated)

Committer:
wchurch
Date:
Mon Apr 10 21:11:01 2017 +0000
Revision:
15:1d21cf90cd47
Parent:
14:90ac96893fcd
Child:
16:27069802baae
velocity read working;

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 5:9247f07a954a 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 0:604ceafb7bb3 23 I2C i2c(PB_9, PB_8); // SDA, SCL
wchurch 0:604ceafb7bb3 24 BNO055 imu(i2c, PA_8); // Reset
wchurch 0:604ceafb7bb3 25
wchurch 15:1d21cf90cd47 26 BNO055_ID_INF_TypeDef bno055_id_inf;
wchurch 15:1d21cf90cd47 27 BNO055_EULER_TypeDef euler_angles;
wchurch 15:1d21cf90cd47 28 BNO055_VEL_TypeDef velocity; //IN PROGRESS
wchurch 6:f2c930a90873 29
wchurch 10:69eb22acaea3 30 double Kbt = 5.4;
wchurch 10:69eb22acaea3 31 double Kbv = 0.33;
wchurch 8:1011786787a4 32 double Kwv = 0.124;
wchurch 14:90ac96893fcd 33
wchurch 14:90ac96893fcd 34 double cur1;
wchurch 8:1011786787a4 35 double r1;
wchurch 8:1011786787a4 36
wchurch 6:f2c930a90873 37 int isPressed;
wchurch 6:f2c930a90873 38
wchurch 6:f2c930a90873 39 void pwmupdate() {
wchurch 6:f2c930a90873 40
wchurch 6:f2c930a90873 41 myled = !myled;
wchurch 13:e41d32a48931 42 cur1 = I1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
wchurch 13:e41d32a48931 43 cur1 = (cur1-2.0)*5000.0; // Change the value to be in the 0 to 3300 range
wchurch 13:e41d32a48931 44
wchurch 6:f2c930a90873 45
wchurch 14:90ac96893fcd 46 r1 = (Kbt*euler_angles.h);
wchurch 14:90ac96893fcd 47
wchurch 8:1011786787a4 48 r1 = (r1 + 6.0)/12.0 ; //Normalize for PWM output
wchurch 10:69eb22acaea3 49
wchurch 10:69eb22acaea3 50 //Limit PWM range
wchurch 10:69eb22acaea3 51 if (r1 > 1.0){
wchurch 10:69eb22acaea3 52 r1 = 1.0;
wchurch 10:69eb22acaea3 53 }
wchurch 10:69eb22acaea3 54 if (r1 < 0.0){
wchurch 10:69eb22acaea3 55 r1 = 0.0;
wchurch 10:69eb22acaea3 56 }
wchurch 8:1011786787a4 57 P1 = r1;
wchurch 7:1be7e6735fe2 58 //P2 = (euler_angles.r/360.0);
wchurch 7:1be7e6735fe2 59 //P3 = (euler_angles.p/360.0);
wchurch 6:f2c930a90873 60
wchurch 6:f2c930a90873 61 }
wchurch 6:f2c930a90873 62
wchurch 6:f2c930a90873 63 void eventFunction() {
wchurch 7:1be7e6735fe2 64
wchurch 6:f2c930a90873 65 if(!isPressed) {
wchurch 9:6a83e2777d24 66 pwmint.attach(&pwmupdate, .005);
wchurch 9:6a83e2777d24 67 EN1 = 1;
wchurch 6:f2c930a90873 68 isPressed=1;
wchurch 7:1be7e6735fe2 69
wchurch 6:f2c930a90873 70 } else {
wchurch 6:f2c930a90873 71 pwmint.detach();
wchurch 15:1d21cf90cd47 72 P1 = 0.0;
wchurch 15:1d21cf90cd47 73 r1 = -1.0;
wchurch 8:1011786787a4 74 myled = 0;
wchurch 9:6a83e2777d24 75 EN1 = 0;
wchurch 7:1be7e6735fe2 76 //P2 = 0;
wchurch 7:1be7e6735fe2 77 //P3 = 0;
wchurch 6:f2c930a90873 78 isPressed=0;
wchurch 6:f2c930a90873 79 }
wchurch 6:f2c930a90873 80 }
wchurch 6:f2c930a90873 81
wchurch 6:f2c930a90873 82 int main()
wchurch 6:f2c930a90873 83 {
wchurch 0:604ceafb7bb3 84
wchurch 0:604ceafb7bb3 85 pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
wchurch 0:604ceafb7bb3 86 if (imu.chip_ready() == 0){
wchurch 4:ae9e664301dd 87 pc.printf("Bosch BNO055 is NOT available!!\r\n");
wchurch 0:604ceafb7bb3 88 }
wchurch 6:f2c930a90873 89
wchurch 0:604ceafb7bb3 90 imu.read_id_inf(&bno055_id_inf);
wchurch 6:f2c930a90873 91
wchurch 7:1be7e6735fe2 92 //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 93 // bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id,
wchurch 7:1be7e6735fe2 94 // bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
wchurch 6:f2c930a90873 95
wchurch 9:6a83e2777d24 96 P1.period(0.0002); //Set PWM frequency
wchurch 7:1be7e6735fe2 97
wchurch 6:f2c930a90873 98 isPressed=0;
wchurch 9:6a83e2777d24 99 button.rise(&eventFunction); //Enable Closed Loop
wchurch 6:f2c930a90873 100
wchurch 6:f2c930a90873 101
wchurch 0:604ceafb7bb3 102
wchurch 0:604ceafb7bb3 103 while(1) {
wchurch 6:f2c930a90873 104
wchurch 6:f2c930a90873 105
wchurch 15:1d21cf90cd47 106 pc.printf("Heading:%+6.4f [rad], R1%+6.4f [PWM], Pitch:%+6.4f [rad/s]\r\n",
wchurch 15:1d21cf90cd47 107 euler_angles.h, r1, velocity.z);
wchurch 7:1be7e6735fe2 108
wchurch 6:f2c930a90873 109 imu.get_Euler_Angles(&euler_angles);
wchurch 15:1d21cf90cd47 110 imu.get_velocities(&velocity);
wchurch 0:604ceafb7bb3 111
wchurch 8:1011786787a4 112 //wait(0.2);
wchurch 0:604ceafb7bb3 113 }
wchurch 0:604ceafb7bb3 114 }