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     basePointKalman[0].x = 37.524505;
00016     basePointKalman[0].y = 139.939510;
00017     
00018     //basePoint[1].x = 13956.2898;
00019     //basePoint[1].y = 3731.5055;
00020     basePoint[1].x = 37.524747;
00021     basePoint[1].y = 139.939510;
00022     basePointKalman[1].x = 37.524747;
00023     basePointKalman[1].y = 139.939510;
00024     
00025     //basePoint[2].x = 13956.2915;
00026     //basePoint[2].y = 3731.5245;
00027     basePoint[2].x = 37.524747;
00028     basePoint[2].y = 139.939530;
00029     basePointKalman[2].x = 37.524747;
00030     basePointKalman[2].y = 139.939530;  
00031       
00032     //basePoint[3].x = 13956.2680;
00033     //basePoint[3].y = 3731.5248;
00034     basePoint[3].x = 37.524505;
00035     basePoint[3].y = 139.939530;
00036     basePointKalman[3].x = 37.524505;
00037     basePointKalman[3].y = 139.939530;
00038     
00039     agzPoint.x=37.524600;agzPoint.y=139.939520;
00040     agzPointKalman.x=37.524600;agzPointKalman.y=139.939520;
00041     agzCov.x=4.385965;agzCov.y=4.385965;
00042     }
00043 
00044 //////////////////////////////
00045 // get関数
00046 //////////////////////////////
00047 double AigamozuControlPackets::get_agzPoint_lati(){
00048     return agzPoint.x;
00049     }
00050 double AigamozuControlPackets::get_agzPoint_longi(){
00051     return agzPoint.y;
00052     }
00053 double AigamozuControlPackets::get_agzPointKalman_lati(){
00054     return agzPointKalman.x;
00055     }
00056 double AigamozuControlPackets::get_agzPointKalman_longi(){
00057     return agzPointKalman.y;
00058     }
00059 double AigamozuControlPackets::get_agzCov_lati(){
00060     return agzCov.x;
00061 }
00062 double AigamozuControlPackets::get_agzCov_longi(){
00063     return agzCov.y;
00064 }
00065 
00066 double AigamozuControlPackets::get_basePointKalman_lati(int i){
00067     return basePointKalman[i].x;
00068     }
00069 double AigamozuControlPackets::get_basePointKalman_longi(int i){
00070     return basePointKalman[i].y;
00071     }
00072 //////////////////////////////
00073 // set関数
00074 //////////////////////////////
00075 void AigamozuControlPackets::set_agzCov(double cov_lati,double cov_longi){
00076     agzCov.x = cov_lati;
00077     agzCov.y = cov_longi;    
00078 }
00079 
00080 
00081 void AigamozuControlPackets::set_agzAutoGPS(){
00082     agzPoint.x += 0.00001;
00083     //agzPoint.y += 0.001;
00084 }
00085 
00086 void AigamozuControlPackets::set_agzKalmanGPS(){
00087     agzPointKalman.x += 0.00001;
00088     //agzPointKalman.y += 0.001;
00089     
00090 }
00091 
00092 //////////////////////////////
00093 // Controller/Base -> Robot //
00094 //////////////////////////////
00095 
00096  void AigamozuControlPackets::createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR)
00097  {         
00098     uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'};
00099     for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
00100     packetLength = RECEIVE_STATUS_COMMNAD_LENGTH;
00101      }
00102      
00103 void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID)
00104 {
00105     uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'};
00106     for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; 
00107     packetLength = REQUEST_COMMNAD_LENGTH;
00108     }
00109     
00110 void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode){
00111     uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,mode,'A','G','E'};
00112     for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i];
00113     packetLength = CHANGE_MODE_COMMAND_LENGTH;
00114     }
00115     
00116 
00117 //////////////////////////////
00118 // Robot -> Controller/Base //
00119 //////////////////////////////
00120 
00121 void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,double latitude,double longitude,double latitudeKalman,double longitudeKalman,double covarLati,double covarLongi)
00122 {
00123     UNION_double_char latitude_data,longitude_data, latitudeKalman_data, longitudeKalman_data;
00124     UNION_double_char covarLati_data,covarLongi_data;
00125     
00126     latitude_data.double_value=latitude;
00127     longitude_data.double_value=longitude;
00128     longitude_data.double_value=latitudeKalman;
00129     longitudeKalman_data.double_value=longitudeKalman;
00130     covarLati_data.double_value=covarLati;
00131     covarLongi_data.double_value=covarLongi;
00132       
00133     uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S',nowMode,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 bool AigamozuControlPackets::gpsAuto(){
00193     
00194    /*
00195     _agzSheild.changeSpeed(2,128,2,128):for moving robot
00196     */
00197     
00198     Timer Automove_Timer;
00199     
00200     bool out_flag = true;
00201     static bool out_count_flag = false;
00202     
00203     if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)){
00204         out_flag = false;
00205         out_count_flag = false;
00206     }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[0],agzPointKalman)){
00207         out_flag = false;
00208         out_count_flag = false;
00209     }else{//if robot is out 
00210         out_flag = true;
00211     }
00212  
00213     //if robot is out:
00214     if(out_flag == true || out_count_flag == false){
00215         Automove_Timer.reset();
00216         out_count_flag = true;
00217     }
00218     if(out_flag){
00219         if(Automove_Timer.read_ms() < 5000){
00220             _agzSheild.changeSpeed(2,64,2,64);//back
00221         }else if(Automove_Timer.read_ms() < 6000){
00222             _agzSheild.changeSpeed(1,64,2,64);//straight
00223         }else if(Automove_Timer.read_ms() >= 6000){
00224             out_count_flag=false;
00225             out_flag=false;
00226         }
00227     }else{
00228     //if robot is inner
00229         _agzSheild.changeSpeed(1,64,1,64);//straight
00230     }
00231     
00232     return out_flag;
00233  
00234 }
00235  
00236  
00237  
00238     
00239     
00240 //Update Robot Point    
00241 void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
00242     agzPoint.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0);
00243     agzPoint.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0);
00244     
00245     }
00246     
00247 //Updata Base Point   
00248 void AigamozuControlPackets::reNewBasePoint(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
00249     basePoint[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0);
00250     basePoint[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0);
00251 }   
00252 
00253 //Update Robot Point    
00254 void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
00255     agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 1000000.0/60.0);
00256     agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 1000000.0/60.0);
00257     
00258     }
00259     
00260 //Updata Base Point   
00261 void AigamozuControlPackets::reNewBasePointKalman(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL){
00262     basePointKalman[id].x = (double)latitudeH + (double)(latitudeL / 1000000.0);
00263     basePointKalman[id].y = (double)longitudeH +(double)(longitudeL / 1000000.0);
00264 }   
00265 
00266 
00267 //Check Hit Point Area
00268 bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){
00269     vector2D AB = AigamozuControlPackets::sub_vector(B, A);
00270     vector2D BP = AigamozuControlPackets::sub_vector(P, B);
00271 
00272     vector2D BC = AigamozuControlPackets::sub_vector(C, B);
00273     vector2D CP = AigamozuControlPackets::sub_vector(P, C);
00274 
00275     vector2D CA = AigamozuControlPackets::sub_vector(A, C);
00276     vector2D AP = AigamozuControlPackets::sub_vector(P, A);
00277     
00278     double c1 = AB.x * BP.y - AB.y * BP.x;
00279     double c2 = BC.x * CP.y - BC.y * CP.x;
00280     double c3 = CA.x * AP.y - CA.y * AP.x;
00281 
00282     if( ( c1 > 0 && c2 > 0 && c3 > 0 ) || ( c1 < 0 && c2 < 0 && c3 < 0 ) ) {    
00283         return true;
00284     }
00285     
00286     return false;
00287 
00288     }
00289 
00290 vector2D AigamozuControlPackets::sub_vector( const vector2D& a, const vector2D& b )
00291 {
00292     vector2D ret;
00293     ret.x = a.x - b.x;
00294     ret.y = a.y - b.y;
00295     return ret;   
00296 }    
00297 
00298 
00299 
00300 
00301 //////////////////////////////
00302 //     Mode change          //
00303 //////////////////////////////
00304 bool AigamozuControlPackets::changeMode(uint8_t *buf){
00305     
00306     //reset
00307     _agzSheild.changeSpeed(0,0,0,0);
00308     eachModeInt.detach();
00309 
00310     //Select Mode
00311     switch(buf[19]){
00312         case 0:
00313             nowMode = STANDBY_MODE;
00314         break;
00315         
00316         case 1:
00317             eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
00318             nowMode = MANUAL_MODE;
00319         break;
00320         
00321         case 2:
00322             //nowMode = AUTO_MODE;
00323             //eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0);
00324 //            eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0);
00325             nowMode = AUTO_GPS_MODE;
00326         break;
00327         
00328         case 3:
00329             //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2);
00330             nowMode = AUTO_GPS_MODE;
00331         break;
00332         
00333         default:
00334             nowMode = STANDBY_MODE;
00335         break;
00336         
00337         }
00338     return false;
00339 }
00340 
00341 
00342