Added BNO080Wheelchair.h
Dependents: BNO080_program wheelchaircontrol8 Version1-9 BNO080_program
Diff: BNO080Wheelchair.cpp
- Revision:
- 11:dbe6d8d0ceb1
- Parent:
- 10:9275e6f7bf1b
- Child:
- 12:f013530d8358
diff -r 9275e6f7bf1b -r dbe6d8d0ceb1 BNO080Wheelchair.cpp --- a/BNO080Wheelchair.cpp Tue Jul 30 18:33:45 2019 +0000 +++ b/BNO080Wheelchair.cpp Fri Aug 02 23:34:30 2019 +0000 @@ -5,67 +5,87 @@ BNO080Wheelchair::BNO080Wheelchair(Serial *debugPort, PinName sdaPin, PinName sclPin, PinName intPin, PinName rstPin, uint8_t i2cAddress, int i2cPortpeed) { - imu = new BNO080(usb, - SDA, - SCL, - INT_PIN, - RST_PIN, - 0x4a, - 100000); + imu = new BNO080(debugPort, sdaPin, sclPin, intPin,rstPin,i2cAddress, i2cPortpeed); //setUp } //Check if all the void BNO080Wheelchair::setup() { imu -> begin(); + //Tell the IMU to report every 100ms + imu -> enableReport(BNO080::TOTAL_ACCELERATION, 200); + imu -> enableReport(BNO080::LINEAR_ACCELERATION, 200); + imu -> enableReport(BNO080::GRAVITY_ACCELERATION, 200); + imu -> enableReport(BNO080::GYROSCOPE, 200); + imu -> enableReport(BNO080::MAG_FIELD, 200); +// imu -> enableReport(BNO080::MAG_FIELD_UNCALIBRATED, 100); +// imu -> enableReport(BNO080::ROTATION, 100); +// imu -> enableReport(BNO080::GEOMAGNETIC_ROTATION, 100); +// imu -> enableReport(BNO080::GAME_ROTATION, 100); +// imu -> enableReport(BNO080::TAP_DETECTOR, 100); +// imu -> enableReport(BNO080::STABILITY_CLASSIFIER, 100); +// imu -> enableReport(BNO080::STEP_DETECTOR, 100); +// imu -> enableReport(BNO080::STEP_COUNTER, 100); +// imu -> enableReport(BNO080::SIGNIFICANT_MOTION, 100); +// imu -> enableReport(BNO080::SHAKE_DETECTOR, 100); + } +//Get the x component of the angular velocity from IMU. Stores the component +//in a float array //Returns a double, the value of the x-acceleration (m/s^2) -double BNO080Wheelchair::gyro_x() { - imu -> updateData(); // hasNewData()? - return (double)gyroRotation[0]; +double BNO080Wheelchair::gyro_x() { + wait(0.05); + imu -> updateData(); + return (double)imu -> gyroRotation[0]; } //Get the y component of the angular velocity from IMU. Stores the component //in a float array //Returns a double, the value of the y-acceleration (m/s^2) double BNO080Wheelchair::gyro_y() { - imu -> updateData(); // hasNewData()? - return (double)gyroRotation[1]; + wait(0.05); + imu -> updateData(); + return (double)imu -> gyroRotation[1]; } //Get the z component of the angular velocity from IMU. Stores the component //in a float array //Returns a double, the value of the z-acceleration (m/s^2) double BNO080Wheelchair::gyro_z() { - imu -> updateData(); // hasNewData()? - return (double)gyroRotation[2]; + wait(0.05); + imu -> updateData(); + return (double)imu -> gyroRotation[2]; } //Get the x component of the linear acceleration from IMU. Stores the component //in a float array -//Returns a double, the value of the z-acceleration (m/s^2) +//Returns a double, the value of the x-acceleration (m/s^2) double BNO080Wheelchair::accel_x() { - imu -> updateData(); // hasNewData()? - return (double)linearAcceleration[0]; + wait(0.05); + imu -> updateData(); + return (double)imu -> totalAcceleration[0]; } //Get the y component of the linear acceleration from IMU. Stores the component //in a float array -//Returns a double, the value of the z-acceleration (m/s^2) +//Returns a double, the value of the y-acceleration (m/s^2) double BNO080Wheelchair::accel_y() { - imu -> updateData(); // hasNewData()? - return (double)linearAcceleration[1]; + wait(0.05); + imu -> updateData(); + return (double)imu -> totalAcceleration[1]; } //Get the z component of the linear acceleration from IMU. Stores the component //in a float array //Returns a double, the value of the z-acceleration (m/s^2) double BNO080Wheelchair::accel_z() { - imu -> updateData(); // hasNewData()? - return (double)linearAcceleration[2]; + wait(0.05); + imu -> updateData(); + return (double)imu -> totalAcceleration[2]; } +//Get yaw double BNO080Wheelchair::yaw() { float gyroZ = .4+(BNO080Wheelchair::gyro_x())*180/3.141593; @@ -82,23 +102,93 @@ return (double)total_yaw; } -double BNO080Wheelchair::compass() { +//Get x component of magnetic field vector +double BNO080Wheelchair::mag_x() { + wait(1); imu -> updateData(); - - float Pi = 3.14159; - - // Calculate the angle of the vector y,x - double heading = (atan2(magField[1],magField[0]) * 180) / Pi; - - // Normalize to 0-360 - if (heading < 0) - { - heading = 360 + heading; - } - - return heading; + return (double)imu -> magField[0]; +} + +//Get y component of magnetic field vector +double BNO080Wheelchair::mag_y() { + wait(1); + imu -> updateData(); + return (double)imu -> magField[1]; +} + +//Get z component of magnetic field vector +double BNO080Wheelchair::mag_z() { + wait(1); + imu -> updateData(); + return (double)imu -> magField[2]; } +//Check if IMU is pointing in one of the 4 cardinal directions (NSWE) +char BNO080Wheelchair::compass() { + imu -> updateData(); + double x = imu -> magField[0]; + double y = imu -> magField[1]; + + if ( (x>10) && (y<-5) && (y>-10) ) { + return 'N'; //Facing NORTH + } + + else if ( (x<-8) && (y>-5) ) { + return 'S'; //Facing SOUTH + } + + else if ( (x>-3) && (x<4) && (y<-15) ) { + return 'W'; //Facing WEST + } + else if ( (x>4) && (x<8) && (y>1) ) { + return 'E'; //Facing EAST + } + + else if ( (x==0) && (y==0) ) { + return 'O'; //IMU reports not enabled correctly, zeros received + } + + else { + return 'I'; //Direction indeterminate + } +} +//Get the rotation of the IMU (from magnetic north) in radians +Quaternion BNO080Wheelchair::rotation() { + wait(0.05); + //printf("Update Data GYRO X: %d \n", imu -> updateData()); // hasNewData()? + imu -> updateData(); + //wait(0.05); + return imu -> rotationVector; +} +/* +//Returns Qw component of rotation vector +double BNO080Wheelchair::rot_w() { + wait(0.05); + imu -> updateData(); + return (double)imu -> rotationVector[0]; +} + +//Returns Qx component of rotation vector +double BNO080Wheelchair::rot_x() { + wait(0.05); + imu -> updateData(); + return (double)imu -> rotationVector[1]; +} + +//Returns Qy component of rotation vector +double BNO080Wheelchair::rot_y() { + wait(0.05); + imu -> updateData(); + return (double)imu -> rotationVector[2]; +} + +//Returns Qz component of rotation vector +double BNO080Wheelchair::rot_z() { + wait(0.05); + imu -> updateData(); + return (double)imu -> rotationVector[2]; +} +*/ \ No newline at end of file