Jesus Fausto / chair_BNO055

With pid left, right, and foward

Dependencies:   BNO055

Dependents:   wheelchaircontrol wheelchaircontrolRosCom wheelchaircontrol wheelchaircontrol2 ... more

Fork of chair_BNO055 by ryan lin

chair_BNO055.cpp@6:ce8aa8208590, 2019-06-27 (annotated)

Committer:
jvfausto
Date:
Thu Jun 27 16:32:06 2019 +0000
Revision:
6:ce8aa8208590
Parent:
4:a6452220ec66 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:ad7a811c859f 1 #include "chair_BNO055.h"
ryanlin97 0:ad7a811c859f 2
ryanlin97 1:3258d62af038 3 static float total_yaw = 0;
ryanlin97 1:3258d62af038 4 static float minGyroZ;
jvfausto 4:a6452220ec66 5 static float x_speed = 0;
jvfausto 4:a6452220ec66 6 static float x_displacement = 0;
ryanlin97 1:3258d62af038 7 chair_BNO055::chair_BNO055(Serial* out, Timer* time)
ryanlin97 1:3258d62af038 8 {
ryanlin97 1:3258d62af038 9 imu = new BNO055(SDA, SCL);
ryanlin97 1:3258d62af038 10 usb = out;
ryanlin97 1:3258d62af038 11 t = time;
ryanlin97 1:3258d62af038 12 start = false;
ryanlin97 0:ad7a811c859f 13 }
ryanlin97 0:ad7a811c859f 14
ryanlin97 1:3258d62af038 15 chair_BNO055::chair_BNO055(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time)
ryanlin97 1:3258d62af038 16 {
ryanlin97 1:3258d62af038 17 imu = new BNO055(sda_pin, scl_pin);
ryanlin97 1:3258d62af038 18 usb = out;
ryanlin97 1:3258d62af038 19 t = time;
ryanlin97 1:3258d62af038 20 start = false;
ryanlin97 0:ad7a811c859f 21 }
ryanlin97 0:ad7a811c859f 22
ryanlin97 1:3258d62af038 23 void chair_BNO055::setup()
ryanlin97 1:3258d62af038 24 {
ryanlin97 1:3258d62af038 25 imu->reset();
ryanlin97 1:3258d62af038 26 usb->printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n");
jvfausto 6:ce8aa8208590 27 /*while (imu->check() == 0) {
ryanlin97 1:3258d62af038 28 usb->printf("Bosch BNO055 is NOT available!!\r\n");
ryanlin97 1:3258d62af038 29 wait(.5);
jvfausto 6:ce8aa8208590 30 }*/
ryanlin97 1:3258d62af038 31 usb->printf("BNO055 found\r\n\r\n");
ryanlin97 1:3258d62af038 32 usb->printf("Chip ID: %0z\r\n",imu->ID.id);
ryanlin97 1:3258d62af038 33 usb->printf("Accelerometer ID: %0z\r\n",imu->ID.accel);
ryanlin97 1:3258d62af038 34 usb->printf("Gyroscope ID: %0z\r\n",imu->ID.gyro);
ryanlin97 1:3258d62af038 35 usb->printf("Magnetometer ID: %0z\r\n\r\n",imu->ID.mag);
ryanlin97 1:3258d62af038 36 usb->printf("Firmware version v%d.%0d\r\n",imu->ID.sw[0],imu->ID.sw[1]);
ryanlin97 1:3258d62af038 37 usb->printf("Bootloader version v%d\r\n\r\n",imu->ID.bootload);
ryanlin97 1:3258d62af038 38 imu->set_accel_units(MPERSPERS);
ryanlin97 1:3258d62af038 39 imu->setmode(OPERATION_MODE_AMG);
ryanlin97 1:3258d62af038 40 imu->read_calibration_data();
ryanlin97 1:3258d62af038 41 imu->write_calibration_data();
ryanlin97 1:3258d62af038 42 imu->set_angle_units(DEGREES);
ryanlin97 0:ad7a811c859f 43
ryanlin97 1:3258d62af038 44 t->start();
ryanlin97 0:ad7a811c859f 45 }
ryanlin97 0:ad7a811c859f 46
ryanlin97 1:3258d62af038 47 void chair_BNO055::calibrate_yaw() {
ryanlin97 1:3258d62af038 48 float start = t->read();
ryanlin97 1:3258d62af038 49
ryanlin97 1:3258d62af038 50 while( t->read()- start <= CAL_TIME ){
ryanlin97 1:3258d62af038 51 total_yaw = boxcar(chair_BNO055::angle_north());
ryanlin97 1:3258d62af038 52 usb->printf("total_yaw %f\n", total_yaw);
ryanlin97 1:3258d62af038 53 }
ryanlin97 1:3258d62af038 54 }
ryanlin97 1:3258d62af038 55
ryanlin97 1:3258d62af038 56
ryanlin97 1:3258d62af038 57 double chair_BNO055::accel_x()
ryanlin97 1:3258d62af038 58 {
ryanlin97 1:3258d62af038 59 imu->get_accel();
ryanlin97 1:3258d62af038 60 return (double)imu->accel.x;
ryanlin97 0:ad7a811c859f 61 }
ryanlin97 0:ad7a811c859f 62
ryanlin97 1:3258d62af038 63 double chair_BNO055::accel_y()
ryanlin97 1:3258d62af038 64 {
ryanlin97 1:3258d62af038 65 imu->get_accel();
ryanlin97 1:3258d62af038 66 return (double)imu->accel.y;
ryanlin97 0:ad7a811c859f 67 }
ryanlin97 0:ad7a811c859f 68
ryanlin97 1:3258d62af038 69 double chair_BNO055::accel_z()
ryanlin97 1:3258d62af038 70 {
ryanlin97 1:3258d62af038 71 imu->get_accel();
ryanlin97 1:3258d62af038 72 return (double)imu->accel.z;
ryanlin97 0:ad7a811c859f 73 }
ryanlin97 0:ad7a811c859f 74
ryanlin97 1:3258d62af038 75 double chair_BNO055::gyro_x()
ryanlin97 1:3258d62af038 76 {
ryanlin97 1:3258d62af038 77 imu->get_gyro();
ryanlin97 1:3258d62af038 78 return (double)imu->gyro.x;
ryanlin97 0:ad7a811c859f 79 }
ryanlin97 0:ad7a811c859f 80
ryanlin97 1:3258d62af038 81 double chair_BNO055::gyro_y()
ryanlin97 1:3258d62af038 82 {
ryanlin97 1:3258d62af038 83 imu->get_gyro();
ryanlin97 1:3258d62af038 84 return (double)imu->gyro.y;
ryanlin97 1:3258d62af038 85 }
ryanlin97 1:3258d62af038 86
ryanlin97 1:3258d62af038 87 double chair_BNO055::gyro_z()
ryanlin97 1:3258d62af038 88 {
ryanlin97 1:3258d62af038 89 imu->get_gyro();
ryanlin97 1:3258d62af038 90 return lowPass(imu->gyro.z);
ryanlin97 1:3258d62af038 91 }
ryanlin97 1:3258d62af038 92
ryanlin97 1:3258d62af038 93 double chair_BNO055::angle_north()
ryanlin97 1:3258d62af038 94 {
ryanlin97 1:3258d62af038 95 imu->get_mag();
ryanlin97 1:3258d62af038 96 float x = imu->mag.x - 520;
ryanlin97 1:3258d62af038 97 float y = imu->mag.y;
ryanlin97 1:3258d62af038 98
ryanlin97 1:3258d62af038 99 float result = x/y;
ryanlin97 1:3258d62af038 100
ryanlin97 1:3258d62af038 101 float angleToNorth;
ryanlin97 1:3258d62af038 102 if(imu->mag.y>0)
ryanlin97 1:3258d62af038 103 angleToNorth = 90.0 - atan(result)*180/PI;
ryanlin97 1:3258d62af038 104 else if(imu->mag.y<0)
ryanlin97 1:3258d62af038 105 angleToNorth = 270.0 - atan(result)*180/PI;
ryanlin97 1:3258d62af038 106 else if(y == 0 && x <= 0)
ryanlin97 1:3258d62af038 107 angleToNorth = 180;
ryanlin97 1:3258d62af038 108 else if(y == 0 && x > 0)
ryanlin97 1:3258d62af038 109 angleToNorth = 0;
ryanlin97 1:3258d62af038 110
ryanlin97 1:3258d62af038 111 return (double)angleToNorth;
ryanlin97 0:ad7a811c859f 112 }
ryanlin97 0:ad7a811c859f 113
ryanlin97 1:3258d62af038 114 double chair_BNO055::roll()
ryanlin97 1:3258d62af038 115 {
ryanlin97 1:3258d62af038 116 imu->get_accel();
ryanlin97 1:3258d62af038 117 imu->get_gyro();
ryanlin97 0:ad7a811c859f 118
ryanlin97 1:3258d62af038 119 float roll = atan2(-imu->accel.x ,( sqrt((imu->accel.y * imu->accel.y) +
ryanlin97 1:3258d62af038 120 (imu->accel.z * imu->accel.z))));
ryanlin97 1:3258d62af038 121 roll = roll*57.3;
ryanlin97 1:3258d62af038 122
ryanlin97 1:3258d62af038 123 t->reset();
ryanlin97 1:3258d62af038 124
ryanlin97 1:3258d62af038 125 return (double)roll;
ryanlin97 0:ad7a811c859f 126 }
ryanlin97 0:ad7a811c859f 127
ryanlin97 1:3258d62af038 128 double chair_BNO055::pitch()
ryanlin97 1:3258d62af038 129 {
ryanlin97 1:3258d62af038 130 imu->get_accel();
ryanlin97 1:3258d62af038 131 imu->get_gyro();
ryanlin97 0:ad7a811c859f 132
ryanlin97 1:3258d62af038 133 float pitch = atan2 (imu->accel.y ,( sqrt ((imu->accel.x * imu->accel.x) +
ryanlin97 1:3258d62af038 134 (imu->accel.z * imu->accel.z))));
ryanlin97 1:3258d62af038 135 pitch = pitch*57.3;
ryanlin97 1:3258d62af038 136
ryanlin97 1:3258d62af038 137 t->reset();
ryanlin97 1:3258d62af038 138
ryanlin97 1:3258d62af038 139 return (double)pitch;
ryanlin97 0:ad7a811c859f 140 }
ryanlin97 0:ad7a811c859f 141
ryanlin97 1:3258d62af038 142 double chair_BNO055::yaw()
ryanlin97 1:3258d62af038 143 {
ryanlin97 1:3258d62af038 144 float gyroZ = chair_BNO055::gyro_z();
ryanlin97 0:ad7a811c859f 145
ryanlin97 1:3258d62af038 146 if(abs(gyroZ) >= .3) {
ryanlin97 1:3258d62af038 147 total_yaw = (total_yaw - t->read()*gyroZ);
ryanlin97 1:3258d62af038 148 //usb->printf("gyroZ %f\n", gyroZ);
ryanlin97 1:3258d62af038 149 }
ryanlin97 1:3258d62af038 150 t->reset();
ryanlin97 1:3258d62af038 151
ryanlin97 1:3258d62af038 152 if(total_yaw > 360)
ryanlin97 1:3258d62af038 153 total_yaw -= 360;
ryanlin97 1:3258d62af038 154 if(total_yaw < 0)
ryanlin97 1:3258d62af038 155 total_yaw += 360;
ryanlin97 1:3258d62af038 156
ryanlin97 1:3258d62af038 157 //usb->printf("yaw %f\n", total_yaw);
ryanlin97 1:3258d62af038 158 return (double)total_yaw;
jvfausto 4:a6452220ec66 159 }