File content as of revision 35:b33b8b2a92b0:

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

//////////////////////////////
//          Init            //
//////////////////////////////
AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){
    packetData = new uint8_t[50];
    packetLength = 0;
    //--init base point--//
    //basePoint[0].x = 13956.2655;
    //basePoint[0].y = 3731.5060;
    basePoint[0].x = 100.2655;
    basePoint[0].y = 30.5060;
    
    //basePoint[1].x = 13956.2898;
    //basePoint[1].y = 3731.5055;
    basePoint[1].x = 200.2898;
    basePoint[1].y = 30.5055;
    
    //basePoint[2].x = 13956.2915;
    //basePoint[2].y = 3731.5245;
    basePoint[2].x = 200.2915;
    basePoint[2].y = 40.5245;
    
    //basePoint[3].x = 13956.2680;
    //basePoint[3].y = 3731.5248;
    basePoint[3].x = 100.2680;
    basePoint[3].y = 40.5248;
    
    agzPoint.x=0;agzPoint.y=0;
    agzPointKalman.x=0;agzPointKalman.y=0;
    agzCov.x=0;agzCov.y=0;
    }

//////////////////////////////
// get関数
//////////////////////////////
double AigamozuControlPackets::get_agzPoint_lati(){
    return agzPoint.x;
    }
double AigamozuControlPackets::get_agzPoint_longi(){
    return agzPoint.y;
    }
double AigamozuControlPackets::get_agzPointKalman_lati(){
    return agzPointKalman.x;
    }
double AigamozuControlPackets::get_agzPointKalman_longi(){
    return agzPointKalman.y;
    }
double AigamozuControlPackets::get_agzCov_lati(){
    return agzCov.x;
}
double AigamozuControlPackets::get_agzCov_longi(){
    return agzCov.y;
}
   
double AigamozuControlPackets::get_basePoint_lati(int i){
    return basePoint[i].x;
    }
double AigamozuControlPackets::get_basePoint_longi(int i){
    return basePoint[i].y;
    }
double AigamozuControlPackets::get_basePointKalman_lati(int i){
    return basePointKalman[i].x;
    }
double AigamozuControlPackets::get_basePointKalman_longi(int i){
    return basePointKalman[i].y;
    }
    
//////////////////////////////
// set関数
//////////////////////////////
void AigamozuControlPackets::set_agzCov(double cov_lati,double cov_longi){
    agzCov.x = cov_lati;
    agzCov.y = cov_longi;    
}

void AigamozuControlPackets::set_agzPointKalman_lati(double kalman_lati){

    agzPointKalman.x = kalman_lati;   
     
}
        
void AigamozuControlPackets::set_agzPointKalman_longi(double kalman_longi){
    
    agzPointKalman.y = kalman_longi;
    
}

//////////////////////////////
// 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 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,int state, double latitude,double longitude,double latitudeKalman,double longitudeKalman,double covarLati,double covarLongi)
{
    UNION_double_char latitude_data,longitude_data, latitudeKalman_data, longitudeKalman_data;
    UNION_double_char covarLati_data,covarLongi_data;
    
    latitude_data.double_value=latitude;
    longitude_data.double_value=longitude;
    latitudeKalman_data.double_value=latitudeKalman;
    longitudeKalman_data.double_value=longitudeKalman;
    covarLati_data.double_value=covarLati;
    covarLongi_data.double_value=covarLongi;
    
    
    uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S', state,71,80,83,
                    latitude_data.char_value[0],latitude_data.char_value[1],latitude_data.char_value[2],latitude_data.char_value[3],latitude_data.char_value[4],latitude_data.char_value[5],latitude_data.char_value[6],latitude_data.char_value[7],
                    longitude_data.char_value[0],longitude_data.char_value[1],longitude_data.char_value[2],longitude_data.char_value[3],longitude_data.char_value[4],longitude_data.char_value[5],longitude_data.char_value[6],longitude_data.char_value[7],
                    latitudeKalman_data.char_value[0],latitudeKalman_data.char_value[1],latitudeKalman_data.char_value[2],latitudeKalman_data.char_value[3],latitudeKalman_data.char_value[4],latitudeKalman_data.char_value[5],latitudeKalman_data.char_value[6],latitudeKalman_data.char_value[7],
                    longitudeKalman_data.char_value[0],longitudeKalman_data.char_value[1],longitudeKalman_data.char_value[2],longitudeKalman_data.char_value[3],longitudeKalman_data.char_value[4],longitudeKalman_data.char_value[5],longitudeKalman_data.char_value[6],longitudeKalman_data.char_value[7],      
                    covarLati_data.char_value[0],covarLati_data.char_value[1],covarLati_data.char_value[2],covarLati_data.char_value[3],covarLati_data.char_value[4],covarLati_data.char_value[5],covarLati_data.char_value[6],covarLati_data.char_value[7],
                    covarLongi_data.char_value[0],covarLongi_data.char_value[1],covarLongi_data.char_value[2],covarLongi_data.char_value[3],covarLongi_data.char_value[4],covarLongi_data.char_value[5],covarLongi_data.char_value[6],covarLongi_data.char_value[7],
                    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]);
        }
    }

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

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


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::test_Auto(int flag){

        if(flag == 0){
            _agzSheild.changeSpeed(1, 64, 1, 64); //straight
        }
        if(flag == 1){
            _agzSheild.changeSpeed(0, 64, 0, 64); 
        }
        if(flag == 2){
             _agzSheild.changeSpeed(1, 64, 2, 64); //Turn Right
        }   
        if(flag == 3){
            _agzSheild.changeSpeed(0, 64, 0, 64); 
        }
}

