IMUWheelchair.cpp

00001 #include "IMUWheelchair.h"
00002 float total_yaw;
00003 
00004 
00005 IMUWheelchair::IMUWheelchair(Serial* out, Timer* time)
00006 {
00007     imu = new MPU6050(SDA, SCL);
00008     usb = out;
00009     t = time;
00010     start = false;
00011 }
00012 
00013 IMUWheelchair::IMUWheelchair(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time){
00014     usb = out;
00015     t = time;
00016     imu = new MPU6050(sda_pin, scl_pin);
00017     IMUWheelchair::setup();
00018     accelD = accelData;
00019     gyroD = gyroData;
00020 }
00021 
00022 void IMUWheelchair::setup() {
00023     imu->setGyroRange(MPU6050_GYRO_RANGE_500);
00024 
00025     if(imu->testConnection()== 0)
00026         printf("not connected\r\n");
00027     else
00028         printf("connected\r\n");
00029     
00030     //timer
00031     t->start();
00032     
00033 }
00034 
00035 //Get the x component of the angular acceleration from IMU. Stores the component
00036 //in a float array
00037 //Returns a double, the value of the x-acceleration (m/s^2)
00038 double IMUWheelchair::accel_x() {  
00039     imu -> getAccelero(accelD); //Change the values in accelerationArray
00040     return (double)accelD[0];
00041 }
00042 
00043 //Get the y component of the angular acceleration from IMU. Stores the component
00044 //in a float array
00045 //Returns a double, the value of the y-acceleration (m/s^2)
00046 double IMUWheelchair::accel_y() {
00047     imu -> getAccelero(accelD); //Change the values in accelerationArray
00048     return (double)accelD[1];   
00049 }
00050 
00051 //Get the z component of the angular acceleration from IMU. Stores the component
00052 //in a float array
00053 //Returns a double, the value of the z-acceleration (m/s^2)
00054 double IMUWheelchair::accel_z() {
00055     imu -> getAccelero(accelD); //Change the values in accelerationArray
00056     return (double)accelD[2];
00057 }
00058 
00059 //Get the x component of the angular velocity from IMU's gyroscope. Stores the 
00060 //component in a float array
00061 //Returns a double, the value of the x-angular velocity (rad/s)
00062 double IMUWheelchair::gyro_x() {
00063 
00064     imu->getGyro(gyroD); //Change the values in gyroArray
00065     return (double)gyroD[0];
00066     
00067 }
00068 
00069 //Get the y component of the angular velocity from IMU's gyroscope. Stores the 
00070 //component in a float array
00071 //Returns a double, the value of the y-angular velocity (rad/s)
00072 double IMUWheelchair::gyro_y() {
00073     imu -> getGyro(gyroD); //Change the values in gyroArray
00074     return (double)gyroD[1];
00075     
00076 }
00077 
00078 //Get the z component of the angular velocity from IMU's gyroscope. Stores the 
00079 //component in a float array
00080 //Returns a double, the value of the z-angular velocity (rad/s)
00081 double IMUWheelchair::gyro_z() {
00082     imu -> getGyro(gyroD); //Change the values in gyroArray
00083     return (double)gyroD[2];
00084 }
00085 
00086 
00087 
00088 //Get the yaw, or the angle turned at a certain time interval
00089 //Return double, the angle or yaw, (degree)
00090 double IMUWheelchair::yaw() {
00091     float gyroZ = IMUWheelchair::gyro_z();
00092     if(abs(gyroZ) >= .3) {
00093      printf("t->read(): %lf", t->read());
00094      total_yaw = total_yaw - t->read()*gyroZ;
00095      
00096      printf("total_yaw: %lf, gyroZ: %f \r\n", total_yaw, gyroZ);
00097     }
00098     t->reset();
00099     if(total_yaw > 360)
00100         total_yaw -= 360;
00101     if(total_yaw < 0)
00102         total_yaw += 360;
00103     return (double)total_yaw;
00104 }
00105 
00106 
00107 double IMUWheelchair::pitch()
00108  {
00109     imu->getAccelero(accelD);
00110     float pitch = atan2 (-accelD[1] ,( sqrt (accelD[0] * accelD[0]) +(accelD[2]  *accelD[2])));
00111     pitch = pitch*57.3;
00112     return (double)pitch;
00113 }
00114 
00115 double IMUWheelchair::roll() {
00116     imu->getAccelero(accelD);
00117     float roll = atan2(-accelD[0]  ,( sqrt((accelD[1] *accelD[1] ) + 
00118  (accelD[2] * accelD[2]))));
00119     roll = roll*57.3;
00120     t->reset();
00121     return (double)roll;
00122 }