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;
+}