c
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sat Jul 16 2022 00:13:05 by 1.7.2