File content as of revision 48:4695e72853c6:

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

#define RIGHT_SPEED 96
#define LEFT_SPEED 96
#define FORWARD_SPEED 96

//////////////////////////////
//          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;
    }

void AigamozuControlPackets::changeSpeed(uint8_t* buf){
    if(nowMode == MANUAL_MODE){
            manualCount =0;
            _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
        }
    }


//////////////////////////////
// Robot -> Xbee     
// ジャイロ・加速度データを送信する     //
//////////////////////////////

 void AigamozuControlPackets::SendDataCommand(uint8_t fromID, uint8_t toID, int16_t gx,int16_t gy, int16_t gz, int16_t ax, int16_t ay,int16_t az, uint8_t flag)
 {         
 
    UNION_int_char gx_data, gy_data, gz_data;
    UNION_int_char ax_data,ay_data, az_data;
    
    gx_data.int_value=gx;
    gy_data.int_value=gy;
    gz_data.int_value=gz;
    ax_data.int_value=ax;
    ay_data.int_value=ay;
    az_data.int_value=az;

    uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID, 
    gx_data.char_value[0],gx_data.char_value[1],
    gy_data.char_value[0],gy_data.char_value[1],
    gz_data.char_value[0],gz_data.char_value[1],
    ax_data.char_value[0],ax_data.char_value[1],
    ay_data.char_value[0],ay_data.char_value[1],
    az_data.char_value[0],az_data.char_value[1],
    flag,'A','G','E'};
    
    for(int i = 0; i < SEND_DATA_COMMNAD_LENGTH; i++) packetData[i] = tmp[i];
    packetLength = SEND_DATA_COMMNAD_LENGTH;
    
    }
     

//////////////////////////////
// Robot -> Xbee     
// 受信したジャイロ・加速度データを変換する    //
//////////////////////////////
//Updata Base Point   
void AigamozuControlPackets::GetData(uint8_t *gx,uint8_t *gy, uint8_t *gz,uint8_t *ax, uint8_t *ay,uint8_t *az){
    UNION_int_char t_gx, t_gy, t_gz, t_ax, t_ay, t_az;
    for(int i = 0;i < 2;i++){
        t_gx.char_value[i]=gx[i];
        t_gy.char_value[i]=gy[i];
        t_gz.char_value[i]=gz[i];
    }
    for(int i = 0;i < 8;i++){
        t_ax.char_value[i]=ax[i];
        t_ay.char_value[i]=ay[i];
        t_az.char_value[i]=az[i];
    }
    
    r_gx = t_gx.int_value;
    r_gy = t_gy.int_value;
    r_gz = t_gz.int_value;
    r_ax = t_ax.int_value;
    r_ay = t_ay.int_value;
    r_az = t_az.int_value;
}  

//////////////////////////////
//   each mode interrupt    //
//////////////////////////////

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

bool AigamozuControlPackets::gpsAuto(/*const int autobase[]*/){
    
   /*
    _agzSheild.changeSpeed(2,128,2,128):for moving robot
    */
    
   if(nowMode == AUTO_GPS_MODE){
       //if(Move_Timer.read_ms() < 5000){
            _agzSheild.changeSpeed(1,LEFT_SPEED,1,RIGHT_SPEED);//turn
      //  }else if(Move_Timer.read_ms() >= 5000 && Move_Timer.read_ms() < 6000){
      //      _agzSheild.changeSpeed(0,0,0,0);//stop
        //    Move_Timer.reset();
        //}
/*
        if(Move_Timer.read_ms() < 4000){
            _agzSheild.changeSpeed(1,LEFT_SPEED,1,RIGHT_SPEED);//turn
        }else if(Move_Timer.read_ms() >= 4000 && Move_Timer.read_ms() < 5000){
            _agzSheild.changeSpeed(0,0,0,0);//stop
        }else if(Move_Timer.read_ms() >= 5000 && Move_Timer.read_ms() < 6000){
            _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);//forward
        }else if(Move_Timer.read_ms() >= 6000 && Move_Timer.read_ms() < 7000){
            _agzSheild.changeSpeed(0,0,0,0);//stop
        }else if(Move_Timer.read_ms() >= 7000 && Move_Timer.read_ms() < 12000){
            _agzSheild.changeSpeed(1,RIGHT_SPEED,1,LEFT_SPEED);//turn
        }else if(Move_Timer.read_ms() >=12000 && Move_Timer.read_ms() < 13000){
            _agzSheild.changeSpeed(0,0,0,0);//stop
        }else if(Move_Timer.read_ms() >= 13000 && Move_Timer.read_ms() < 14000){
            _agzSheild.changeSpeed(1,FORWARD_SPEED,1,FORWARD_SPEED);//forward
        }else if(Move_Timer.read_ms() >= 14000 && Move_Timer.read_ms() < 15000){
            _agzSheild.changeSpeed(0,0,0,0);//stop
             Move_Timer.reset();
        }
        */
    }   

 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;
            Move_Timer.reset();
        break;
        
        case 3:
            nowMode = AUTO_GPS_MODE;
            Move_Timer.reset();
        break;
        
        default:
            nowMode = STANDBY_MODE;
        break;
        
        }
    return false;
}