bool AigamozuControlPackets::gpsAuto(){
    
   /*
    _agzSheild.changeSpeed(2,128,2,128):for moving robot
    */
    
    Timer Automove_Timer;
    
    bool out_flag = true;
    static bool out_count_flag = false;
    
    if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){
        out_flag = false;
        out_count_flag = false;
    }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){
        out_flag = false;
        out_count_flag = false;
    }else{//if robot is out 
        out_flag = true;
    }
 
    //if robot is out:
    if(out_flag == true || out_count_flag == false){
        Automove_Timer.reset();
        out_count_flag = true;
    }
/*    if(nowMode == AUTO_GPS_MODE){
         
   if(out_flag){
        if(Automove_Timer.read_ms() < 5000){
            _agzSheild.changeSpeed(2,64,2,64);//back
        }else if(Automove_Timer.read_ms() < 6000){
            _agzSheild.changeSpeed(1,64,1,64);//straight
        }else if(Automove_Timer.read_ms() >= 6000){
            out_count_flag=false;
            out_flag=false;
        }
    }else{
    //if robot is inner
        _agzSheild.changeSpeed(1,64,1,64);//straight
    }
    }   
*/ 
    return out_flag;
 
}
    
    
//Update Robot Point    
void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
    agzPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
    agzPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
    
    }
    
//Updata Base Point   
void AigamozuControlPackets::reNewBasePoint(int id, uint8_t *latitude,uint8_t *longitude){
    UNION_double_char lat,longi;
    for(int i = 0;i < 8;i++){
        lat.char_value[i]=latitude[i];
    }
    for(int i = 0;i < 8;i++){
        longi.char_value[i]=longitude[i];
    }
    basePoint[id].x = lat.double_value;
    basePoint[id].y = longi.double_value;
}   

//Update Robot Point    
void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
    agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
    agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
    
    }
    
//Updata Base Point   
void AigamozuControlPackets::reNewBasePointKalman(int id, uint8_t *latitude,uint8_t *longitude){
    UNION_double_char lat,longi;
    for(int i = 0;i < 8;i++){
        lat.char_value[i]=latitude[i];
    }
    for(int i = 0;i < 8;i++){
        longi.char_value[i]=longitude[i];
    }
    
    basePointKalman[id].x = lat.double_value;
    basePointKalman[id].y = longi.double_value;
} 


//Check Hit Point Area
bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){
    vector2D AB = AigamozuControlPackets::sub_vector(B, A);
    vector2D BP = AigamozuControlPackets::sub_vector(P, B);

    vector2D BC = AigamozuControlPackets::sub_vector(C, B);
    vector2D CP = AigamozuControlPackets::sub_vector(P, C);

    vector2D CA = AigamozuControlPackets::sub_vector(A, C);
    vector2D AP = AigamozuControlPackets::sub_vector(P, A);
    
    double c1 = AB.x * BP.y - AB.y * BP.x;
    double c2 = BC.x * CP.y - BC.y * CP.x;
    double c3 = CA.x * AP.y - CA.y * AP.x;

    if( ( c1 > 0 && c2 > 0 && c3 > 0 ) || ( c1 < 0 && c2 < 0 && c3 < 0 ) ) {    
        return true;
    }
    
    return false;

    }

vector2D AigamozuControlPackets::sub_vector( const vector2D& a, const vector2D& b )
{
    vector2D ret;
    ret.x = a.x - b.x;
    ret.y = a.y - b.y;
    return ret;   
}    




//////////////////////////////
//     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_MODE;
            //eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0);
            //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0);
            nowMode = AUTO_GPS_MODE;
            Move_Timer.reset();
        break;
        
        case 3:
            //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2);
            nowMode = AUTO_GPS_MODE;
            Move_Timer.reset();
        break;
        
        default:
            nowMode = STANDBY_MODE;
        break;
        
        }
    return false;
}