a
Diff: IMU6050/IMUWheelchair.cpp
- Revision:
- 13:d66766523196
- Child:
- 14:365c1c1bf6ee
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU6050/IMUWheelchair.cpp Wed Jul 03 22:09:33 2019 +0000 @@ -0,0 +1,113 @@ +#include "IMUWheelchair.h" +static float total_yaw; + + +IMUWheelchair::IMUWheelchair(Serial* out, Timer* time) +{ + imu = new MPU6050(SDA, SCL); + usb = out; + t = time; + start = false; +} + +IMUWheelchair::IMUWheelchair(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time){ + usb = out; + t = time; + imu = new MPU6050(sda_pin, scl_pin); + IMUWheelchair::setup(); + accelD = accelData; + gyroD = gyroData; +} + +void IMUWheelchair::setup() { + if(imu->testConnection()== 0) + printf("not connected\r\n"); + else + printf("connected\r\n"); + + //timer + t->start(); +} + +//Get the x component of the angular acceleration from IMU. Stores the component +//in a float array +//Returns a double, the value of the x-acceleration (m/s^2) +double IMUWheelchair::accel_x() { + imu -> getAccelero(accelD); //Change the values in accelerationArray + return (double)accelData[0]; +} + +//Get the y component of the angular acceleration from IMU. Stores the component +//in a float array +//Returns a double, the value of the y-acceleration (m/s^2) +double IMUWheelchair::accel_y() { + imu -> getAccelero(accelD); //Change the values in accelerationArray + return (double)accelData[1]; +} + +//Get the z component of the angular acceleration from IMU. Stores the component +//in a float array +//Returns a double, the value of the z-acceleration (m/s^2) +double IMUWheelchair::accel_z() { + imu -> getAccelero(accelD); //Change the values in accelerationArray + return (double)accelData[2]; +} + +//Get the x component of the angular velocity from IMU's gyroscope. Stores the +//component in a float array +//Returns a double, the value of the x-angular velocity (rad/s) +double IMUWheelchair::gyro_x() { + imu -> getGyro(gyroD); //Change the values in gyroArray + return (double)gyroData[0]; + +} + +//Get the y component of the angular velocity from IMU's gyroscope. Stores the +//component in a float array +//Returns a double, the value of the y-angular velocity (rad/s) +double IMUWheelchair::gyro_y() { + imu -> getGyro(gyroD); //Change the values in gyroArray + return (double)gyroData[1]; + +} + +//Get the z component of the angular velocity from IMU's gyroscope. Stores the +//component in a float array +//Returns a double, the value of the z-angular velocity (rad/s) +double IMUWheelchair::gyro_z() { + imu -> getGyro(gyroD); //Change the values in gyroArray + return (double)gyroData[2]; +} + +//Get the yaw, or the angle turned at a certain time interval +//Return double, the angle or yaw, (degree) +double IMUWheelchair::yaw() { + float gyroZ = IMUWheelchair::gyro_z(); + if(abs(gyroZ) >= .3) { + total_yaw = (total_yaw - t->read()*gyroZ); + } + t->reset(); + if(total_yaw > 360) + total_yaw -= 360; + if(total_yaw < 0) + total_yaw += 360; + return (double)total_yaw; +} + + +double IMUWheelchair::pitch() + { + imu->getAccelero(accelD); + float pitch = atan2 (-accelData[1] ,( sqrt (accelData[0] * accelData[0]) +(accelData[2] *accelData[2]))); + pitch = pitch*57.3; + return (double)pitch; +} + +double IMUWheelchair::roll() { + imu->getAccelero(accelD); + float roll = atan2(-accelData[0] ,( sqrt((accelData[1] *accelData[1] ) + + (accelData[2] * accelData[2])))); + roll = roll*57.3; + t->reset(); + return (double)roll; +}