2016_05_19ver Auto mode 10sec forward, 2sec stop, 2sec right turn Please change test_mode's right turn ppm

Dependencies:   VNH5019 AigamozuControlPackets_2016

Dependents:   Aigamozu_Robot_2016_ver1 GPSLOG_program AigamozuControlPackets_2016

Fork of AigamozuControlPackets by aigamozu

Files at this revision

API Documentation at this revision

Comitter:
s1210160
Date:
Sun Oct 29 00:56:51 2017 +0000
Parent:
48:ee5a6906273e
Commit message:
commit;

Changed in this revision

AigamozuControlPackets.cpp Show annotated file Show diff for this revision Revisions of this file
AigamozuControlPackets.h Show annotated file Show diff for this revision Revisions of this file
AigamozuControlPackets.lib Show annotated file Show diff for this revision Revisions of this file
diff -r ee5a6906273e -r 3511be172d81 AigamozuControlPackets.cpp
--- 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
diff -r ee5a6906273e -r 3511be172d81 AigamozuControlPackets.h
--- a/AigamozuControlPackets.h	Sat Jun 11 03:40:16 2016 +0000
+++ b/AigamozuControlPackets.h	Sun Oct 29 00:56:51 2017 +0000
@@ -15,114 +15,76 @@
 #define speed 96
 
 enum COMMAND_TYPE {MANUAL = 'M', STATUS_REQUEST = 'S', CHANGE_MODE = 'C', RECEIVE_STATUS = 'R',RECEIVE_KALMAN = 'K'};
-enum MODE {STANDBY_MODE = 0, MANUAL_MODE = 1,AUTO_MODE = 2,AUTO_GPS_MODE = 3};
-enum STATUS {GPS_AVAIL = 0, GPS_UNAVAIL = 1,GPS_OUT_AREA = 2};
-
-union TEST_T{
-    long a;
-    uint8_t b[4];
-    };
-
-union UNION_double_char{
-    double double_value;
-    uint8_t char_value[8];
-    };
-
-struct vector2D{
-    double x;
-    double y;
-    };
+enum MODE {STANDBY_MODE = 0, MANUAL_MODE = 1, AUTO_MODE = 2, AUTO_GPS_MODE = 3};
+enum STATUS {GPS_AVAIL = 0, GPS_UNAVAIL = 1, GPS_OUT_AREA = 2};
 
-class AigamozuControlPackets{
-    
-    //--------PUBLIC-----------//
-    public:
-    AigamozuControlPackets(VNH5019 agzSheild);
-    
-    Ticker eachModeInt;
-    
-    uint8_t* packetData;
-    //Create Packet: Controller/Base -> Robot 
-    void createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR);
-    void createRequestCommand(uint8_t fromID,uint8_t toID);
-    void createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode);
-    
-    //Create Packet: Robot -> Controller/Base 
-    void createReceiveStatusCommand(uint8_t fromID,uint8_t toID, int status, double latitude,double longitude,double latitudeKalman,double longitudeKalman,
-                                    double covarLati,double covarLongi);
-    //uint8_t* createAckPacket(uint8_t fromID,uint8_t toID);
-    
-    //using create packet
-    uint8_t* getPacketData();
-    int getPacketLength();
-    
-    //Change Mode: 
-    bool changeMode(uint8_t *buf); 
-    
-    //Check Command Type using xbee buffer;
-    uint8_t checkCommnadType(uint8_t* buf);
-    
-    //Change Speed
-    void changeSpeed(uint8_t* buf);
-    
+class AigamozuControlPackets
+{
+    //--------PRIVATE-----------//
+private:
+    VNH5019 _agzSheild;
+
     MODE nowMode;
     STATUS nowStatus;
+    bool out_flag;
+
+    union UNION_double_char {
+        double double_value;
+        uint8_t char_value[8];
+    };
+
+    struct vector2D {
+        double x;
+        double y;
+    };
+
+    uint8_t* packetData;
     int packetLength;
-    
-    
-    void reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL);
-    void reNewBasePointKalman(int id, uint8_t *latitude,uint8_t *longitude);
-    void reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL);
-    void reNewBasePoint(int id, uint8_t *latitude,uint8_t *longitude);
+
+    // 割り込み
+    Ticker eachModeInt;
+
+    //自分自身の位置の情報(double型)
+    vector2D agzPoint;
+    //ベースの位置の情報(double型)
+    vector2D basePoint[BASENUMBER];
+
+    //--------PUBLIC-----------//
+public:
+    AigamozuControlPackets(VNH5019 agzSheild);
     
-    double get_agzPoint_lati();
-    double get_agzPoint_longi();
-    double get_agzPointKalman_lati();
-    double get_agzPointKalman_longi();
-    double get_agzCov_lati();
-    double get_agzCov_longi();
-    double get_basePoint_lati(int);
-    double get_basePoint_longi(int);
-    double get_basePointKalman_lati(int);
-    double get_basePointKalman_longi(int);
-
-    void set_agzPointKalman_lati(double);
-    void set_agzPointKalman_longi(double);
-    
-    void set_agzCov(double cov_lati,double cov_longi);
+    vector2D sub_vector(vector2D A, vector2D B);
+    bool checkInOut(vector2D A, vector2D B, vector2D C, vector2D P);
+    void autoMoving(void);
 
-    //Auto Type 2 -> GPS    
-    bool gpsAuto();
-    void test_Auto(int);
-    Timer Move_Timer;
-    Timer Automove_Timer;
-    bool out_flag;
-    bool out_count_flag;
-    
-    int auto_count;
+    // Create Packet: Robot -> Base, Manager -> Robot/Base
+    void createRequestCommand(uint8_t fromID, uint8_t toID);
+
+    // Create Packet: Base -> Robot/Manager, Robot -> Manager
+    void createReceiveStatusCommand(uint8_t fromID, uint8_t toID, int state);
+
+    // update data
+    void reNewRobotPoint(long latitudeH, long latitudeL, long longitudeH, long longitudeL);
+    void reNewBasePoint(int id, uint8_t *latitude, uint8_t *longitude);
+
+    // Manual Mode
+    void manualMode(void);
+
+    // Change Mode
+    void changeMode(uint8_t *buf);
 
-    //--------PRIVATE-----------//
-    private:
-    //
-    VNH5019 _agzSheild;
-    
-    void manualMode();
-    int manualCount;
-    
-    //Auto Type 1 -> Random
-    void randomAuto();
-    int randomCount; 
-    
-    vector2D agzPoint;//自分自身の位置の情報(double型)
-    vector2D agzPointKalman;
-    vector2D agzCov;//カルマンフィルタの共分散
-    vector2D sub_vector( const vector2D& a, const vector2D& b );
-    bool checkGpsHit( vector2D A, vector2D B, vector2D C, vector2D P);
-    
-    vector2D basePoint[BASENUMBER];//ベースの位置の情報(double型)
-    vector2D basePointKalman[BASENUMBER];
-    
-    
-    };
-    
+    // set
+    void set_nowStatus(STATUS s);
+
+    // get
+    MODE get_nowMode(void);
+    uint8_t* get_packetData(void);
+    int get_packetLength(void);
+    bool get_out_flag(void);
+    double get_agzPoint_latitude(void);
+    double get_agzPoint_longitude(void);
+    double get_basePoint_latitude(int i);
+    double get_basePoint_longitude(int i);
+
+};
 #endif
\ No newline at end of file
diff -r ee5a6906273e -r 3511be172d81 AigamozuControlPackets.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AigamozuControlPackets.lib	Sun Oct 29 00:56:51 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/aigamozu/code/AigamozuControlPackets_2016/#ee5a6906273e