Sheila Pham
/
IMU6050Ver1
Added for gyro and testing
IMU6050/IMUWheelchair.cpp
- Committer:
- jahutchi
- Date:
- 2019-07-03
- Revision:
- 13:d66766523196
- Child:
- 14:365c1c1bf6ee
File content as of revision 13:d66766523196:
#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; }