File content as of revision 16:0e815cca2cc7:

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


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

void AigamozuControlPackets::createReceiveStatusCommandwithKalman(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','K','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]);
        }
    }

//////////////////////////////
//   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::gpsAuto(){
    
   /*
    _agzSheild.changeSpeed(2,128,2,128):for moving robot
    */
    
    Timer Automove_Timer;
    
    bool out_flag = true;
    static bool out_count_flag = false;
    int i;
    /*
    printf("Check Start\n");
    printf("agzPoint=%f,%f\n",agzPoint.x,agzPoint.y);
    printf("bsdrPoint1=%f,%f\n",basePoint[0].x,basePoint[0].y);
    printf("bsdrPoint2=%f,%f\n",basePoint[1].x,basePoint[1].y);
    printf("bsdrPoint3=%f,%f\n",basePoint[2].x,basePoint[2].y);
    printf("bsdrPoint4=%f,%f\n",basePoint[3].x,basePoint[3].y);
    */
printf("normal\n");
    printf("-1, %f, %f\n",agzPoint.x,agzPoint.y);
    for(i = 0; i < 4; i++){
        printf(" %d, %f, %f\n", i, basePoint[i].x,basePoint[i].y);
    }
    
    if(AigamozuControlPackets::checkGpsHit(basePoint[0],basePoint[1],basePoint[2],agzPoint)){
        printf("InArea\n");
        out_flag = false;
        out_count_flag = false;
    }else if(AigamozuControlPackets::checkGpsHit(basePoint[2],basePoint[3],basePoint[0],agzPoint)){
        printf("InArea\n");
        out_flag = false;
        out_count_flag = false;
    }else{//if robot is out 
        printf("OutArea\n");
        out_flag = true;
    }
    printf("Kalman\n");
    printf("-1, %f, %f\n",agzPointKalman.x,agzPointKalman.y);
    for(i = 0; i < 4; i++){
        printf(" %d, %f, %f\n", i, basePointKalman[i].x,basePointKalman[i].y);
    }
    if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){
        printf("InArea\n");
        //out_flag = false;
        //out_count_flag = false;
    }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){
        printf("InArea\n");
        //out_flag = false;
        //out_count_flag = false;
    }else{//if robot is out 
        printf("OutArea\n");
        //out_flag = true;
    }
 
    //if robot is out:
    if(out_flag == true || out_count_flag == false){
        Automove_Timer.reset();
        out_count_flag = true;
    }
    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,2,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
    }
 
}
 
 
 
 
    
    
//Update Robot Point    
void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
    agzPoint.x = (double)latitudeH + (double)(latitudeL / 1000000.0 / 60.0);
    agzPoint.y = (double)longitudeH +(double)(longitudeL / 1000000.0 / 60.0);
    
    }
    
//Updata Base Point   
void AigamozuControlPackets::reNewBasePoint(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
    basePoint[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0 / 60.0);
    basePoint[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0 / 60.0);
}   

//Update Robot Point    
void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
    agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 1000000.0 / 60.0);
    agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 1000000.0 / 60.0);
    
    }
    
//Updata Base Point   
void AigamozuControlPackets::reNewBasePointKalman(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
    basePointKalman[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0 / 60.0);
    basePointKalman[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0 / 60.0);
}   


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