File content as of revision 52:d4d739f01b38:

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

#define LOW_SPEED 16
#define HIGH_SPEED 64
#define FORWARD_SPEED 32

//////////////////////////////
//          Init            //
//////////////////////////////
AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){
    packetData = new uint8_t[50];
    }

//////////////////////////////
//    Using createPacket    //
//////////////////////////////
uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){
    return buf[14];
    }

uint8_t* AigamozuControlPackets::getPacketData(){
    return packetData;
    }

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

//changeSpeed()
//ロボットを操縦する
void AigamozuControlPackets::changeSpeed(uint8_t* buf){
    if(nowMode == MANUAL_MODE){
            manualCount =0;
            _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
        }
}



 void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,int mode){
    uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,(uint8_t)mode,'A','G','E'};
    for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i];
    packetLength = CHANGE_MODE_COMMAND_LENGTH;
}
    
    
///////////////////////////////////////
//   each mode interrupt             //
///////////////////////////////////////

//manualMode()
//マニュアルコマンドを連続で
//10回以上処理した場合、一瞬停止する

void AigamozuControlPackets::manualMode(){
    manualCount++;
    if(manualCount > 10){
        _agzSheild.changeSpeed(0,0,0,0);
        manualCount = 0;
        }
    }

//autoMove()
//ロボットがオートモードの際に
//シーケンス動作させるために使用する
bool AigamozuControlPackets::autoMove(/*const int autobase[]*/){
    
   /*
    _agzSheild.changeSpeed(2,128,2,128):for moving robot
    VNH5019参照
    */
        
    if(nowMode == AUTO_GPS_MODE){

        //if(moveTimer.read_ms() <= 400){
            _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);
        //}else if(moveTimer.read_ms() > 400 && moveTimer.read_ms() <= 500){
         //   _agzSheild.changeSpeed(0,LOW_SPEED,0,LOW_SPEED); //stop
        //}
    }   

    return 1;
}

//////////////////////////////
//     Mode change          //
//////////////////////////////
bool AigamozuControlPackets::changeMode(uint8_t *buf){
    
    //reset
    _agzSheild.changeSpeed(0,0,0,0);
    eachModeInt.detach();

    //Select Mode
    switch(buf[19]){
        case 0: //スタンバイ
            nowMode = STANDBY_MODE;
        break;
        
        case 1: //マニュアル
            eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
            nowMode = MANUAL_MODE;
        break;
        
        case 2: //オートモード
            nowMode = AUTO_GPS_MODE;
            moveTimer.reset();
        break;
        
        case 3: //オートモード
            nowMode = AUTO_GPS_MODE;
            moveTimer.reset();
        break;
        
        default:
            nowMode = STANDBY_MODE;
        break;
        
        }
    return false;
}