Affordable Smart Wheelchair / BNOWrapper

Added BNO080Wheelchair.h

Dependents:   BNO080_program wheelchaircontrol8 Version1-9 BNO080_program

BNO080Wheelchair.cpp@11:dbe6d8d0ceb1, 2019-08-02 (annotated)

Committer:
t1jain
Date:
Fri Aug 02 23:34:30 2019 +0000
Revision:
11:dbe6d8d0ceb1
Parent:
10:9275e6f7bf1b
Child:
12:f013530d8358
Added all relevant functions

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 10:9275e6f7bf1b 13 void BNO080Wheelchair::setup() {
sepham 8:217f510db255 14 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 11:dbe6d8d0ceb1 31
t1jain 10:9275e6f7bf1b 32 }
t1jain 10:9275e6f7bf1b 33
t1jain 11:dbe6d8d0ceb1 34 //Get the x component of the angular velocity from IMU. Stores the component
t1jain 11:dbe6d8d0ceb1 35 //in a float array
t1jain 10:9275e6f7bf1b 36 //Returns a double, the value of the x-acceleration (m/s^2)
t1jain 11:dbe6d8d0ceb1 37 double BNO080Wheelchair::gyro_x() {
t1jain 11:dbe6d8d0ceb1 38 wait(0.05);
t1jain 11:dbe6d8d0ceb1 39 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 40 return (double)imu -> gyroRotation[0];
t1jain 10:9275e6f7bf1b 41 }
t1jain 10:9275e6f7bf1b 42
t1jain 10:9275e6f7bf1b 43 //Get the y component of the angular velocity from IMU. Stores the component
t1jain 10:9275e6f7bf1b 44 //in a float array
t1jain 10:9275e6f7bf1b 45 //Returns a double, the value of the y-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 46 double BNO080Wheelchair::gyro_y() {
t1jain 11:dbe6d8d0ceb1 47 wait(0.05);
t1jain 11:dbe6d8d0ceb1 48 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 49 return (double)imu -> gyroRotation[1];
t1jain 10:9275e6f7bf1b 50 }
t1jain 10:9275e6f7bf1b 51
t1jain 10:9275e6f7bf1b 52 //Get the z component of the angular velocity from IMU. Stores the component
t1jain 10:9275e6f7bf1b 53 //in a float array
t1jain 10:9275e6f7bf1b 54 //Returns a double, the value of the z-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 55 double BNO080Wheelchair::gyro_z() {
t1jain 11:dbe6d8d0ceb1 56 wait(0.05);
t1jain 11:dbe6d8d0ceb1 57 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 58 return (double)imu -> gyroRotation[2];
t1jain 10:9275e6f7bf1b 59 }
t1jain 10:9275e6f7bf1b 60
t1jain 10:9275e6f7bf1b 61 //Get the x component of the linear acceleration from IMU. Stores the component
t1jain 10:9275e6f7bf1b 62 //in a float array
t1jain 11:dbe6d8d0ceb1 63 //Returns a double, the value of the x-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 64 double BNO080Wheelchair::accel_x() {
t1jain 11:dbe6d8d0ceb1 65 wait(0.05);
t1jain 11:dbe6d8d0ceb1 66 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 67 return (double)imu -> totalAcceleration[0];
t1jain 10:9275e6f7bf1b 68 }
t1jain 10:9275e6f7bf1b 69
t1jain 10:9275e6f7bf1b 70 //Get the y component of the linear acceleration from IMU. Stores the component
t1jain 10:9275e6f7bf1b 71 //in a float array
t1jain 11:dbe6d8d0ceb1 72 //Returns a double, the value of the y-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 73 double BNO080Wheelchair::accel_y() {
t1jain 11:dbe6d8d0ceb1 74 wait(0.05);
t1jain 11:dbe6d8d0ceb1 75 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 76 return (double)imu -> totalAcceleration[1];
t1jain 10:9275e6f7bf1b 77 }
t1jain 10:9275e6f7bf1b 78
t1jain 10:9275e6f7bf1b 79 //Get the z component of the linear acceleration from IMU. Stores the component
t1jain 10:9275e6f7bf1b 80 //in a float array
t1jain 10:9275e6f7bf1b 81 //Returns a double, the value of the z-acceleration (m/s^2)
t1jain 10:9275e6f7bf1b 82 double BNO080Wheelchair::accel_z() {
t1jain 11:dbe6d8d0ceb1 83 wait(0.05);
t1jain 11:dbe6d8d0ceb1 84 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 85 return (double)imu -> totalAcceleration[2];
t1jain 10:9275e6f7bf1b 86 }
t1jain 10:9275e6f7bf1b 87
t1jain 11:dbe6d8d0ceb1 88 //Get yaw
t1jain 10:9275e6f7bf1b 89 double BNO080Wheelchair::yaw() {
t1jain 10:9275e6f7bf1b 90
t1jain 10:9275e6f7bf1b 91 float gyroZ = .4+(BNO080Wheelchair::gyro_x())*180/3.141593;
t1jain 10:9275e6f7bf1b 92 if(abs(gyroZ) >= .5) {
t1jain 10:9275e6f7bf1b 93 //printf("t->read(): %lf, gyroscope %lf, change %lf\r\n", t->read(), gyroZ, t->read()*gyroZ*2.25);
t1jain 10:9275e6f7bf1b 94 total_yaw = total_yaw - t->read()*gyroZ;
t1jain 10:9275e6f7bf1b 95 //printf("total_yaw: %lf, gyroZ: %f \r\n", total_yaw, gyroZ);
t1jain 10:9275e6f7bf1b 96 }
t1jain 10:9275e6f7bf1b 97 t->reset();
t1jain 10:9275e6f7bf1b 98 if(total_yaw > 360)
t1jain 10:9275e6f7bf1b 99 total_yaw -= 360;
t1jain 10:9275e6f7bf1b 100 if(total_yaw < 0)
t1jain 10:9275e6f7bf1b 101 total_yaw += 360;
t1jain 10:9275e6f7bf1b 102 return (double)total_yaw;
t1jain 10:9275e6f7bf1b 103 }
t1jain 10:9275e6f7bf1b 104
t1jain 11:dbe6d8d0ceb1 105 //Get x component of magnetic field vector
t1jain 11:dbe6d8d0ceb1 106 double BNO080Wheelchair::mag_x() {
t1jain 11:dbe6d8d0ceb1 107 wait(1);
t1jain 10:9275e6f7bf1b 108 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 109 return (double)imu -> magField[0];
t1jain 11:dbe6d8d0ceb1 110 }
t1jain 11:dbe6d8d0ceb1 111
t1jain 11:dbe6d8d0ceb1 112 //Get y component of magnetic field vector
t1jain 11:dbe6d8d0ceb1 113 double BNO080Wheelchair::mag_y() {
t1jain 11:dbe6d8d0ceb1 114 wait(1);
t1jain 11:dbe6d8d0ceb1 115 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 116 return (double)imu -> magField[1];
t1jain 11:dbe6d8d0ceb1 117 }
t1jain 11:dbe6d8d0ceb1 118
t1jain 11:dbe6d8d0ceb1 119 //Get z component of magnetic field vector
t1jain 11:dbe6d8d0ceb1 120 double BNO080Wheelchair::mag_z() {
t1jain 11:dbe6d8d0ceb1 121 wait(1);
t1jain 11:dbe6d8d0ceb1 122 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 123 return (double)imu -> magField[2];
t1jain 10:9275e6f7bf1b 124 }
t1jain 10:9275e6f7bf1b 125
t1jain 11:dbe6d8d0ceb1 126 //Check if IMU is pointing in one of the 4 cardinal directions (NSWE)
t1jain 11:dbe6d8d0ceb1 127 char BNO080Wheelchair::compass() {
t1jain 11:dbe6d8d0ceb1 128 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 129 double x = imu -> magField[0];
t1jain 11:dbe6d8d0ceb1 130 double y = imu -> magField[1];
t1jain 11:dbe6d8d0ceb1 131
t1jain 11:dbe6d8d0ceb1 132 if ( (x>10) && (y<-5) && (y>-10) ) {
t1jain 11:dbe6d8d0ceb1 133 return 'N'; //Facing NORTH
t1jain 11:dbe6d8d0ceb1 134 }
t1jain 11:dbe6d8d0ceb1 135
t1jain 11:dbe6d8d0ceb1 136 else if ( (x<-8) && (y>-5) ) {
t1jain 11:dbe6d8d0ceb1 137 return 'S'; //Facing SOUTH
t1jain 11:dbe6d8d0ceb1 138 }
t1jain 11:dbe6d8d0ceb1 139
t1jain 11:dbe6d8d0ceb1 140 else if ( (x>-3) && (x<4) && (y<-15) ) {
t1jain 11:dbe6d8d0ceb1 141 return 'W'; //Facing WEST
t1jain 11:dbe6d8d0ceb1 142 }
t1jain 10:9275e6f7bf1b 143
t1jain 11:dbe6d8d0ceb1 144 else if ( (x>4) && (x<8) && (y>1) ) {
t1jain 11:dbe6d8d0ceb1 145 return 'E'; //Facing EAST
t1jain 11:dbe6d8d0ceb1 146 }
t1jain 11:dbe6d8d0ceb1 147
t1jain 11:dbe6d8d0ceb1 148 else if ( (x==0) && (y==0) ) {
t1jain 11:dbe6d8d0ceb1 149 return 'O'; //IMU reports not enabled correctly, zeros received
t1jain 11:dbe6d8d0ceb1 150 }
t1jain 11:dbe6d8d0ceb1 151
t1jain 11:dbe6d8d0ceb1 152 else {
t1jain 11:dbe6d8d0ceb1 153 return 'I'; //Direction indeterminate
t1jain 11:dbe6d8d0ceb1 154 }
t1jain 11:dbe6d8d0ceb1 155 }
t1jain 10:9275e6f7bf1b 156
t1jain 11:dbe6d8d0ceb1 157 //Get the rotation of the IMU (from magnetic north) in radians
t1jain 11:dbe6d8d0ceb1 158 Quaternion BNO080Wheelchair::rotation() {
t1jain 11:dbe6d8d0ceb1 159 wait(0.05);
t1jain 11:dbe6d8d0ceb1 160 //printf("Update Data GYRO X: %d \n", imu -> updateData()); // hasNewData()?
t1jain 11:dbe6d8d0ceb1 161 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 162 //wait(0.05);
t1jain 11:dbe6d8d0ceb1 163 return imu -> rotationVector;
t1jain 11:dbe6d8d0ceb1 164 }
t1jain 10:9275e6f7bf1b 165
t1jain 11:dbe6d8d0ceb1 166 /*
t1jain 11:dbe6d8d0ceb1 167 //Returns Qw component of rotation vector
t1jain 11:dbe6d8d0ceb1 168 double BNO080Wheelchair::rot_w() {
t1jain 11:dbe6d8d0ceb1 169 wait(0.05);
t1jain 11:dbe6d8d0ceb1 170 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 171 return (double)imu -> rotationVector[0];
t1jain 11:dbe6d8d0ceb1 172 }
t1jain 11:dbe6d8d0ceb1 173
t1jain 11:dbe6d8d0ceb1 174 //Returns Qx component of rotation vector
t1jain 11:dbe6d8d0ceb1 175 double BNO080Wheelchair::rot_x() {
t1jain 11:dbe6d8d0ceb1 176 wait(0.05);
t1jain 11:dbe6d8d0ceb1 177 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 178 return (double)imu -> rotationVector[1];
t1jain 11:dbe6d8d0ceb1 179 }
t1jain 11:dbe6d8d0ceb1 180
t1jain 11:dbe6d8d0ceb1 181 //Returns Qy component of rotation vector
t1jain 11:dbe6d8d0ceb1 182 double BNO080Wheelchair::rot_y() {
t1jain 11:dbe6d8d0ceb1 183 wait(0.05);
t1jain 11:dbe6d8d0ceb1 184 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 185 return (double)imu -> rotationVector[2];
t1jain 11:dbe6d8d0ceb1 186 }
t1jain 11:dbe6d8d0ceb1 187
t1jain 11:dbe6d8d0ceb1 188 //Returns Qz component of rotation vector
t1jain 11:dbe6d8d0ceb1 189 double BNO080Wheelchair::rot_z() {
t1jain 11:dbe6d8d0ceb1 190 wait(0.05);
t1jain 11:dbe6d8d0ceb1 191 imu -> updateData();
t1jain 11:dbe6d8d0ceb1 192 return (double)imu -> rotationVector[2];
t1jain 11:dbe6d8d0ceb1 193 }
t1jain 11:dbe6d8d0ceb1 194 */