20150512 Nakazawa
Embed:
(wiki syntax)
Show/hide line numbers
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 = 37.524505; 00014 basePoint[0].y = 139.939510; 00015 00016 //basePoint[1].x = 13956.2898; 00017 //basePoint[1].y = 3731.5055; 00018 basePoint[1].x = 37.524747; 00019 basePoint[1].y = 139.939510; 00020 00021 //basePoint[2].x = 13956.2915; 00022 //basePoint[2].y = 3731.5245; 00023 basePoint[2].x = 37.524747; 00024 basePoint[2].y = 139.939530; 00025 00026 //basePoint[3].x = 13956.2680; 00027 //basePoint[3].y = 3731.5248; 00028 basePoint[3].x = 37.524505; 00029 basePoint[3].y = 139.939530; 00030 00031 agzPoint.x=37.524600;agzPoint.y=139.939520; 00032 agzPointKalman.x=37.524600;agzPointKalman.y=139.939520; 00033 agzCov.x=4.385965;agzCov.y=4.385965; 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_basePointKalman_lati(int i){ 00059 return basePointKalman[i].x; 00060 } 00061 double AigamozuControlPackets::get_basePointKalman_longi(int i){ 00062 return basePointKalman[i].y; 00063 } 00064 ////////////////////////////// 00065 // set関数 00066 ////////////////////////////// 00067 void AigamozuControlPackets::set_agzCov(double cov_lati,double cov_longi){ 00068 agzCov.x = cov_lati; 00069 agzCov.y = cov_longi; 00070 } 00071 00072 00073 void AigamozuControlPackets::set_agzAutoGPS(){ 00074 agzPoint.x += 0.001; 00075 //agzPoint.y += 0.001; 00076 } 00077 00078 void AigamozuControlPackets::set_agzKalmanGPS(){ 00079 agzPointKalman.x += 0.001; 00080 //agzPointKalman.y += 0.001; 00081 00082 } 00083 00084 ////////////////////////////// 00085 // Controller/Base -> Robot // 00086 ////////////////////////////// 00087 00088 void AigamozuControlPackets::createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR) 00089 { 00090 uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'}; 00091 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; 00092 packetLength = RECEIVE_STATUS_COMMNAD_LENGTH; 00093 } 00094 00095 void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID) 00096 { 00097 uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'}; 00098 for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; 00099 packetLength = REQUEST_COMMNAD_LENGTH; 00100 } 00101 00102 void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode){ 00103 uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,mode,'A','G','E'}; 00104 for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i]; 00105 packetLength = CHANGE_MODE_COMMAND_LENGTH; 00106 } 00107 00108 00109 ////////////////////////////// 00110 // Robot -> Controller/Base // 00111 ////////////////////////////// 00112 00113 void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,double latitude,double longitude,double latitudeKalman,double longitudeKalman,double covarLati,double covarLongi) 00114 { 00115 UNION_double_char latitude_data,longitude_data, latitudeKalman_data, longitudeKalman_data; 00116 UNION_double_char covarLati_data,covarLongi_data; 00117 00118 latitude_data.double_value=latitude; 00119 longitude_data.double_value=longitude; 00120 longitude_data.double_value=latitudeKalman; 00121 longitudeKalman_data.double_value=longitudeKalman; 00122 covarLati_data.double_value=covarLati; 00123 covarLongi_data.double_value=covarLongi; 00124 00125 uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S',nowMode,71,80,83, 00126 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], 00127 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], 00128 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], 00129 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], 00130 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], 00131 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], 00132 65,71,69}; 00133 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; 00134 packetLength = RECEIVE_STATUS_COMMNAD_LENGTH; 00135 } 00136 00137 00138 00139 ////////////////////////////// 00140 // Using createPacket // 00141 ////////////////////////////// 00142 uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){ 00143 return buf[14]; 00144 } 00145 00146 uint8_t* AigamozuControlPackets::getPacketData(){ 00147 return packetData; 00148 } 00149 00150 int AigamozuControlPackets::getPacketLength(){ 00151 return packetLength; 00152 } 00153 00154 void AigamozuControlPackets::changeSpeed(uint8_t* buf){ 00155 if(nowMode == MANUAL_MODE){ 00156 manualCount =0; 00157 _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]); 00158 } 00159 } 00160 00161 ////////////////////////////// 00162 // each mode interrupt // 00163 ////////////////////////////// 00164 00165 void AigamozuControlPackets::manualMode(){ 00166 manualCount++; 00167 if(manualCount > 10){ 00168 _agzSheild.changeSpeed(0,0,0,0); 00169 manualCount = 0; 00170 } 00171 } 00172 00173 00174 void AigamozuControlPackets::randomAuto(){ 00175 00176 randomCount++; 00177 00178 if(randomCount < 10) _agzSheild.changeSpeed(1,128,1,128); 00179 else if(randomCount < 11) _agzSheild.changeSpeed(2,128,2,128); 00180 else randomCount = 0; 00181 00182 } 00183 00184 bool AigamozuControlPackets::gpsAuto(){ 00185 00186 /* 00187 _agzSheild.changeSpeed(2,128,2,128):for moving robot 00188 */ 00189 00190 Timer Automove_Timer; 00191 00192 bool out_flag = true; 00193 static bool out_count_flag = false; 00194 00195 if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){ 00196 out_flag = false; 00197 out_count_flag = false; 00198 }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){ 00199 out_flag = false; 00200 out_count_flag = false; 00201 }else{//if robot is out 00202 out_flag = true; 00203 } 00204 00205 //if robot is out: 00206 if(out_flag == true || out_count_flag == false){ 00207 Automove_Timer.reset(); 00208 out_count_flag = true; 00209 } 00210 if(out_flag){ 00211 if(Automove_Timer.read_ms() < 5000){ 00212 _agzSheild.changeSpeed(2,64,2,64);//back 00213 }else if(Automove_Timer.read_ms() < 6000){ 00214 _agzSheild.changeSpeed(1,64,2,64);//straight 00215 }else if(Automove_Timer.read_ms() >= 6000){ 00216 out_count_flag=false; 00217 out_flag=false; 00218 } 00219 }else{ 00220 //if robot is inner 00221 _agzSheild.changeSpeed(1,64,1,64);//straight 00222 } 00223 00224 return out_flag; 00225 00226 } 00227 00228 00229 00230 00231 00232 //Update Robot Point 00233 void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){ 00234 agzPoint.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0); 00235 agzPoint.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0); 00236 00237 } 00238 00239 //Updata Base Point 00240 void AigamozuControlPackets::reNewBasePoint(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){ 00241 basePoint[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0); 00242 basePoint[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0); 00243 } 00244 00245 //Update Robot Point 00246 void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){ 00247 agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0); 00248 agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0); 00249 00250 } 00251 00252 //Updata Base Point 00253 void AigamozuControlPackets::reNewBasePointKalman(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){ 00254 basePointKalman[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0); 00255 basePointKalman[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0); 00256 } 00257 00258 00259 //Check Hit Point Area 00260 bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){ 00261 vector2D AB = AigamozuControlPackets::sub_vector(B, A); 00262 vector2D BP = AigamozuControlPackets::sub_vector(P, B); 00263 00264 vector2D BC = AigamozuControlPackets::sub_vector(C, B); 00265 vector2D CP = AigamozuControlPackets::sub_vector(P, C); 00266 00267 vector2D CA = AigamozuControlPackets::sub_vector(A, C); 00268 vector2D AP = AigamozuControlPackets::sub_vector(P, A); 00269 00270 double c1 = AB.x * BP.y - AB.y * BP.x; 00271 double c2 = BC.x * CP.y - BC.y * CP.x; 00272 double c3 = CA.x * AP.y - CA.y * AP.x; 00273 00274 if( ( c1 > 0 && c2 > 0 && c3 > 0 ) || ( c1 < 0 && c2 < 0 && c3 < 0 ) ) { 00275 return true; 00276 } 00277 00278 return false; 00279 00280 } 00281 00282 vector2D AigamozuControlPackets::sub_vector( const vector2D& a, const vector2D& b ) 00283 { 00284 vector2D ret; 00285 ret.x = a.x - b.x; 00286 ret.y = a.y - b.y; 00287 return ret; 00288 } 00289 00290 00291 00292 00293 ////////////////////////////// 00294 // Mode change // 00295 ////////////////////////////// 00296 bool AigamozuControlPackets::changeMode(uint8_t *buf){ 00297 00298 //reset 00299 _agzSheild.changeSpeed(0,0,0,0); 00300 eachModeInt.detach(); 00301 00302 //Select Mode 00303 switch(buf[19]){ 00304 case 0: 00305 nowMode = STANDBY_MODE; 00306 break; 00307 00308 case 1: 00309 eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0); 00310 nowMode = MANUAL_MODE; 00311 break; 00312 00313 case 2: 00314 //nowMode = AUTO_MODE; 00315 //eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0); 00316 // eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0); 00317 nowMode = AUTO_GPS_MODE; 00318 break; 00319 00320 case 3: 00321 //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2); 00322 nowMode = AUTO_GPS_MODE; 00323 break; 00324 00325 default: 00326 nowMode = STANDBY_MODE; 00327 break; 00328 00329 } 00330 return false; 00331 } 00332 00333 00334
Generated on Wed Jul 13 2022 04:31:34 by 1.7.2