Diff: AigamozuControlPackets.h

Revision:

0:806e9a434e81

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AigamozuControlPackets.h	Tue May 12 10:40:07 2015 +0000
@@ -0,0 +1,114 @@
+#include "mbed.h"
+#include "VNH5019.h"
+
+#ifndef AIGAMOZU_CONTROL_PACKETS
+#define AIGAMOZU_CONTROL_PACKETS
+
+#define MANUAL_COMMAND_LENGTH 17
+#define REQUEST_COMMNAD_LENGTH 11
+#define CHANGE_MODE_COMMAND_LENGTH 12
+#define RECEIVE_STATUS_COMMNAD_LENGTH 64
+
+#define vertex2D vector2D
+
+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;
+    };
+
+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,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);
+    
+    MODE nowMode;
+    STATUS nowStatus;
+    int packetLength;
+    
+    
+    void reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL);
+    void reNewBasePointKalman(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL);
+    void reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL);
+    void reNewBasePoint(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL);
+    
+    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_basePointKalman_lati(int);
+    double get_basePointKalman_longi(int);
+    
+    void set_agzAutoGPS();
+    void set_agzKalmanGPS();
+        
+    void set_agzCov(double cov_lati,double cov_longi);
+    
+    //Auto Type 2 -> GPS 
+    bool gpsAuto();
+    //--------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( vertex2D A, vertex2D B, vertex2D C, vertex2D P);
+    
+    vector2D basePoint[4];//ベースの位置の情報（double型）
+    vector2D basePointKalman[4];
+    
+    };
+    
+#endif
\ No newline at end of file