GenCMD.cpp

00001 #include "GenCMD.h"
00002 #include <cmath>
00003 
00004 #define LSM9DS0_XM_ADDR  0x1D // Would be 0x1E if SDO_XM is LOW
00005 #define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW
00006 
00007 
00008 GenCMD::GenCMD():imu(p28,p27,LSM9DS0_G_ADDR,LSM9DS0_XM_ADDR){}
00009 
00010 void GenCMD::init()
00011 {
00012     imu.begin();
00013 }
00014 
00015 void GenCMD::GenerateCMD(char& cmd_buffer, const size_t sz)
00016 { 
00017     imu.readAccel();
00018     
00019     if(imu.fay > 0.0 && imu.fay <= 0.20)
00020     {
00021         cmd_buffer = STRAIGHT;
00022     }
00023     else if(imu.fay > 0.20 && imu.fay <= 0.45)
00024     {
00025         cmd_buffer = TURN_LEFT;
00026     }
00027     else if(imu.fay > 0.45)
00028     {
00029         cmd_buffer = TURN_LEFT;
00030     }
00031     else if(imu.fay < 0.0 && imu.fay >= -0.20)
00032     {
00033         cmd_buffer = STRAIGHT;
00034     }
00035     else if(imu.fay < -0.20 && imu.fay >= -0.45)
00036     {
00037         cmd_buffer = TURN_RIGHT;
00038     }
00039     else if(imu.fay < -0.45)
00040     {
00041         cmd_buffer = TURN_RIGHT;
00042     }
00043     
00044     if(abs(imu.fax) < 0.25 )
00045     {
00046         cmd_buffer |= SPEED1;
00047     }
00048     else if(abs(imu.fax) > 0.25 && abs(imu.fax) < 0.45)
00049     {
00050         cmd_buffer |= SPEED2;
00051     }
00052     else if(abs(imu.fax) > 0.45)
00053     {
00054         cmd_buffer |= SPEED3;
00055     }
00056     
00057     if(imu.fax < 0 && (cmd_buffer & 0x8)==0)
00058     {
00059         cmd_buffer |= NEGATIVE;
00060     }
00061 }