20150512 Nakazawa
Fork of AigamozuControlPackets by
AigamozuControlPackets.cpp
- Committer:
- m5171135
- Date:
- 2014-06-08
- Revision:
- 5:3f51eeb5aedc
- Parent:
- 4:04dadf67ecb6
- Child:
- 6:f164a716be46
File content as of revision 5:3f51eeb5aedc:
#include "AigamozuControlPackets.h" #include "VNH5019.h" AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){ packetData = new uint8_t[50]; packetLength = 0; } ////////////////////////////// // Controller/Base -> Robot // ////////////////////////////// void AigamozuControlPackets::createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR) { uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'}; for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; packetLength = RECEIVE_STATUS_COMMNAD_LENGTH; } void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID,uint8_t) { uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'}; for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; packetLength = REQUEST_COMMNAD_LENGTH; } void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode){ uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,mode,'A','G','E'}; for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i]; packetLength = CHANGE_MODE_COMMAND_LENGTH; } ////////////////////////////// // Robot -> Controller/Base // ////////////////////////////// void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,long latitudeH,long latitudeL,long longitudeH,long longitudeL) { //useing union's split::: long hoge.a(4byte) == uint8_t hoge.b[4] TEST_T latH,latL,lonH,lonL; latH.a = latitudeH; latL.a = latitudeL; lonH.a = longitudeH; lonL.a = longitudeL; uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S',nowStatus,71,80,83,latH.b[0],latH.b[1],latH.b[2],latH.b[3],latL.b[0],latL.b[1],latL.b[2],latL.b[3],lonH.b[0],lonH.b[1],lonH.b[2],lonH.b[3],lonL.b[0],lonL.b[1],lonL.b[2],lonL.b[3],65,71,69}; for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; packetLength = RECEIVE_STATUS_COMMNAD_LENGTH; } ////////////////////////////// // Using createPacket // ////////////////////////////// uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){ return buf[14]; } uint8_t* AigamozuControlPackets::getPacketData(){ return packetData; } int AigamozuControlPackets::getPacketLength(){ return packetLength; } void AigamozuControlPackets::changeSpeed(uint8_t* buf){ if(nowMode == MANUAL_MODE){ manualCount =0; _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]); } } void AigamozuControlPackets::randomAuto(){ randomCount++; if(randomCount < 10) _agzSheild.changeSpeed(1,128,1,128); else if(randomCount < 11) _agzSheild.changeSpeed(2,128,2,128); else randomCount = 0; } void AigamozuControlPackets::manualMode(){ manualCount++; if(manualCount > 10){ _agzSheild.changeSpeed(0,0,0,0); manualCount = 0; } } bool AigamozuControlPackets::changeMode(uint8_t *buf){ _agzSheild.changeSpeed(0,0,0,0); hoge.detach(); switch(buf[19]){ case 0: nowMode = STANDBY_MODE; break; case 1: hoge.attach(this,&AigamozuControlPackets::manualMode,1.0); nowMode = MANUAL_MODE; break; case 2: nowMode = AUTO_MODE; hoge.attach(this,&AigamozuControlPackets::randomAuto,1.0); break; case 3: nowMode = AUTO_GPS_MODE; break; } return false; }