2016_05_19ver Auto mode 10sec forward, 2sec stop, 2sec right turn Please change test_mode's right turn ppm
Dependencies: VNH5019 AigamozuControlPackets_2016
Dependents: Aigamozu_Robot_2016_ver1 GPSLOG_program AigamozuControlPackets_2016
Fork of AigamozuControlPackets by
Diff: AigamozuControlPackets.cpp
- Revision:
- 17:a6fa8cc96d94
- Parent:
- 14:e441331aa4a1
- Child:
- 18:01882120e6cf
diff -r e441331aa4a1 -r a6fa8cc96d94 AigamozuControlPackets.cpp --- a/AigamozuControlPackets.cpp Sat Apr 25 01:05:51 2015 +0000 +++ b/AigamozuControlPackets.cpp Tue May 12 01:02:47 2015 +0000 @@ -27,10 +27,42 @@ //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; +} ////////////////////////////// +// set関数 +////////////////////////////// +void AigamozuControlPackets::set_agzCov(double cov_lati,double cov_longi){ + agzCov.x = cov_lati; + agzCov.y = cov_longi; +} +////////////////////////////// // Controller/Base -> Robot // ////////////////////////////// @@ -59,35 +91,30 @@ // Robot -> Controller/Base // ////////////////////////////// -void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,long latitudeH,long latitudeL,long longitudeH,long longitudeL) +void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,double latitude,double longitude,double latitudeKalman,double longitudeKalman,double covarLati,double covarLongi) { - //useing union's split::: long hoge.a(4byte) == uint8_t hoge.b[4] - TEST_T latH,latL,lonH,lonL; + UNION_double_char latitude_data,longitude_data, latitudeKalman_data, longitudeKalman_data; + UNION_double_char covarLati_data,covarLongi_data; - 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}; + 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; } -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; -} ////////////////////////////// @@ -156,7 +183,7 @@ */ printf("normal\n"); printf("-1, %f, %f\n",agzPoint.x,agzPoint.y); - for(i = 0; i < 4; i++){ + for(i = 0; i < 5; i++){ printf(" %d, %f, %f\n", i, basePoint[i].x,basePoint[i].y); } @@ -174,7 +201,7 @@ } printf("Kalman\n"); printf("-1, %f, %f\n",agzPointKalman.x,agzPointKalman.y); - for(i = 0; i < 4; i++){ + for(i = 0; i < 5; i++){ printf(" %d, %f, %f\n", i, basePointKalman[i].x,basePointKalman[i].y); } if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){ @@ -196,11 +223,11 @@ out_count_flag = true; } if(out_flag){ - if(Automove_Timer.read_ms() < 1000){ - _agzSheild.changeSpeed(1,64,2,64);//turn - }else if(Automove_Timer.read_ms() < 5000){ - _agzSheild.changeSpeed(1,64,1,64);//straight - }else if(Automove_Timer.read_ms() >= 5000){ + 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; } @@ -213,13 +240,12 @@ - //Update Robot Point void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){ - agzPoint.x = (double)latitudeH + (double)(latitudeL / 1000000.0); - agzPoint.y = (double)longitudeH +(double)(longitudeL / 1000000.0); + agzPoint.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0); + agzPoint.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0); } @@ -231,8 +257,8 @@ //Update Robot Point void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){ - agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 1000000.0); - agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 1000000.0); + agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0); + agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0); }