ryan lin / chair_BNO055

wrapper class for BNO055

Dependencies:   BNO055

Dependents:   wheelchaircontrol wheelchaircontrolRos

chair_BNO055.cpp@0:ad7a811c859f, 2018-07-20 (annotated)

Committer:
ryanlin97
Date:
Fri Jul 20 17:54:09 2018 +0000
Revision:
0:ad7a811c859f
Child:
1:3258d62af038
save point

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:ad7a811c859f 1 #include "chair_BNO055.h"
ryanlin97 0:ad7a811c859f 2
ryanlin97 0:ad7a811c859f 3 Timer t;
ryanlin97 0:ad7a811c859f 4
ryanlin97 0:ad7a811c859f 5
ryanlin97 0:ad7a811c859f 6 chair_BNO055::chair_BNO055(){
ryanlin97 0:ad7a811c859f 7 imu = new BNO055(SDA, SCL);
ryanlin97 0:ad7a811c859f 8 }
ryanlin97 0:ad7a811c859f 9
ryanlin97 0:ad7a811c859f 10 chair_BNO055::chair_BNO055(PinName sda_pin, PinName scl_pin){
ryanlin97 0:ad7a811c859f 11 imu = new BNO055(sda_pin, scl_pin);
ryanlin97 0:ad7a811c859f 12 }
ryanlin97 0:ad7a811c859f 13
ryanlin97 0:ad7a811c859f 14 void chair_BNO055::setup(){
ryanlin97 0:ad7a811c859f 15 imu->reset();
ryanlin97 0:ad7a811c859f 16 while (imu->check() == 0) {
ryanlin97 0:ad7a811c859f 17 wait(.5);
ryanlin97 0:ad7a811c859f 18 }
ryanlin97 0:ad7a811c859f 19 imu->set_accel_units(MPERSPERS);
ryanlin97 0:ad7a811c859f 20 imu->setmode(OPERATION_MODE_AMG);
ryanlin97 0:ad7a811c859f 21 imu->get_calib();
ryanlin97 0:ad7a811c859f 22 imu->write_calibration_data();
ryanlin97 0:ad7a811c859f 23 imu->set_angle_units(DEGREES);
ryanlin97 0:ad7a811c859f 24 imu->setmode(OPERATION_MODE_AMG); //put into while loop
ryanlin97 0:ad7a811c859f 25 t.start();
ryanlin97 0:ad7a811c859f 26 }
ryanlin97 0:ad7a811c859f 27
ryanlin97 0:ad7a811c859f 28 double chair_BNO055::accel_x(){
ryanlin97 0:ad7a811c859f 29 imu->get_accel();
ryanlin97 0:ad7a811c859f 30 return (double)imu->accel.x;
ryanlin97 0:ad7a811c859f 31 }
ryanlin97 0:ad7a811c859f 32
ryanlin97 0:ad7a811c859f 33 double chair_BNO055::accel_y(){
ryanlin97 0:ad7a811c859f 34 imu->get_accel();
ryanlin97 0:ad7a811c859f 35 return (double)imu->accel.y;
ryanlin97 0:ad7a811c859f 36 }
ryanlin97 0:ad7a811c859f 37
ryanlin97 0:ad7a811c859f 38 double chair_BNO055::accel_z(){
ryanlin97 0:ad7a811c859f 39 imu->get_accel();
ryanlin97 0:ad7a811c859f 40 return (double)imu->accel.z;
ryanlin97 0:ad7a811c859f 41 }
ryanlin97 0:ad7a811c859f 42
ryanlin97 0:ad7a811c859f 43 double chair_BNO055::gyro_x(){
ryanlin97 0:ad7a811c859f 44 imu->get_gyro();
ryanlin97 0:ad7a811c859f 45 return (double)imu->gyro.x;
ryanlin97 0:ad7a811c859f 46 }
ryanlin97 0:ad7a811c859f 47
ryanlin97 0:ad7a811c859f 48 double chair_BNO055::gyro_y(){
ryanlin97 0:ad7a811c859f 49 imu->get_gyro();
ryanlin97 0:ad7a811c859f 50 return (double)imu->gyro.y;
ryanlin97 0:ad7a811c859f 51 }
ryanlin97 0:ad7a811c859f 52
ryanlin97 0:ad7a811c859f 53 double chair_BNO055::gyro_z(){
ryanlin97 0:ad7a811c859f 54 imu->get_gyro();
ryanlin97 0:ad7a811c859f 55 return (double)imu->gyro.z;
ryanlin97 0:ad7a811c859f 56 }
ryanlin97 0:ad7a811c859f 57
ryanlin97 0:ad7a811c859f 58 double chair_BNO055::angle_north(){
ryanlin97 0:ad7a811c859f 59 imu->get_mag();
ryanlin97 0:ad7a811c859f 60 float x = imu->mag.x;
ryanlin97 0:ad7a811c859f 61 float y = imu->mag.y;
ryanlin97 0:ad7a811c859f 62
ryanlin97 0:ad7a811c859f 63 float result = x/y;
ryanlin97 0:ad7a811c859f 64
ryanlin97 0:ad7a811c859f 65 float angleToNorth;
ryanlin97 0:ad7a811c859f 66 if(imu->mag.y>0)
ryanlin97 0:ad7a811c859f 67 angleToNorth = 90.0 - atan(result)*180/PI;
ryanlin97 0:ad7a811c859f 68 else if(imu->mag.y<0)
ryanlin97 0:ad7a811c859f 69 angleToNorth = 270.0 - atan(result)*180/PI;
ryanlin97 0:ad7a811c859f 70 else if(y == 0 && x <= 0)
ryanlin97 0:ad7a811c859f 71 angleToNorth = 180;
ryanlin97 0:ad7a811c859f 72 else if(y == 0 && x > 0)
ryanlin97 0:ad7a811c859f 73 angleToNorth = 0;
ryanlin97 0:ad7a811c859f 74
ryanlin97 0:ad7a811c859f 75 return (double)angleToNorth;
ryanlin97 0:ad7a811c859f 76 }
ryanlin97 0:ad7a811c859f 77
ryanlin97 0:ad7a811c859f 78 double chair_BNO055::roll(){
ryanlin97 0:ad7a811c859f 79 imu->get_accel();
ryanlin97 0:ad7a811c859f 80 imu->get_gyro();
ryanlin97 0:ad7a811c859f 81
ryanlin97 0:ad7a811c859f 82 float roll = atan2(-imu->accel.x ,( sqrt((imu->accel.y * imu->accel.y) +
ryanlin97 0:ad7a811c859f 83 (imu->accel.z * imu->accel.z))));
ryanlin97 0:ad7a811c859f 84 roll = roll*57.3;
ryanlin97 0:ad7a811c859f 85
ryanlin97 0:ad7a811c859f 86 t.reset();
ryanlin97 0:ad7a811c859f 87
ryanlin97 0:ad7a811c859f 88 return (double)roll;
ryanlin97 0:ad7a811c859f 89 }
ryanlin97 0:ad7a811c859f 90
ryanlin97 0:ad7a811c859f 91 double chair_BNO055::pitch(){
ryanlin97 0:ad7a811c859f 92 imu->get_accel();
ryanlin97 0:ad7a811c859f 93 imu->get_gyro();
ryanlin97 0:ad7a811c859f 94
ryanlin97 0:ad7a811c859f 95 float pitch = atan2 (imu->accel.y ,( sqrt ((imu->accel.x * imu->accel.x) +
ryanlin97 0:ad7a811c859f 96 (imu->accel.z * imu->accel.z))));
ryanlin97 0:ad7a811c859f 97 pitch = pitch*57.3;
ryanlin97 0:ad7a811c859f 98
ryanlin97 0:ad7a811c859f 99 t.reset();
ryanlin97 0:ad7a811c859f 100
ryanlin97 0:ad7a811c859f 101 return (double)pitch;
ryanlin97 0:ad7a811c859f 102 }
ryanlin97 0:ad7a811c859f 103
ryanlin97 0:ad7a811c859f 104 double chair_BNO055::yaw(){
ryanlin97 0:ad7a811c859f 105 imu->get_gyro();
ryanlin97 0:ad7a811c859f 106 float yaw = (yaw - t.read()*imu->gyro.z);
ryanlin97 0:ad7a811c859f 107
ryanlin97 0:ad7a811c859f 108 if(yaw > 360)
ryanlin97 0:ad7a811c859f 109 yaw -= 360;
ryanlin97 0:ad7a811c859f 110 if(yaw < 0)
ryanlin97 0:ad7a811c859f 111 yaw += 360;
ryanlin97 0:ad7a811c859f 112
ryanlin97 0:ad7a811c859f 113 t.reset();
ryanlin97 0:ad7a811c859f 114
ryanlin97 0:ad7a811c859f 115 return (double)yaw;
ryanlin97 0:ad7a811c859f 116 }
ryanlin97 0:ad7a811c859f 117
ryanlin97 0:ad7a811c859f 118