chair_imu.cpp

00001 #include "chair_imu.h"
00002  
00003 Serial pc(USBTX, USBRX);
00004 Timer t;
00005 
00006 // Default Initialization
00007 chair_imu::chair_imu(){
00008     imu = new BNO055(SDA, SCL);
00009 }
00010 
00011 // Initialize Pins 
00012 chair_imu::chair_imu(PinName SDA_Pin, PinName SCL_Pin){
00013     imu = new BNO055(SDA_Pin, SCL_Pin);
00014 }
00015  
00016 // Check to make sure BNO055 is properly working
00017 void chair_imu::setup(){
00018     imu->reset();
00019     pc.printf("Bosch Sensortec BNO055 test program on \r\n");
00020     while (imu->check() == 0) {
00021         pc.printf("Bosch BNO055 is NOT available!!\r\n");
00022         wait(.5);
00023     }
00024     pc.printf("Bosch Sensortec BNO055 available \r\n");
00025 
00026     //Set the SI units & start the timer
00027     imu->set_accel_units(MPERSPERS);
00028     imu->setmode(OPERATION_MODE_AMG);
00029     imu->set_angle_units(DEGREES);
00030     imu->get_calib();
00031     imu->write_calibration_data();
00032     imu->setmode(OPERATION_MODE_AMG); //put into while loop 
00033     t.start();
00034 }
00035  
00036 // Receive acceleration data (in meters per second)
00037 double chair_imu::accel_x(){
00038     imu->get_accel();
00039     return (double)imu->accel.x;
00040 }
00041  
00042 double chair_imu::accel_y(){
00043     imu->get_accel();
00044     return (double)imu->accel.y;
00045 }
00046  
00047 double chair_imu::accel_z(){
00048     imu->get_accel();
00049     return (double)imu->accel.z;
00050 }
00051  
00052  
00053 // Receive Gyroscope data (in degrees)
00054 double chair_imu::gyro_x(){
00055     imu->get_gyro();
00056     return (double)imu->gyro.x;
00057 }
00058  
00059 double chair_imu::gyro_y(){
00060     imu->get_gyro();
00061     return (double)imu->gyro.y;
00062 }
00063  
00064 double chair_imu::gyro_z(){
00065     imu->get_gyro();
00066     return (double)imu->gyro.z;
00067 }
00068 
00069 
00070 // Receive angle relative to North
00071 double chair_imu::angle_north(){
00072     imu->get_mag();
00073     float x = imu->mag.x;
00074     float y = imu->mag.y;
00075   
00076     float result = x/y;
00077     float angleToNorth;
00078   
00079     if(imu->mag.y > 0)
00080         angleToNorth = 90.0 - atan(result) * 180/PI;
00081     else if(imu->mag.y < 0)
00082         angleToNorth = 270.0 - atan(result) * 180/PI;
00083     else if(y == 0 && x <= 0)
00084         angleToNorth = 180;
00085     else if(y == 0 && x > 0)
00086         angleToNorth = 0;
00087   
00088     return (double)angleToNorth;
00089 }
00090  
00091 // Rotation about the X-axis
00092 double chair_imu::roll(){
00093     imu->get_accel();
00094     imu->get_gyro();
00095   
00096     float roll = atan2 (-imu->accel.x , (sqrt ((imu->accel.y * imu->accel.y) + 
00097                        (imu->accel.z * imu->accel.z)) ));
00098     roll = roll * 57.3;         // Scaling factor   
00099 
00100     t.reset();
00101  
00102     return (double)roll;
00103 }
00104 
00105 // Rotation about the Y-axis
00106 double chair_imu::pitch(){
00107     imu->get_accel();
00108     imu->get_gyro();
00109   
00110     float pitch = atan2 (imu->accel.y , (sqrt ((imu->accel.x * imu->accel.x) + 
00111                         (imu->accel.z * imu->accel.z)) ));
00112     pitch = pitch * 57.3;       // Scaling Factor
00113 
00114     t.reset();
00115 
00116     return (double)pitch;
00117 }
00118  
00119 // Rotation about the Z-axis
00120 double chair_imu::yaw(){
00121     imu->get_gyro();
00122     float yaw = (yaw - t.read() * imu->gyro.z);
00123 
00124     // Return a yaw value between 0 and 360 degrees
00125     if(yaw > 360)
00126     yaw -= 360;
00127     if(yaw < 0)
00128     yaw += 360;
00129 
00130     t.reset();
00131 
00132     return (double)yaw;
00133 }