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