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