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){
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::control_Motor(int flag){
00193 
00194         if(flag == 0){
00195             _agzSheild.changeSpeed(1, 64, 1, 64); //straight
00196             motor_command = 'f';
00197         }
00198         if(flag == 1){
00199             _agzSheild.changeSpeed(0, 64, 0, 64); 
00200             motor_command = 's';
00201         }
00202         if(flag == 2){
00203              _agzSheild.changeSpeed(1, 64, 2, 64); //Turn Right
00204             motor_command = 'r';
00205         }   
00206         if(flag == 3){
00207             _agzSheild.changeSpeed(2, 64, 2, 64); 
00208             motor_command = 'b';
00209         }
00210 }
00211 
00212 bool AigamozuControlPackets::gpsAuto(const int autobase[]){
00213     
00214    /*
00215     _agzSheild.changeSpeed(2,128,2,128):for moving robot
00216     */
00217     
00218     Timer Automove_Timer;
00219     
00220     bool out_flag = true;
00221     static bool out_count_flag = false;
00222     
00223     if(AigamozuControlPackets::checkGpsHit(basePointKalman[autobase[0]],basePointKalman[autobase[1]],basePointKalman[autobase[2]],agzPointKalman)){
00224         out_flag = false;
00225         out_count_flag = false;
00226     }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[autobase[2]],basePointKalman[autobase[3]],basePointKalman[autobase[0]],agzPointKalman)){
00227         out_flag = false;
00228         out_count_flag = false;
00229     }else{//if robot is out 
00230         out_flag = true;
00231     }
00232  
00233     //if robot is out:
00234     if(out_flag == true || out_count_flag == false){
00235         Automove_Timer.reset();
00236         out_count_flag = true;
00237     }
00238 /*    if(nowMode == AUTO_GPS_MODE){
00239          
00240    if(out_flag){
00241         if(Automove_Timer.read_ms() < 5000){
00242             _agzSheild.changeSpeed(2,64,2,64);//back
00243         }else if(Automove_Timer.read_ms() < 6000){
00244             _agzSheild.changeSpeed(1,64,1,64);//straight
00245         }else if(Automove_Timer.read_ms() >= 6000){
00246             out_count_flag=false;
00247             out_flag=false;
00248         }
00249     }else{
00250     //if robot is inner
00251         _agzSheild.changeSpeed(1,64,1,64);//straight
00252     }
00253     }   
00254 */ 
00255     return out_flag;
00256  
00257 }
00258     
00259     
00260 //Update Robot Point    
00261 void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
00262     agzPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
00263     agzPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
00264     
00265     }
00266     
00267 //Updata Base Point   
00268 void AigamozuControlPackets::reNewBasePoint(int id, uint8_t *latitude,uint8_t *longitude){
00269     UNION_double_char lat,longi;
00270     for(int i = 0;i < 8;i++){
00271         lat.char_value[i]=latitude[i];
00272     }
00273     for(int i = 0;i < 8;i++){
00274         longi.char_value[i]=longitude[i];
00275     }
00276     basePoint[id].x = lat.double_value;
00277     basePoint[id].y = longi.double_value;
00278 }   
00279 
00280 //Update Robot Point    
00281 void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
00282     agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
00283     agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
00284     
00285     }
00286     
00287 //Updata Base Point   
00288 void AigamozuControlPackets::reNewBasePointKalman(int id, uint8_t *latitude,uint8_t *longitude){
00289     UNION_double_char lat,longi;
00290     for(int i = 0;i < 8;i++){
00291         lat.char_value[i]=latitude[i];
00292     }
00293     for(int i = 0;i < 8;i++){
00294         longi.char_value[i]=longitude[i];
00295     }
00296     
00297     basePointKalman[id].x = lat.double_value;
00298     basePointKalman[id].y = longi.double_value;
00299 } 
00300 
00301 
00302 //Check Hit Point Area
00303 bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){
00304     vector2D AB = AigamozuControlPackets::sub_vector(B, A);
00305     vector2D BP = AigamozuControlPackets::sub_vector(P, B);
00306 
00307     vector2D BC = AigamozuControlPackets::sub_vector(C, B);
00308     vector2D CP = AigamozuControlPackets::sub_vector(P, C);
00309 
00310     vector2D CA = AigamozuControlPackets::sub_vector(A, C);
00311     vector2D AP = AigamozuControlPackets::sub_vector(P, A);
00312     
00313     double c1 = AB.x * BP.y - AB.y * BP.x;
00314     double c2 = BC.x * CP.y - BC.y * CP.x;
00315     double c3 = CA.x * AP.y - CA.y * AP.x;
00316 
00317     if( ( c1 > 0 && c2 > 0 && c3 > 0 ) || ( c1 < 0 && c2 < 0 && c3 < 0 ) ) {    
00318         return true;
00319     }
00320     
00321     return false;
00322 
00323     }
00324 
00325 vector2D AigamozuControlPackets::sub_vector( const vector2D& a, const vector2D& b )
00326 {
00327     vector2D ret;
00328     ret.x = a.x - b.x;
00329     ret.y = a.y - b.y;
00330     return ret;   
00331 }    
00332 
00333 
00334 
00335 
00336 //////////////////////////////
00337 //     Mode change          //
00338 //////////////////////////////
00339 bool AigamozuControlPackets::changeMode(uint8_t *buf){
00340     
00341     //reset
00342     _agzSheild.changeSpeed(0,0,0,0);
00343     eachModeInt.detach();
00344 
00345     //Select Mode
00346     switch(buf[19]){
00347         case 0:
00348             nowMode = STANDBY_MODE;
00349         break;
00350         
00351         case 1:
00352             eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
00353             nowMode = MANUAL_MODE;
00354         break;
00355         
00356         case 2:
00357             //nowMode = AUTO_MODE;
00358             //eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0);
00359             //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0);
00360             nowMode = AUTO_GPS_MODE;
00361             Move_Timer.reset();
00362         break;
00363         
00364         case 3:
00365             //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2);
00366             nowMode = AUTO_GPS_MODE;
00367             Move_Timer.reset();
00368         break;
00369         
00370         default:
00371             nowMode = STANDBY_MODE;
00372         break;
00373         
00374         }
00375     return false;
00376 }
00377 
00378 
00379