Diff: AigamozuControlPackets.cpp

Revision:

41:fbf8f617a6ab

Parent:

40:c0cf45cf5eeb

Child:

42:04ca5242eaf2

Child:

50:61d72f617aa5

--- a/AigamozuControlPackets.cpp	Wed Jan 06 10:48:57 2016 +0000
+++ b/AigamozuControlPackets.cpp	Mon Oct 24 08:14:05 2016 +0000
@@ -1,150 +1,17 @@
 #include "AigamozuControlPackets.h"
 #include "VNH5019.h"
 
+#define RIGHT_SPEED 16
+#define LEFT_SPEED 54
+#define FORWARD_SPEED 64
+
 //////////////////////////////
 //          Init            //
 //////////////////////////////
 AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){
-    packetData = new uint8_t[50];
-    packetLength = 0;
-    //--init base point--//
-    //basePoint[0].x = 13956.2655;
-    //basePoint[0].y = 3731.5060;
-    basePoint[0].x = 100.2655;
-    basePoint[0].y = 30.5060;
-    
-    //basePoint[1].x = 13956.2898;
-    //basePoint[1].y = 3731.5055;
-    basePoint[1].x = 200.2898;
-    basePoint[1].y = 30.5055;
-    
-    //basePoint[2].x = 13956.2915;
-    //basePoint[2].y = 3731.5245;
-    basePoint[2].x = 200.2915;
-    basePoint[2].y = 40.5245;
-    
-    //basePoint[3].x = 13956.2680;
-    //basePoint[3].y = 3731.5248;
-    basePoint[3].x = 100.2680;
-    basePoint[3].y = 40.5248;
-    
-    agzPoint.x=0;agzPoint.y=0;
-    agzPointKalman.x=0;agzPointKalman.y=0;
-    agzCov.x=0;agzCov.y=0;
     }
 
 //////////////////////////////
-// get関数
-//////////////////////////////
-double AigamozuControlPackets::get_agzPoint_lati(){
-    return agzPoint.x;
-    }
-double AigamozuControlPackets::get_agzPoint_longi(){
-    return agzPoint.y;
-    }
-double AigamozuControlPackets::get_agzPointKalman_lati(){
-    return agzPointKalman.x;
-    }
-double AigamozuControlPackets::get_agzPointKalman_longi(){
-    return agzPointKalman.y;
-    }
-double AigamozuControlPackets::get_agzCov_lati(){
-    return agzCov.x;
-}
-double AigamozuControlPackets::get_agzCov_longi(){
-    return agzCov.y;
-}
-   
-double AigamozuControlPackets::get_basePoint_lati(int i){
-    return basePoint[i].x;
-    }
-double AigamozuControlPackets::get_basePoint_longi(int i){
-    return basePoint[i].y;
-    }
-double AigamozuControlPackets::get_basePointKalman_lati(int i){
-    return basePointKalman[i].x;
-    }
-double AigamozuControlPackets::get_basePointKalman_longi(int i){
-    return basePointKalman[i].y;
-    }
-    
-//////////////////////////////
-// set関数
-//////////////////////////////
-void AigamozuControlPackets::set_agzCov(double cov_lati,double cov_longi){
-    agzCov.x = cov_lati;
-    agzCov.y = cov_longi;    
-}
-
-void AigamozuControlPackets::set_agzPointKalman_lati(double kalman_lati){
-
-    agzPointKalman.x = kalman_lati;   
-     
-}
-        
-void AigamozuControlPackets::set_agzPointKalman_longi(double kalman_longi){
-    
-    agzPointKalman.y = kalman_longi;
-    
-}
-
-//////////////////////////////
-// Controller/Base -> Robot //
-//////////////////////////////
-
- void AigamozuControlPackets::createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR)
- {         
-    uint8_t tmp[] = {'A','G','S','M','F',fromID,'T',toID,'L',directionL,pwmL,'R',directionR,pwmR,'A','G','E'};
-    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
-    packetLength = RECEIVE_STATUS_COMMNAD_LENGTH;
-     }
-     
-void AigamozuControlPackets::createRequestCommand(uint8_t fromID,uint8_t toID)
-{
-    uint8_t tmp[] = {'A','G','S','S','F',fromID,'T',toID,'A','G','E'};
-    for(int i = 0; i < REQUEST_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i]; 
-    packetLength = REQUEST_COMMNAD_LENGTH;
-    }
-    
-void AigamozuControlPackets::createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t mode){
-    uint8_t tmp[] = {'A','G','S','C','F',fromID,'T',toID,mode,'A','G','E'};
-    for(int i = 0; i < CHANGE_MODE_COMMAND_LENGTH; ++i) packetData[i] = tmp[i];
-    packetLength = CHANGE_MODE_COMMAND_LENGTH;
-    }
-    
-
-//////////////////////////////
-// Robot -> Controller/Base //
-//////////////////////////////
-
-void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID,uint8_t toID,int state, double latitude,double longitude,double latitudeKalman,double longitudeKalman,double covarLati,double covarLongi)
-{
-    UNION_double_char latitude_data,longitude_data, latitudeKalman_data, longitudeKalman_data;
-    UNION_double_char covarLati_data,covarLongi_data;
-    
-    latitude_data.double_value=latitude;
-    longitude_data.double_value=longitude;
-    latitudeKalman_data.double_value=latitudeKalman;
-    longitudeKalman_data.double_value=longitudeKalman;
-    covarLati_data.double_value=covarLati;
-    covarLongi_data.double_value=covarLongi;
-    
-    
-    uint8_t tmp[] = {'A','G','S','R','F',fromID,'T',toID,'S', state,71,80,83,
-                    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],
-                    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],
-                    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],
-                    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],      
-                    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],
-                    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],
-                    65,71,69};
-    for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) packetData[i] = tmp[i];
-    packetLength =  RECEIVE_STATUS_COMMNAD_LENGTH;
-}
-
-
-
-//////////////////////////////
 //    Using createPacket    //
 //////////////////////////////
 uint8_t AigamozuControlPackets::checkCommnadType(uint8_t* buf){
@@ -178,157 +45,25 @@
         }
     }
 
