Diff: AigamozuControlPackets.cpp

Revision:

50:3511be172d81

Parent:

48:ee5a6906273e

--- a/AigamozuControlPackets.cpp	Sat Jun 11 03:40:16 2016 +0000
+++ b/AigamozuControlPackets.cpp	Sun Oct 29 00:56:51 2017 +0000
@@ -1,405 +1,207 @@
 #include "AigamozuControlPackets.h"
-#include "VNH5019.h"
 
-const int turn_time[3] = {1, 2, 3};
-int tt = 0;
-
-//////////////////////////////
-//          Init            //
-//////////////////////////////
-AigamozuControlPackets::AigamozuControlPackets(VNH5019 agzSheild):_agzSheild(agzSheild){
+/////////////////////////////////////////
+//
+// Constructor
+//
+/////////////////////////////////////////
+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;
+
+    agzPoint.x = 0.0;
+    agzPoint.y = 0.0;
+
+    for(int i=0; i<BASENUMBER; i++) {
+        basePoint[i].x = 0.0;
+        basePoint[i].y = 0.0;
+    }
+}
+
+AigamozuControlPackets::vector2D AigamozuControlPackets::sub_vector(vector2D A, vector2D B)
+{
+    vector2D ret;
+    ret.x = B.x - A.x;
+    ret.y = B.y - A.y;
+    return ret;
+}
+
+bool AigamozuControlPackets::checkInOut(vector2D A, vector2D B, vector2D C, vector2D P)
+{
+    vector2D BA = sub_vector(B, A);
+    vector2D BP = sub_vector(B, P);
+    vector2D BC = sub_vector(B, C);
+
+    double c1 = BA.x * BP.y - BA.y * BP.x;
+    double c2 = BP.x * BC.y - BP.y * BC.x;
+
+    if((c1>0 && c2>0) || (c1<0 && c2<0)) return true;
+    else return false;
+}
+
+void AigamozuControlPackets::autoMoving(void)
+{
+
+    if(checkInOut(basePoint[0], basePoint[1], basePoint[2], agzPoint)
+            && checkInOut(basePoint[2], basePoint[3], basePoint[0], agzPoint)) {
+        out_flag = false;
+    } else {
+        out_flag = true;
     }
 
-//////////////////////////////
-// 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;    
+
+/////////////////////////////////////////
+//
+// Create Packet: Robot -> Base,
+//                Manager -> Robot/Base
+//
+/////////////////////////////////////////
+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::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)
+/////////////////////////////////////////
+//
+// Create Packet: Base -> Robot/Manager,
+//                Robot -> Manager
+//
+/////////////////////////////////////////
+void AigamozuControlPackets::createReceiveStatusCommand(uint8_t fromID, uint8_t toID, int state)
 {
-    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 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;
-    }
-    
+    UNION_double_char latitude_data,longitude_data;
 
-//////////////////////////////
-// Robot -> Controller/Base //
-//////////////////////////////
+    latitude_data.double_value=agzPoint.y;
+    longitude_data.double_value=agzPoint.x;
+
 
