LG1300L.cpp

00001 #include "mbed.h"
00002 
00003 #include "LG1300L.h"
00004 
00005 
00006 LG1300L::LG1300L(I2C& i2c, int ACC_SETTING) 
00007     :  ACC_RANGE(ACC_SETTING), IMU(i2c), IMU_ADDR(0x02){
00008 
00009     ANGLE_LSB_REG[0] = 0x42;
00010     ANGLE_MSB_REG[0] = 0x43;
00011     ROT_LSB_REG[0] = 0x44;
00012     ROT_MSB_REG[0] = 0x45;
00013     X_ACC_LSB_REG[0] = 0x46;
00014     X_ACC_MSB_REG[0] = 0x47;
00015     Y_ACC_LSB_REG[0] = 0x48;
00016     Y_ACC_MSB_REG[0] = 0x49;
00017     Z_ACC_LSB_REG[1] = 0x4A;
00018     Z_ACC_MSB_REG[1] = 0x4B;
00019  
00020     RESET_REG[0] = 0x60;
00021     SELECT_2G_REG[0] = 0x61;
00022     SELECT_4G_REG[0] = 0x62;
00023     SELECT_8G_REG[0] = 0x63;
00024     
00025     init();
00026 }
00027 
00028 
00029 void LG1300L::init(){
00030     
00031     IMU.frequency(9600);
00032     IMU.write(IMU_ADDR, RESET_REG, 1);
00033     switch(ACC_RANGE){
00034         case 2: IMU.write(IMU_ADDR, SELECT_2G_REG, 1);
00035                 break;
00036         case 4: IMU.write(IMU_ADDR, SELECT_4G_REG, 1);
00037                 break;
00038         case 8: IMU.write(IMU_ADDR, SELECT_8G_REG, 1);
00039                 break;
00040         default: break;
00041     }
00042 }
00043 
00044 float LG1300L::GetAngle(){
00045     
00046     IMU.write(IMU_ADDR, ANGLE_LSB_REG , 1);
00047     IMU.read(IMU_ADDR, data, 2);
00048     wait(.1);
00049     ANGLE_MSB = data[1];
00050     ANGLE_LSB = data[0];
00051     ANGLE_MSB = ((ANGLE_MSB << 8)+ANGLE_LSB);
00052     CALC_ANG = ((float)ANGLE_MSB)/ 100.0f;
00053     return CALC_ANG;
00054 }
00055 
00056 float LG1300L::GetROT(){
00057 
00058     IMU.write(IMU_ADDR, ROT_LSB_REG , 1);
00059     IMU.read(IMU_ADDR, data, 2);
00060     ROT_MSB = data[1];
00061     ROT_LSB = data[0];
00062     ROT_MSB = (ROT_MSB<<8)+ROT_LSB;
00063     CALC_ROT = ((float)ROT_MSB)/100.0f;
00064     return CALC_ROT;
00065 }
00066 
00067 float LG1300L::GetACC_X(){
00068     IMU.write(IMU_ADDR, X_ACC_LSB_REG , 1);
00069     IMU.read(IMU_ADDR, data, 2);
00070     ACC_X_MSB = data[1];
00071     ACC_X_LSB = data[0];
00072     ACC_X_MSB = (ACC_X_MSB<<8)+ ACC_X_LSB;
00073     CALC_X = ((float)ACC_X_MSB/1000.0f)*(float)ACC_RANGE/2.0f;
00074     return CALC_X;
00075 }
00076 
00077 float LG1300L::GetACC_Y(){
00078 
00079     IMU.write(IMU_ADDR, Y_ACC_LSB_REG , 1);
00080     IMU.read(IMU_ADDR, data, 2);
00081     ACC_Y_MSB = data[1];
00082     ACC_Y_LSB = data[0];
00083     ACC_Y_MSB = (ACC_Y_MSB<<8)+ ACC_Y_LSB;
00084     CALC_Y = ((float)ACC_Y_MSB/1000.0f)*(float)ACC_RANGE/2.0f;
00085     return CALC_Y;
00086 }
00087 
00088 float LG1300L::GetACC_Z(){
00089 
00090     IMU.write(IMU_ADDR, Z_ACC_LSB_REG , 1);
00091     IMU.read(IMU_ADDR, data, 2);
00092     ACC_Z_MSB = data[1];
00093     ACC_Z_LSB = data[0];
00094     ACC_Z_MSB = (ACC_Z_MSB<<8)+ ACC_Z_LSB;
00095     CALC_Z = ((float)ACC_Z_MSB/1000.0f)*(float)ACC_RANGE/2.0f;
00096     return CALC_Z;
00097 }