20150512 Nakazawa
Embed:
(wiki syntax)
Show/hide line numbers
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 double get_basePointKalman_lati(int); 00082 double get_basePointKalman_longi(int); 00083 00084 void set_agzAutoGPS(); 00085 void set_agzKalmanGPS(); 00086 00087 void set_agzCov(double cov_lati,double cov_longi); 00088 00089 //Auto Type 2 -> GPS 00090 bool gpsAuto(); 00091 //--------PRIVATE-----------// 00092 private: 00093 // 00094 VNH5019 _agzSheild; 00095 00096 void manualMode(); 00097 int manualCount; 00098 00099 //Auto Type 1 -> Random 00100 void randomAuto(); 00101 int randomCount; 00102 00103 vector2D agzPoint;//自分自身の位置の情報(double型) 00104 vector2D agzPointKalman; 00105 vector2D agzCov;//カルマンフィルタの共分散 00106 vector2D sub_vector( const vector2D& a, const vector2D& b ); 00107 bool checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P); 00108 00109 vector2D basePoint[4];//ベースの位置の情報(double型) 00110 vector2D basePointKalman[4]; 00111 00112 }; 00113 00114 #endif
Generated on Wed Jul 13 2022 04:31:34 by 1.7.2