-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};
+    uint8_t tmp[] = {'A', 'G', 'S', 'R', 'F', fromID, 'T', toID, 'S', state,'G', 'P', 'S',
+                     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],
+                     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                     'A', 'G', 'E'
+                    };
     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){
-    return buf[14];
-    }
-
-uint8_t* AigamozuControlPackets::getPacketData(){
-    return packetData;
-    }
-
-int AigamozuControlPackets::getPacketLength(){
-        return packetLength;
-    }
-
-void AigamozuControlPackets::changeSpeed(uint8_t* buf){
-    if(nowMode == MANUAL_MODE){
-            manualCount =0;
-            _agzSheild.changeSpeed(buf[20],buf[21],buf[23],buf[24]);
-        }
-    }
-
-//////////////////////////////
-//   each mode interrupt    //
-//////////////////////////////
-
-void AigamozuControlPackets::manualMode(){
-    manualCount++;
-    if(manualCount > 10){
-        _agzSheild.changeSpeed(0,0,0,0);
-        manualCount = 0;
-        }
-    }
-
-
-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::test_Auto(int flag){
-
-        if(flag == 0){
-            _agzSheild.changeSpeed(1, 64, 1, 64); //straight
-        }
-        if(flag == 1){
-            _agzSheild.changeSpeed(0, 64, 0, 64); 
-        }
-        if(flag == 2){
-             _agzSheild.changeSpeed(1, 64, 1, 16); //Turn Right
-        }   
-        if(flag == 3){
-            _agzSheild.changeSpeed(0, 64, 0, 64); 
-        }
-}
-*/
-
-bool AigamozuControlPackets::gpsAuto(){
-    
-   /*
-    _agzSheild.changeSpeed(2,128,2,128):for moving robot
-    */
-    
-    //Timer Automove_Timer;
-   // bool out_flag = true;
-   // static bool out_count_flag = false;
-    const int straight = 7000, wait = 8000;
-    int turning = wait + 1000*turn_time[tt]; //decide turning time
-    tt++;
-    if(tt == 3){
-        tt = 0;
-    }
-    
-
-    if(AigamozuControlPackets::checkGpsHit(basePointKalman[0],basePointKalman[1],basePointKalman[2],agzPointKalman)
-     && AigamozuControlPackets::checkGpsHit(basePointKalman[2],basePointKalman[3],basePointKalman[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 == true){
-         /*   if(Automove_Timer.read_ms() <= 5000){
-                _agzSheild.changeSpeed(1,16,1,64);//turn left
-            }else if(Automove_Timer.read_ms() <= 6000){
-                _agzSheild.changeSpeed(0,64,0,64);
-            }else if(Automove_Timer.read_ms() <= 10000){
-                _agzSheild.changeSpeed(1,64,1,64);//straight*/
-            if(Automove_Timer.read_ms() <5000){
-                 _agzSheild.changeSpeed(2,speed,2, speed);
-            }else if(Automove_Timer.read_ms() > 5000){
-                Move_Timer.reset();
-                _agzSheild.changeSpeed(0,speed,0,speed);
-                out_count_flag=false;
-                out_flag=false;
-                printf("reset timer\n");
-            }
-        }else{
-        //if robot is inner
-           // _agzSheild.changeSpeed(1,64,1,64);//straight
-            if(Move_Timer.read_ms() < straight){
-                _agzSheild.changeSpeed(1, speed, 1, speed); //straight
-            }
-            else if(Move_Timer.read_ms() < wait){
-                _agzSheild.changeSpeed(0, speed, 0, speed); 
-            }
-            else if(Move_Timer.read_ms() < turning){
-                _agzSheild.changeSpeed(1, speed, 1, 16); //Turn Right
-            }   
-            else if(Move_Timer.read_ms() > turning){
-                _agzSheild.changeSpeed(0, speed, 0, speed); 
-                wait_ms(500);
-                Move_Timer.reset();
-            }
-        }  
-    } 
-    
-    return out_flag;
- 
-}
-        
-        
-//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( vector2D A, vector2D B, vector2D C, vector2D P){
-    vector2D AB = AigamozuControlPackets::sub_vector(A, B);
-    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 = BP.x * BC.y - BP.y * BC.x;
-    
-    if( ( c1 > 0 && c2 > 0) || ( c1 < 0 && c2 < 0) ) {    
-        return true;
-    }
-    
-    return false;
-
+/////////////////////////////////////////
+//
+// Update data
+//
+/////////////////////////////////////////
+void AigamozuControlPackets::reNewRobotPoint(long latitudeH, long latitudeL, long longitudeH, long longitudeL)
+{
+    agzPoint.x = (double)longitudeH + (double)(longitudeL / 10000.0/60.0);
+    agzPoint.y = (double)latitudeH +(double)(latitudeL / 10000.0/60.0);
 }
 
-vector2D AigamozuControlPackets::sub_vector( const vector2D& a, const vector2D& b )
+void AigamozuControlPackets::reNewBasePoint(int id, uint8_t *latitude, uint8_t *longitude)
 {
-    vector2D ret;
-    ret.x = a.x - b.x;
-    ret.y = a.y - b.y;
-    return ret;   
-}    
+    UNION_double_char latitude_data,longitude_data;
+    for(int i = 0; i < 8; i++) {
+        latitude_data.char_value[i]=latitude[i];
+    }
+    for(int i = 0; i < 8; i++) {
+        longitude_data.char_value[i]=longitude[i];
+    }
+    basePoint[id].x = longitude_data.double_value;
+    basePoint[id].y = latitude_data.double_value;
+}
 
-
-
+/////////////////////////////////////////
+//
+// Manual Mode
+//
+/////////////////////////////////////////
+void AigamozuControlPackets::manualMode() {}
 
-//////////////////////////////
-//     Mode change          //
-//////////////////////////////
-bool AigamozuControlPackets::changeMode(uint8_t *buf){
-    
+/////////////////////////////////////////
+//
+// Change Mode
+//
+/////////////////////////////////////////
+void AigamozuControlPackets::changeMode(uint8_t *buf)
+{
+
     //reset
     _agzSheild.changeSpeed(0,0,0,0);
     eachModeInt.detach();
 
     //Select Mode
-    switch(buf[19]){
+    switch(buf[8]) {
         case 0:
             nowMode = STANDBY_MODE;
-        break;
-        
+            break;
+
         case 1:
             eachModeInt.attach(this,&AigamozuControlPackets::manualMode,1.0);
             nowMode = MANUAL_MODE;
-        break;
-        
+            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;
-        
+            nowMode = AUTO_MODE;
+            //Move_Timer.reset();
+            break;
+
         default:
             nowMode = STANDBY_MODE;
-        break;
-        
-        }
-    return false;
+            break;
+
+    }
+}
+/////////////////////////////////////////
+//
+// Set
+//
+/////////////////////////////////////////
+void AigamozuControlPackets::set_nowStatus(STATUS s)
+{
+    nowStatus = s;
 }
 
 
-    
+
+/////////////////////////////////////////
+//
+// Get
+//
+/////////////////////////////////////////
+MODE AigamozuControlPackets::get_nowMode(void)
+{
+    return nowMode;
+}
+uint8_t* AigamozuControlPackets::get_packetData(void)
+{
+    return packetData;
+}
+int AigamozuControlPackets::get_packetLength(void)
+{
+    return packetLength;
+}
+bool AigamozuControlPackets::get_out_flag(void)
+{
+    return out_flag;
+}
+double AigamozuControlPackets::get_agzPoint_latitude(void)
+{
+    return agzPoint.y;
+}
+double AigamozuControlPackets::get_agzPoint_longitude(void)
+{
+    return agzPoint.x;
+}
+double AigamozuControlPackets::get_basePoint_latitude(int i)
+{
+    return basePoint[i].y;
+}
+double AigamozuControlPackets::get_basePoint_longitude(int i)
+{
+    return basePoint[i].x;
+}
\ No newline at end of file