File content as of revision 2:3f2d4f53ceed:

#include "AigamozuControlPackets.h"
#include "VNH5019.h"


AigamozuControlPackets::AigamozuControlPackets(){
    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::getPacketData(){
    return packetData;
    }

int AigamozuControlPackets::getPacketLength(){
        return packetLength;
    }

uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){
    return buf[14];
    }
    
bool AigamozuControlPackets::changeMode(uint8_t *buf){
    
    switch(buf[19]){
        
        case 0:
            nowMode = STANDBY_MODE;
        break;
        
        case 1:
            nowMode = MANUAL_MODE;
        break;
        
        case 2:
            nowMode = AUTO_MODE;
        break;
        }
    return false;
}