File content as of revision 50:3511be172d81:

#include "AigamozuControlPackets.h"

/////////////////////////////////////////
//
// Constructor
//
/////////////////////////////////////////
AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild)
{
    packetData = new uint8_t[50];
    packetLength = 0;

    agzPoint.x = 0.0;
    agzPoint.y = 0.0;

    for(int i=0; i<BASENUMBER; i++) {
        basePoint[i].x = 0.0;
        basePoint[i].y = 0.0;
    }
}

AigamozuControlPackets::vector2D AigamozuControlPackets::sub_vector(vector2D A, vector2D B)
{
    vector2D ret;
    ret.x = B.x - A.x;
    ret.y = B.y - A.y;
    return ret;
}

bool AigamozuControlPackets::checkInOut(vector2D A, vector2D B, vector2D C, vector2D P)
{
    vector2D BA = sub_vector(B, A);
    vector2D BP = sub_vector(B, P);
    vector2D BC = sub_vector(B, C);

    double c1 = BA.x * BP.y - BA.y * BP.x;
    double c2 = BP.x * BC.y - BP.y * BC.x;

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

void AigamozuControlPackets::autoMoving(void)
{

    if(checkInOut(basePoint[0], basePoint[1], basePoint[2], agzPoint)
            && checkInOut(basePoint[2], basePoint[3], basePoint[0], agzPoint)) {
        out_flag = false;
    } else {
        out_flag = true;
    }

}

/////////////////////////////////////////
//
// Create Packet: Robot -> Base,
//                Manager -> Robot/Base
//
/////////////////////////////////////////
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;
}

/////////////////////////////////////////
//
// Create Packet: Base -> Robot/Manager,
//                Robot -> Manager
//
/////////////////////////////////////////
void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID, uint8_t toID, int state)
{
    UNION_double_char latitude_data,longitude_data;

    latitude_data.double_value=agzPoint.y;
    longitude_data.double_value=agzPoint.x;


    uint8_t tmp[] = {'A', 'G', 'S', 'R', 'F', fromID, 'T', toID, 'S', state,'G', 'P', 'S',
                     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],
                     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                     'A', 'G', 'E'
                    };
    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
    packetLength =  RECEIVE_STATUS_COMMNAD_LENGTH;
}

/////////////////////////////////////////
//
// Update data
//
/////////////////////////////////////////
void AigamozuControlPackets::reNewRobotPoint(long latitudeH, long latitudeL, long longitudeH, long longitudeL)
{
    agzPoint.x = (double)longitudeH + (double)(longitudeL / 10000.0/60.0);
    agzPoint.y = (double)latitudeH +(double)(latitudeL / 10000.0/60.0);
}

void AigamozuControlPackets::reNewBasePoint(int id, uint8_t *latitude, uint8_t *longitude)
{
    UNION_double_char latitude_data,longitude_data;
    for(int i = 0; i < 8; i++) {
        latitude_data.char_value[i]=latitude[i];
    }
    for(int i = 0; i < 8; i++) {
        longitude_data.char_value[i]=longitude[i];
    }
    basePoint[id].x = longitude_data.double_value;
    basePoint[id].y = latitude_data.double_value;
}

/////////////////////////////////////////
//
// Manual Mode
//
/////////////////////////////////////////
void AigamozuControlPackets::manualMode() {}

/////////////////////////////////////////
//
// Change Mode
//
/////////////////////////////////////////
void AigamozuControlPackets::changeMode(uint8_t *buf)
{

    //reset
    _agzSheild.changeSpeed(0,0,0,0);
    eachModeInt.detach();

    //Select Mode
    switch(buf[8]) {
        case 0:
            nowMode = STANDBY_MODE;
            break;

        case 1:
            eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
            nowMode = MANUAL_MODE;
            break;

        case 2:
            nowMode = AUTO_MODE;
            //Move_Timer.reset();
            break;

        default:
            nowMode = STANDBY_MODE;
            break;

    }
}
/////////////////////////////////////////
//
// Set
//
/////////////////////////////////////////
void AigamozuControlPackets::set_nowStatus(STATUS s)
{
    nowStatus = s;
}



/////////////////////////////////////////
//
// Get
//
/////////////////////////////////////////
MODE AigamozuControlPackets::get_nowMode(void)
{
    return nowMode;
}
uint8_t* AigamozuControlPackets::get_packetData(void)
{
    return packetData;
}
int AigamozuControlPackets::get_packetLength(void)
{
    return packetLength;
}
bool AigamozuControlPackets::get_out_flag(void)
{
    return out_flag;
}
double AigamozuControlPackets::get_agzPoint_latitude(void)
{
    return agzPoint.y;
}
double AigamozuControlPackets::get_agzPoint_longitude(void)
{
    return agzPoint.x;
}
double AigamozuControlPackets::get_basePoint_latitude(int i)
{
    return basePoint[i].y;
}
double AigamozuControlPackets::get_basePoint_longitude(int i)
{
    return basePoint[i].x;
}