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