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?
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 | 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 | */ |