Affordable Smart Wheelchair / BNOWrapper

Added BNO080Wheelchair.h

Dependents:   BNO080_program wheelchaircontrol8 Version1-9 BNO080_program

BNO080Wheelchair.cpp@12:f013530d8358, 2019-08-07 (annotated)

Committer:
t1jain
Date:
Wed Aug 07 18:49:44 2019 +0000
Revision:
12:f013530d8358
Parent:
11:dbe6d8d0ceb1 
Updated Quaternion to TVector4

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t1jain 10:9275e6f7bf1b 1 #include "BNO080Wheelchair.h"
t1jain 10:9275e6f7bf1b 2 float total_yaw;
sepham 8:217f510db255 3
t1jain 10:9275e6f7bf1b 4 //The constructor for the BNO080 imu. Needs 7 parameters
t1jain 10:9275e6f7bf1b 5 BNO080Wheelchair::BNO080Wheelchair(Serial *debugPort, PinName sdaPin,
sepham 8:217f510db255 6 PinName sclPin, PinName intPin, PinName rstPin,
sepham 8:217f510db255 7 uint8_t i2cAddress, int i2cPortpeed) {
t1jain 11:dbe6d8d0ceb1 8 imu = new BNO080(debugPort, sdaPin, sclPin, intPin,rstPin,i2cAddress, i2cPortpeed);
sepham 8:217f510db255 9 //setUp
sepham 8:217f510db255 10
sepham 8:217f510db255 11 }
sepham 8:217f510db255 12 //Check if all the
t1jain 12:f013530d8358 13 bool BNO080Wheelchair::setup() {
t1jain 12:f013530d8358 14 bool setup = imu -> begin();
t1jain 11:dbe6d8d0ceb1 15 //Tell the IMU to report every 100ms
t1jain 11:dbe6d8d0ceb1 16 imu -> enableReport(BNO080::TOTAL_ACCELERATION, 200);
t1jain 11:dbe6d8d0ceb1 17 imu -> enableReport(BNO080::LINEAR_ACCELERATION, 200);
t1jain 11:dbe6d8d0ceb1 18 imu -> enableReport(BNO080::GRAVITY_ACCELERATION, 200);
t1jain 11:dbe6d8d0ceb1 19 imu -> enableReport(BNO080::GYROSCOPE, 200);
t1jain 11:dbe6d8d0ceb1 20 imu -> enableReport(BNO080::MAG_FIELD, 200);
t1jain 11:dbe6d8d0ceb1 21 // imu -> enableReport(BNO080::MAG_FIELD_UNCALIBRATED, 100);
t1jain 11:dbe6d8d0ceb1 22 // imu -> enableReport(BNO080::ROTATION, 100);
t1jain 11:dbe6d8d0ceb1 23 // imu -> enableReport(BNO080::GEOMAGNETIC_ROTATION, 100);
t1jain 11:dbe6d8d0ceb1 24 // imu -> enableReport(BNO080::GAME_ROTATION, 100);
t1jain 11:dbe6d8d0ceb1 25 // imu -> enableReport(BNO080::TAP_DETECTOR, 100);
t1jain 11:dbe6d8d0ceb1 26 // imu -> enableReport(BNO080::STABILITY_CLASSIFIER, 100);
t1jain 11:dbe6d8d0ceb1 27 // imu -> enableReport(BNO080::STEP_DETECTOR, 100);
t1jain 11:dbe6d8d0ceb1 28 // imu -> enableReport(BNO080::STEP_COUNTER, 100);
t1jain 11:dbe6d8d0ceb1 29 // imu -> enableReport(BNO080::SIGNIFICANT_MOTION, 100);
t1jain 11:dbe6d8d0ceb1 30 // imu -> enableReport(BNO080::SHAKE_DETECTOR, 100);
t1jain 12:f013530d8358 31 return setup;
t1jain 12:f013530d8358 32 }
t1jain 11:dbe6d8d0ceb1 33
t1jain 12:f013530d8358 34 bool BNO080Wheelchair::hasNewData(BNO080::Report report) {
t1jain 12:f013530d8358 35 return imu -> hasNewData(report);
t1jain 10:9275e6f7bf1b 36 }
t1jain 10:9275e6f7bf1b 37
t1jain 11:dbe6d8d0ceb1 38 //Get the x component of the angular velocity from IMU. Stores the component
t1jain 11:dbe6d8d0ceb1 39 //in a float array
t1jain 10:9275e6f7bf1b 40 //Returns a double, the value of the x-acceleration (m/s^2)
t1jain 11:dbe6d8d0ceb1 41 double BNO080Wheelchair::gyro_x() {
t1jain 11:dbe6d8d0ceb1 42 wait(0.05);
t1jain 11:dbe6d8d0ceb1 43 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 44 return (double)imu -> gyroRotation[0];
t1jain 10:9275e6f7bf1b 45 }
t1jain 10:9275e6f7bf1b 46
t1jain 10:9275e6f7bf1b 47 //Get the y component of the angular velocity from IMU. Stores the component
t1jain 10:9275e6f7bf1b 48 //in a float array
t1jain 10:9275e6f7bf1b 49 //Returns a double, the value of the y-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 50 double BNO080Wheelchair::gyro_y() {
t1jain 11:dbe6d8d0ceb1 51 wait(0.05);
t1jain 11:dbe6d8d0ceb1 52 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 53 return (double)imu -> gyroRotation[1];
t1jain 10:9275e6f7bf1b 54 }
t1jain 10:9275e6f7bf1b 55
t1jain 10:9275e6f7bf1b 56 //Get the z component of the angular velocity from IMU. Stores the component
t1jain 10:9275e6f7bf1b 57 //in a float array
t1jain 10:9275e6f7bf1b 58 //Returns a double, the value of the z-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 59 double BNO080Wheelchair::gyro_z() {
t1jain 11:dbe6d8d0ceb1 60 wait(0.05);
t1jain 11:dbe6d8d0ceb1 61 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 62 return (double)imu -> gyroRotation[2];
t1jain 10:9275e6f7bf1b 63 }
t1jain 10:9275e6f7bf1b 64
t1jain 10:9275e6f7bf1b 65 //Get the x component of the linear acceleration from IMU. Stores the component
t1jain 10:9275e6f7bf1b 66 //in a float array
t1jain 11:dbe6d8d0ceb1 67 //Returns a double, the value of the x-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 68 double BNO080Wheelchair::accel_x() {
t1jain 11:dbe6d8d0ceb1 69 wait(0.05);
t1jain 11:dbe6d8d0ceb1 70 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 71 return (double)imu -> totalAcceleration[0];
t1jain 10:9275e6f7bf1b 72 }
t1jain 10:9275e6f7bf1b 73
t1jain 10:9275e6f7bf1b 74 //Get the y component of the linear acceleration from IMU. Stores the component
t1jain 10:9275e6f7bf1b 75 //in a float array
t1jain 11:dbe6d8d0ceb1 76 //Returns a double, the value of the y-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 77 double BNO080Wheelchair::accel_y() {
t1jain 11:dbe6d8d0ceb1 78 wait(0.05);
t1jain 11:dbe6d8d0ceb1 79 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 80 return (double)imu -> totalAcceleration[1];
t1jain 10:9275e6f7bf1b 81 }
t1jain 10:9275e6f7bf1b 82
t1jain 10:9275e6f7bf1b 83 //Get the z component of the linear acceleration from IMU. Stores the component
t1jain 10:9275e6f7bf1b 84 //in a float array
t1jain 10:9275e6f7bf1b 85 //Returns a double, the value of the z-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 86 double BNO080Wheelchair::accel_z() {
t1jain 11:dbe6d8d0ceb1 87 wait(0.05);
t1jain 11:dbe6d8d0ceb1 88 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 89 return (double)imu -> totalAcceleration[2];
t1jain 10:9275e6f7bf1b 90 }
t1jain 10:9275e6f7bf1b 91
t1jain 11:dbe6d8d0ceb1 92 //Get yaw
t1jain 10:9275e6f7bf1b 93 double BNO080Wheelchair::yaw() {
t1jain 10:9275e6f7bf1b 94
t1jain 10:9275e6f7bf1b 95 float gyroZ = .4+(BNO080Wheelchair::gyro_x())*180/3.141593;
t1jain 10:9275e6f7bf1b 96 if(abs(gyroZ) >= .5) {
t1jain 10:9275e6f7bf1b 97 //printf("t->read(): %lf, gyroscope %lf, change %lf\r\n", t->read(), gyroZ, t->read()*gyroZ*2.25);
t1jain 10:9275e6f7bf1b 98 total_yaw = total_yaw - t->read()*gyroZ;
t1jain 10:9275e6f7bf1b 99 //printf("total_yaw: %lf, gyroZ: %f \r\n", total_yaw, gyroZ);
t1jain 10:9275e6f7bf1b 100 }
t1jain 10:9275e6f7bf1b 101 t->reset();
t1jain 10:9275e6f7bf1b 102 if(total_yaw > 360)
t1jain 10:9275e6f7bf1b 103 total_yaw -= 360;
t1jain 10:9275e6f7bf1b 104 if(total_yaw < 0)
t1jain 10:9275e6f7bf1b 105 total_yaw += 360;
t1jain 10:9275e6f7bf1b 106 return (double)total_yaw;
t1jain 10:9275e6f7bf1b 107 }
t1jain 10:9275e6f7bf1b 108
t1jain 11:dbe6d8d0ceb1 109 //Get x component of magnetic field vector
t1jain 11:dbe6d8d0ceb1 110 double BNO080Wheelchair::mag_x() {
t1jain 11:dbe6d8d0ceb1 111 wait(1);
t1jain 10:9275e6f7bf1b 112 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 113 return (double)imu -> magField[0];
t1jain 11:dbe6d8d0ceb1 114 }
t1jain 11:dbe6d8d0ceb1 115
t1jain 11:dbe6d8d0ceb1 116 //Get y component of magnetic field vector
t1jain 11:dbe6d8d0ceb1 117 double BNO080Wheelchair::mag_y() {
t1jain 11:dbe6d8d0ceb1 118 wait(1);
t1jain 11:dbe6d8d0ceb1 119 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 120 return (double)imu -> magField[1];
t1jain 11:dbe6d8d0ceb1 121 }
t1jain 11:dbe6d8d0ceb1 122
t1jain 11:dbe6d8d0ceb1 123 //Get z component of magnetic field vector
t1jain 11:dbe6d8d0ceb1 124 double BNO080Wheelchair::mag_z() {
t1jain 11:dbe6d8d0ceb1 125 wait(1);
t1jain 11:dbe6d8d0ceb1 126 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 127 return (double)imu -> magField[2];
t1jain 10:9275e6f7bf1b 128 }
t1jain 10:9275e6f7bf1b 129
t1jain 11:dbe6d8d0ceb1 130 //Check if IMU is pointing in one of the 4 cardinal directions (NSWE)
t1jain 11:dbe6d8d0ceb1 131 char BNO080Wheelchair::compass() {
t1jain 11:dbe6d8d0ceb1 132 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 133 double x = imu -> magField[0];
t1jain 11:dbe6d8d0ceb1 134 double y = imu -> magField[1];
t1jain 11:dbe6d8d0ceb1 135
t1jain 11:dbe6d8d0ceb1 136 if ( (x>10) && (y<-5) && (y>-10) ) {
t1jain 11:dbe6d8d0ceb1 137 return 'N'; //Facing NORTH
t1jain 11:dbe6d8d0ceb1 138 }
t1jain 11:dbe6d8d0ceb1 139
t1jain 11:dbe6d8d0ceb1 140 else if ( (x<-8) && (y>-5) ) {
t1jain 11:dbe6d8d0ceb1 141 return 'S'; //Facing SOUTH
t1jain 11:dbe6d8d0ceb1 142 }
t1jain 11:dbe6d8d0ceb1 143
t1jain 11:dbe6d8d0ceb1 144 else if ( (x>-3) && (x<4) && (y<-15) ) {
t1jain 11:dbe6d8d0ceb1 145 return 'W'; //Facing WEST
t1jain 11:dbe6d8d0ceb1 146 }
t1jain 10:9275e6f7bf1b 147
t1jain 11:dbe6d8d0ceb1 148 else if ( (x>4) && (x<8) && (y>1) ) {
t1jain 11:dbe6d8d0ceb1 149 return 'E'; //Facing EAST
t1jain 11:dbe6d8d0ceb1 150 }
t1jain 11:dbe6d8d0ceb1 151
t1jain 11:dbe6d8d0ceb1 152 else if ( (x==0) && (y==0) ) {
t1jain 11:dbe6d8d0ceb1 153 return 'O'; //IMU reports not enabled correctly, zeros received
t1jain 11:dbe6d8d0ceb1 154 }
t1jain 11:dbe6d8d0ceb1 155
t1jain 11:dbe6d8d0ceb1 156 else {
t1jain 11:dbe6d8d0ceb1 157 return 'I'; //Direction indeterminate
t1jain 11:dbe6d8d0ceb1 158 }
t1jain 11:dbe6d8d0ceb1 159 }
t1jain 10:9275e6f7bf1b 160
t1jain 11:dbe6d8d0ceb1 161 //Get the rotation of the IMU (from magnetic north) in radians
t1jain 12:f013530d8358 162 TVector4 BNO080Wheelchair::rotation() {
t1jain 11:dbe6d8d0ceb1 163 wait(0.05);
t1jain 11:dbe6d8d0ceb1 164 //printf("Update Data GYRO X: %d \n", imu -> updateData()); // hasNewData()?
t1jain 11:dbe6d8d0ceb1 165 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 166 //wait(0.05);
t1jain 12:f013530d8358 167 return imu -> rotationVector.vector();
t1jain 11:dbe6d8d0ceb1 168 }
t1jain 10:9275e6f7bf1b 169
t1jain 11:dbe6d8d0ceb1 170 /*
t1jain 11:dbe6d8d0ceb1 171 //Returns Qw component of rotation vector
t1jain 11:dbe6d8d0ceb1 172 double BNO080Wheelchair::rot_w() {
t1jain 11:dbe6d8d0ceb1 173 wait(0.05);
t1jain 11:dbe6d8d0ceb1 174 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 175 return (double)imu -> rotationVector[0];
t1jain 11:dbe6d8d0ceb1 176 }
t1jain 11:dbe6d8d0ceb1 177
t1jain 11:dbe6d8d0ceb1 178 //Returns Qx component of rotation vector
t1jain 11:dbe6d8d0ceb1 179 double BNO080Wheelchair::rot_x() {
t1jain 11:dbe6d8d0ceb1 180 wait(0.05);
t1jain 11:dbe6d8d0ceb1 181 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 182 return (double)imu -> rotationVector[1];
t1jain 11:dbe6d8d0ceb1 183 }
t1jain 11:dbe6d8d0ceb1 184
t1jain 11:dbe6d8d0ceb1 185 //Returns Qy component of rotation vector
t1jain 11:dbe6d8d0ceb1 186 double BNO080Wheelchair::rot_y() {
t1jain 11:dbe6d8d0ceb1 187 wait(0.05);
t1jain 11:dbe6d8d0ceb1 188 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 189 return (double)imu -> rotationVector[2];
t1jain 11:dbe6d8d0ceb1 190 }
t1jain 11:dbe6d8d0ceb1 191
t1jain 11:dbe6d8d0ceb1 192 //Returns Qz component of rotation vector
t1jain 11:dbe6d8d0ceb1 193 double BNO080Wheelchair::rot_z() {
t1jain 11:dbe6d8d0ceb1 194 wait(0.05);
t1jain 11:dbe6d8d0ceb1 195 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 196 return (double)imu -> rotationVector[2];
t1jain 11:dbe6d8d0ceb1 197 }
t1jain 11:dbe6d8d0ceb1 198 */