forward
Dependencies: VNH5019
Fork of AigamozuControlPackets by
AigamozuControlPackets.cpp
00001 #include "AigamozuControlPackets.h" 00002 #include "VNH5019.h" 00003 00004 ////////////////////////////// 00005 // Init // 00006 ////////////////////////////// 00007 AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){ 00008 packetData = new uint8_t[50]; 00009 packetLength = 0; 00010 //--init base point--// 00011 //basePoint[0].x = 13956.2655; 00012 //basePoint[0].y = 3731.5060; 00013 basePoint[0].x = 100.2655; 00014 basePoint[0].y = 30.5060; 00015 00016 //basePoint[1].x = 13956.2898; 00017 //basePoint[1].y = 3731.5055; 00018 basePoint[1].x = 200.2898; 00019 basePoint[1].y = 30.5055; 00020 00021 //basePoint[2].x = 13956.2915; 00022 //basePoint[2].y = 3731.5245; 00023 basePoint[2].x = 200.2915; 00024 basePoint[2].y = 40.5245; 00025 00026 //basePoint[3].x = 13956.2680; 00027 //basePoint[3].y = 3731.5248; 00028 basePoint[3].x = 100.2680; 00029 basePoint[3].y = 40.5248; 00030 00031 agzPoint.x=0;agzPoint.y=0; 00032 agzPointKalman.x=0;agzPointKalman.y=0; 00033 agzCov.x=0;agzCov.y=0; 00034 } 00035 00036 ////////////////////////////// 00037 // get関数 00038 ////////////////////////////// 00039 double AigamozuControlPackets::get_agzPoint_lati(){ 00040 return agzPoint.x; 00041 } 00042 double AigamozuControlPackets::get_agzPoint_longi(){ 00043 return agzPoint.y; 00044 } 00045 double AigamozuControlPackets::get_agzPointKalman_lati(){ 00046 return agzPointKalman.x; 00047 } 00048 double AigamozuControlPackets::get_agzPointKalman_longi(){ 00049 return agzPointKalman.y; 00050 } 00051 double AigamozuControlPackets::get_agzCov_lati(){ 00052 return agzCov.x; 00053 } 00054 double AigamozuControlPackets::get_agzCov_longi(){ 00055 return agzCov.y; 00056 } 00057 00058 double AigamozuControlPackets::get_basePoint_lati(int i){ 00059 return basePoint[i].x; 00060 } 00061 double AigamozuControlPackets::get_basePoint_longi(int i){ 00062 return basePoint[i].y; 00063 } 00064 double AigamozuControlPackets::get_basePointKalman_lati(int i){ 00065 return basePointKalman[i].x; 00066 } 00067 double AigamozuControlPackets::get_basePointKalman_longi(int i){ 00068 return basePointKalman[i].y; 00069 } 00070 00071 ////////////////////////////// 00072 // set関数 00073 ////////////////////////////// 00074 void AigamozuControlPackets::set_agzCov(double cov_lati,double cov_longi){ 00075 agzCov.x = cov_lati; 00076 agzCov.y = cov_longi; 00077 } 00078 00079 void AigamozuControlPackets::set_agzPointKalman_lati(double kalman_lati){ 00080 00081 agzPointKalman.x = kalman_lati; 00082 00083 } 00084 00085 void AigamozuControlPackets::set_agzPointKalman_longi(double kalman_longi){ 00086 00087 agzPointKalman.y = kalman_longi; 00088 00089 } 00090 00091 ////////////////////////////// 00092 // Controller/Base -> Robot // 00093 ////////////////////////////// 00094 00095 void AigamozuControlPackets::createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR) 00096 { 00097 uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'}; 00098 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; 00099 packetLength = RECEIVE_STATUS_COMMNAD_LENGTH; 00100 } 00101 00102 void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID) 00103 { 00104 uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'}; 00105 for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; 00106 packetLength = REQUEST_COMMNAD_LENGTH; 00107 } 00108 00109 void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode){ 00110 uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,mode,'A','G','E'}; 00111 for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i]; 00112 packetLength = CHANGE_MODE_COMMAND_LENGTH; 00113 } 00114 00115 00116 ////////////////////////////// 00117 // Robot -> Controller/Base // 00118 ////////////////////////////// 00119 00120 void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,int state, double latitude,double longitude,double latitudeKalman,double longitudeKalman,double covarLati,double covarLongi) 00121 { 00122 UNION_double_char latitude_data,longitude_data, latitudeKalman_data, longitudeKalman_data; 00123 UNION_double_char covarLati_data,covarLongi_data; 00124 00125 latitude_data.double_value=latitude; 00126 longitude_data.double_value=longitude; 00127 latitudeKalman_data.double_value=latitudeKalman; 00128 longitudeKalman_data.double_value=longitudeKalman; 00129 covarLati_data.double_value=covarLati; 00130 covarLongi_data.double_value=covarLongi; 00131 00132 00133 uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S', state,71,80,83, 00134 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], 00135 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], 00136 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], 00137 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], 00138 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], 00139 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], 00140 65,71,69}; 00141 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; 00142 packetLength = RECEIVE_STATUS_COMMNAD_LENGTH; 00143 } 00144 00145 00146 00147 ////////////////////////////// 00148 // Using createPacket // 00149 ////////////////////////////// 00150 uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){ 00151 return buf[14]; 00152 } 00153 00154 uint8_t* AigamozuControlPackets::getPacketData(){ 00155 return packetData; 00156 } 00157 00158 int AigamozuControlPackets::getPacketLength(){ 00159 return packetLength; 00160 } 00161 00162 void AigamozuControlPackets::changeSpeed(uint8_t* buf){ 00163 if(nowMode == MANUAL_MODE){ 00164 manualCount =0; 00165 _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]); 00166 } 00167 } 00168 00169 ////////////////////////////// 00170 // each mode interrupt // 00171 ////////////////////////////// 00172 00173 void AigamozuControlPackets::manualMode(){ 00174 manualCount++; 00175 if(manualCount > 10){ 00176 _agzSheild.changeSpeed(0,0,0,0); 00177 manualCount = 0; 00178 } 00179 } 00180 00181 00182 void AigamozuControlPackets::randomAuto(){ 00183 00184 randomCount++; 00185 00186 if(randomCount < 10) _agzSheild.changeSpeed(1,128,1,128); 00187 else if(randomCount < 11) _agzSheild.changeSpeed(2,128,2,128); 00188 else randomCount = 0; 00189 00190 } 00191 00192 void AigamozuControlPackets::test_Auto(int flag){ 00193 00194 if(flag == 0){ 00195 _agzSheild.changeSpeed(1, 32, 1, 32); //straight 00196 } 00197 if(flag == 1){ 00198 _agzSheild.changeSpeed(0, 32, 0, 32); 00199 } 00200 if(flag == 2){ 00201 _agzSheild.changeSpeed(1, 32, 2, 32); //Turn Right 00202 } 00203 if(flag == 3){ 00204 _agzSheild.changeSpeed(0, 32, 0, 32); 00205 } 00206 } 00207 00208 bool AigamozuControlPackets::gpsAuto(){ 00209 00210 /* 00211 _agzSheild.changeSpeed(2,128,2,128):for moving robot 00212 */ 00213 00214 Timer Automove_Timer; 00215 00216 bool out_flag = true; 00217 static bool out_count_flag = false; 00218 00219 if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){ 00220 out_flag = false; 00221 out_count_flag = false; 00222 }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){ 00223 out_flag = false; 00224 out_count_flag = false; 00225 }else{//if robot is out 00226 out_flag = true; 00227 } 00228 00229 //if robot is out: 00230 if(out_flag == true || out_count_flag == false){ 00231 Automove_Timer.reset(); 00232 out_count_flag = true; 00233 } 00234 /* if(nowMode == AUTO_GPS_MODE){ 00235 00236 if(out_flag){ 00237 if(Automove_Timer.read_ms() < 5000){ 00238 _agzSheild.changeSpeed(2,64,2,64);//back 00239 }else if(Automove_Timer.read_ms() < 6000){ 00240 _agzSheild.changeSpeed(1,64,1,64);//straight 00241 }else if(Automove_Timer.read_ms() >= 6000){ 00242 out_count_flag=false; 00243 out_flag=false; 00244 } 00245 }else{ 00246 //if robot is inner 00247 _agzSheild.changeSpeed(1,64,1,64);//straight 00248 } 00249 } 00250 */ 00251 return out_flag; 00252 00253 } 00254 00255 00256 //Update Robot Point 00257 void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){ 00258 agzPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0); 00259 agzPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0); 00260 00261 } 00262 00263 //Updata Base Point 00264 void AigamozuControlPackets::reNewBasePoint(int id, uint8_t *latitude,uint8_t *longitude){ 00265 UNION_double_char lat,longi; 00266 for(int i = 0;i < 8;i++){ 00267 lat.char_value[i]=latitude[i]; 00268 } 00269 for(int i = 0;i < 8;i++){ 00270 longi.char_value[i]=longitude[i]; 00271 } 00272 basePoint[id].x = lat.double_value; 00273 basePoint[id].y = longi.double_value; 00274 } 00275 00276 //Update Robot Point 00277 void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){ 00278 agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0); 00279 agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0); 00280 00281 } 00282 00283 //Updata Base Point 00284 void AigamozuControlPackets::reNewBasePointKalman(int id, uint8_t *latitude,uint8_t *longitude){ 00285 UNION_double_char lat,longi; 00286 for(int i = 0;i < 8;i++){ 00287 lat.char_value[i]=latitude[i]; 00288 } 00289 for(int i = 0;i < 8;i++){ 00290 longi.char_value[i]=longitude[i]; 00291 } 00292 00293 basePointKalman[id].x = lat.double_value; 00294 basePointKalman[id].y = longi.double_value; 00295 } 00296 00297 00298 //Check Hit Point Area 00299 bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){ 00300 vector2D AB = AigamozuControlPackets::sub_vector(B, A); 00301 vector2D BP = AigamozuControlPackets::sub_vector(P, B); 00302 00303 vector2D BC = AigamozuControlPackets::sub_vector(C, B); 00304 vector2D CP = AigamozuControlPackets::sub_vector(P, C); 00305 00306 vector2D CA = AigamozuControlPackets::sub_vector(A, C); 00307 vector2D AP = AigamozuControlPackets::sub_vector(P, A); 00308 00309 double c1 = AB.x * BP.y - AB.y * BP.x; 00310 double c2 = BC.x * CP.y - BC.y * CP.x; 00311 double c3 = CA.x * AP.y - CA.y * AP.x; 00312 00313 if( ( c1 > 0 && c2 > 0 && c3 > 0 ) || ( c1 < 0 && c2 < 0 && c3 < 0 ) ) { 00314 return true; 00315 } 00316 00317 return false; 00318 00319 } 00320 00321 vector2D AigamozuControlPackets::sub_vector( const vector2D& a, const vector2D& b ) 00322 { 00323 vector2D ret; 00324 ret.x = a.x - b.x; 00325 ret.y = a.y - b.y; 00326 return ret; 00327 } 00328 00329 00330 00331 00332 ////////////////////////////// 00333 // Mode change // 00334 ////////////////////////////// 00335 bool AigamozuControlPackets::changeMode(uint8_t *buf){ 00336 00337 //reset 00338 _agzSheild.changeSpeed(0,0,0,0); 00339 eachModeInt.detach(); 00340 00341 //Select Mode 00342 switch(buf[19]){ 00343 case 0: 00344 nowMode = STANDBY_MODE; 00345 break; 00346 00347 case 1: 00348 eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0); 00349 nowMode = MANUAL_MODE; 00350 break; 00351 00352 case 2: 00353 //nowMode = AUTO_MODE; 00354 //eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0); 00355 //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0); 00356 nowMode = AUTO_GPS_MODE; 00357 Move_Timer.reset(); 00358 break; 00359 00360 case 3: 00361 //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2); 00362 nowMode = AUTO_GPS_MODE; 00363 Move_Timer.reset(); 00364 break; 00365 00366 default: 00367 nowMode = STANDBY_MODE; 00368 break; 00369 00370 } 00371 return false; 00372 } 00373 00374 00375
Generated on Sat Jul 23 2022 04:12:00 by 1.7.2