AigamozuControlPackets.h

00001 #include "mbed.h"
00002 #include "VNH5019.h"
00003 
00004 #ifndef AIGAMOZU_CONTROL_PACKETS
00005 #define AIGAMOZU_CONTROL_PACKETS
00006 
00007 #define MANUAL_COMMAND_LENGTH 17
00008 #define REQUEST_COMMNAD_LENGTH 11
00009 #define CHANGE_MODE_COMMAND_LENGTH 12
00010 #define RECEIVE_STATUS_COMMNAD_LENGTH 64
00011 
00012 #define vertex2D vector2D
00013 
00014 enum COMMAND_TYPE {MANUAL = 'M', STATUS_REQUEST = 'S', CHANGE_MODE = 'C', RECEIVE_STATUS = 'R',RECEIVE_KALMAN = 'K'};
00015 enum MODE {STANDBY_MODE = 0, MANUAL_MODE = 1,AUTO_MODE = 2,AUTO_GPS_MODE = 3};
00016 enum STATUS {GPS_AVAIL = 0, GPS_UNAVAIL = 1,GPS_OUT_AREA = 2};
00017 
00018 union TEST_T{
00019     long a;
00020     uint8_t b[4];
00021     };
00022 
00023 union UNION_double_char{
00024     double double_value;
00025     uint8_t char_value[8];
00026     };
00027 
00028 struct vector2D{
00029     double x;
00030     double y;
00031     };
00032 
00033 class AigamozuControlPackets{
00034     
00035     //--------PUBLIC-----------//
00036     public:
00037     AigamozuControlPackets(VNH5019 agzSheild);
00038     
00039     Ticker eachModeInt;
00040     
00041     uint8_t* packetData;
00042     //Create Packet: Controller/Base -> Robot 
00043     void createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR);
00044     void createRequestCommand(uint8_t fromID,uint8_t toID);
00045     void createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode);
00046     
00047     //Create Packet: Robot -> Controller/Base 
00048     void createReceiveStatusCommand(uint8_t fromID,uint8_t toID,double latitude,double longitude,double latitudeKalman,double longitudeKalman,
00049                                     double covarLati,double covarLongi);
00050     //uint8_t* createAckPacket(uint8_t fromID,uint8_t toID);
00051     
00052     //using create packet
00053     uint8_t* getPacketData();
00054     int getPacketLength();
00055     
00056     //Change Mode: 
00057     bool changeMode(uint8_t *buf); 
00058     
00059     //Check Command Type using xbee buffer;
00060     uint8_t checkCommnadType(uint8_t* buf);
00061     
00062     //Change Speed
00063     void changeSpeed(uint8_t* buf);
00064     
00065     MODE nowMode;
00066     STATUS nowStatus;
00067     int packetLength;
00068     
00069     
00070     void reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL);
00071     void reNewBasePointKalman(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL);
00072     void reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL);
00073     void reNewBasePoint(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL);
00074     
00075     double get_agzPoint_lati();
00076     double get_agzPoint_longi();
00077     double get_agzPointKalman_lati();
00078     double get_agzPointKalman_longi();
00079     double get_agzCov_lati();
00080     double get_agzCov_longi();
00081         
00082     void set_agzCov(double cov_lati,double cov_longi);
00083     //--------PRIVATE-----------//
00084     private:
00085     //
00086     VNH5019 _agzSheild;
00087     
00088     void manualMode();
00089     int manualCount;
00090     
00091     //Auto Type 1 -> Random
00092     void randomAuto();
00093     int randomCount;
00094     
00095     //Auto Type 2 -> GPS 
00096     void gpsAuto();
00097     
00098     vector2D agzPoint;//自分自身の位置の情報（double型）
00099     vector2D agzPointKalman;
00100     vector2D agzCov;//カルマンフィルタの共分散
00101     vector2D sub_vector( const vector2D& a, const vector2D& b );
00102     bool checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P);
00103     
00104     vector2D basePoint[4];//ベースの位置の情報（double型）
00105     vector2D basePointKalman[4];
00106     
00107     };
00108     
00109 #endif