Added BNO080Wheelchair.h
Dependents: BNO080_program wheelchaircontrol8 Version1-9 BNO080_program
Diff: BNO080Wheelchair.cpp
- Revision:
- 10:9275e6f7bf1b
- Parent:
- 8:217f510db255
- Child:
- 11:dbe6d8d0ceb1
diff -r a12af7cb3c6d -r 9275e6f7bf1b BNO080Wheelchair.cpp --- a/BNO080Wheelchair.cpp Mon Jul 29 18:24:40 2019 +0000 +++ b/BNO080Wheelchair.cpp Tue Jul 30 18:33:45 2019 +0000 @@ -1,14 +1,104 @@ -#include "BNO80Wheelchair.h" +#include "BNO080Wheelchair.h" +float total_yaw; -//The constructor for the BNO80 imu. Needs 7 parameters -BNO80Wheelchair::BNO80Wheelchair(Serial *debugPort, PinName sdaPin, +//The constructor for the BNO080 imu. Needs 7 parameters +BNO080Wheelchair::BNO080Wheelchair(Serial *debugPort, PinName sdaPin, PinName sclPin, PinName intPin, PinName rstPin, uint8_t i2cAddress, int i2cPortpeed) { - imu = new BNO80(); + imu = new BNO080(usb, + SDA, + SCL, + INT_PIN, + RST_PIN, + 0x4a, + 100000); //setUp } //Check if all the -void BNO80Wheelchair::setup() { +void BNO080Wheelchair::setup() { imu -> begin(); -} \ No newline at end of file +} + +//Returns a double, the value of the x-acceleration (m/s^2) +double BNO080Wheelchair::gyro_x() { + imu -> updateData(); // hasNewData()? + return (double)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]; +} + +//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]; +} + +//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) +double BNO080Wheelchair::accel_x() { + imu -> updateData(); // hasNewData()? + return (double)linearAcceleration[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) +double BNO080Wheelchair::accel_y() { + imu -> updateData(); // hasNewData()? + return (double)linearAcceleration[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]; +} + +double BNO080Wheelchair::yaw() { + + float gyroZ = .4+(BNO080Wheelchair::gyro_x())*180/3.141593; + if(abs(gyroZ) >= .5) { + //printf("t->read(): %lf, gyroscope %lf, change %lf\r\n", t->read(), gyroZ, t->read()*gyroZ*2.25); + total_yaw = total_yaw - t->read()*gyroZ; + //printf("total_yaw: %lf, gyroZ: %f \r\n", total_yaw, gyroZ); + } + t->reset(); + if(total_yaw > 360) + total_yaw -= 360; + if(total_yaw < 0) + total_yaw += 360; + return (double)total_yaw; +} + +double BNO080Wheelchair::compass() { + 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; +} + + + +