-
-void AigamozuControlPackets::randomAuto(){
-    
-    randomCount++;
-    
-    if(randomCount < 10) _agzSheild.changeSpeed(1,128,1,128);
-    else if(randomCount < 11) _agzSheild.changeSpeed(2,128,2,128);
-    else randomCount = 0;
-    
-    }
-    
-void AigamozuControlPackets::control_Motor(int flag){
-
-        if(flag == 0){
-            _agzSheild.changeSpeed(1, 64, 1, 64); //straight
-            motor_command = 'f';
-        }
-        if(flag == 1){
-            _agzSheild.changeSpeed(0, 64, 0, 64); 
-            motor_command = 's';
-        }
-        if(flag == 2){
-             _agzSheild.changeSpeed(1, 64, 2, 64); //Turn Right
-            motor_command = 'r';
-        }   
-        if(flag == 3){
-            _agzSheild.changeSpeed(2, 64, 2, 64); 
-            motor_command = 'b';
-        }
-}
-
-bool AigamozuControlPackets::gpsAuto(const int autobase[]){
+bool AigamozuControlPackets::gpsAuto(/*const int autobase[]*/){
     
    /*
     _agzSheild.changeSpeed(2,128,2,128):for moving robot
     */
     
-    Timer Automove_Timer;
-    
-    bool out_flag = true;
-    static bool out_count_flag = false;
-    
-    if(AigamozuControlPackets::checkGpsHit(basePointKalman[autobase[0]],basePointKalman[autobase[1]],basePointKalman[autobase[2]],agzPointKalman)){
-        out_flag = false;
-        out_count_flag = false;
-    }else if(AigamozuControlPackets::checkGpsHit(basePointKalman[autobase[2]],basePointKalman[autobase[3]],basePointKalman[autobase[0]],agzPointKalman)){
-        out_flag = false;
-        out_count_flag = false;
-    }else{//if robot is out 
-        out_flag = true;
-    }
- 
-    //if robot is out:
-    if(out_flag == true || out_count_flag == false){
-        Automove_Timer.reset();
-        out_count_flag = true;
-    }
-/*    if(nowMode == AUTO_GPS_MODE){
-         
-   if(out_flag){
-        if(Automove_Timer.read_ms() < 5000){
-            _agzSheild.changeSpeed(2,64,2,64);//back
-        }else if(Automove_Timer.read_ms() < 6000){
-            _agzSheild.changeSpeed(1,64,1,64);//straight
-        }else if(Automove_Timer.read_ms() >= 6000){
-            out_count_flag=false;
-            out_flag=false;
+   if(nowMode == AUTO_GPS_MODE){
+
+        if(Move_Timer.read_ms() < 6000){
+            _agzSheild.changeSpeed(1,LEFT_SPEED,1,RIGHT_SPEED);//straight
+        }else if(Move_Timer.read_ms() >= 6000){
+            _agzSheild.changeSpeed(0,LEFT_SPEED,0,RIGHT_SPEED);
+            Move_Timer.reset();
         }
-    }else{
-    //if robot is inner
-        _agzSheild.changeSpeed(1,64,1,64);//straight
-    }
     }   
-*/ 
-    return out_flag;
- 
+
+ return 1;
 }
-    
-    
-//Update Robot Point    
-void AigamozuControlPackets::reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
-    agzPoint.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
-    agzPoint.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
-    
-    }
-    
-//Updata Base Point   
-void AigamozuControlPackets::reNewBasePoint(int id, uint8_t *latitude,uint8_t *longitude){
-    UNION_double_char lat,longi;
-    for(int i = 0;i < 8;i++){
-        lat.char_value[i]=latitude[i];
-    }
-    for(int i = 0;i < 8;i++){
-        longi.char_value[i]=longitude[i];
-    }
-    basePoint[id].x = lat.double_value;
-    basePoint[id].y = longi.double_value;
-}   
-
-//Update Robot Point    
-void AigamozuControlPackets::reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL){
-    agzPointKalman.x = (double)latitudeH + (double)(latitudeL / 10000.0/60.0);
-    agzPointKalman.y = (double)longitudeH +(double)(longitudeL / 10000.0/60.0);
-    
-    }
-    
-//Updata Base Point   
-void AigamozuControlPackets::reNewBasePointKalman(int id, uint8_t *latitude,uint8_t *longitude){
-    UNION_double_char lat,longi;
-    for(int i = 0;i < 8;i++){
-        lat.char_value[i]=latitude[i];
-    }
-    for(int i = 0;i < 8;i++){
-        longi.char_value[i]=longitude[i];
-    }
-    
-    basePointKalman[id].x = lat.double_value;
-    basePointKalman[id].y = longi.double_value;
-} 
-
-
-//Check Hit Point Area
-bool AigamozuControlPackets::checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P){
-    vector2D AB = AigamozuControlPackets::sub_vector(B, A);
-    vector2D BP = AigamozuControlPackets::sub_vector(P, B);
-
-    vector2D BC = AigamozuControlPackets::sub_vector(C, B);
-    vector2D CP = AigamozuControlPackets::sub_vector(P, C);
-
-    vector2D CA = AigamozuControlPackets::sub_vector(A, C);
-    vector2D AP = AigamozuControlPackets::sub_vector(P, A);
-    
-    double c1 = AB.x * BP.y - AB.y * BP.x;
-    double c2 = BC.x * CP.y - BC.y * CP.x;
-    double c3 = CA.x * AP.y - CA.y * AP.x;
-
-    if( ( c1 > 0 && c2 > 0 && c3 > 0 ) || ( c1 < 0 && c2 < 0 && c3 < 0 ) ) {    
-        return true;
-    }
-    
-    return false;
-
-    }
-
-vector2D AigamozuControlPackets::sub_vector( const vector2D& a, const vector2D& b )
-{
-    vector2D ret;
-    ret.x = a.x - b.x;
-    ret.y = a.y - b.y;
-    return ret;   
-}    
+ 
 
 
 
@@ -354,15 +89,11 @@
         break;
         
         case 2:
-            //nowMode = AUTO_MODE;
-            //eachModeInt.attach(this,&AigamozuControlPackets::randomAuto,1.0);
-            //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,2.0);
             nowMode = AUTO_GPS_MODE;
             Move_Timer.reset();
         break;
         
         case 3:
-            //eachModeInt.attach(this,&AigamozuControlPackets::gpsAuto,0.2);
             nowMode = AUTO_GPS_MODE;
             Move_Timer.reset();
         break;