Diff: AigamozuControlPackets.h

Revision:

50:3511be172d81

Parent:

48:ee5a6906273e

--- 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