20150512 Nakazawa
Diff: AigamozuControlPackets.cpp
- Revision:
- 0:806e9a434e81
- Child:
- 1:6d7c854c927d
diff -r 000000000000 -r 806e9a434e81 AigamozuControlPackets.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AigamozuControlPackets.cpp Tue May 12 10:40:07 2015 +0000 @@ -0,0 +1,334 @@ +#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_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_agzAutoGPS(){ + agzPoint.x += 0.001; + //agzPoint.y += 0.001; +} + +void AigamozuControlPackets::set_agzKalmanGPS(){ + agzPointKalman.x += 0.001; + //agzPointKalman.y += 0.001; + +} + +////////////////////////////// +// 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,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; + longitude_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',nowMode,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; + + } + +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(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 + } + + return out_flag; + +} + + + + + +//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); + basePoint[id].y = (double)longitudeH +(double)(longitudeL / 1000000.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); + basePointKalman[id].y = (double)longitudeH +(double)(longitudeL / 1000000.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; +} + + +