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?
User | Revision | Line number | New 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 | */ |