2016_05_19ver Auto mode 10sec forward, 2sec stop, 2sec right turn Please change test_mode's right turn ppm
Dependencies: VNH5019 AigamozuControlPackets_2016
Dependents: Aigamozu_Robot_2016_ver1 GPSLOG_program AigamozuControlPackets_2016
Fork of AigamozuControlPackets by
Diff: AigamozuControlPackets.h
- Revision:
- 50:3511be172d81
- Parent:
- 48:ee5a6906273e
diff -r ee5a6906273e -r 3511be172d81 AigamozuControlPackets.h --- 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