forward
Dependencies: VNH5019
Fork of AigamozuControlPackets by
AigamozuControlPackets.h@17:a6fa8cc96d94, 2015-05-12 (annotated)
- Committer:
- kityann
- Date:
- Tue May 12 01:02:47 2015 +0000
- Revision:
- 17:a6fa8cc96d94
- Parent:
- 13:a5bc425540a7
- Child:
- 19:13b24b50800e
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m5171135 | 0:2b8b56ac7a82 | 1 | #include "mbed.h" |
m5171135 | 0:2b8b56ac7a82 | 2 | #include "VNH5019.h" |
m5171135 | 0:2b8b56ac7a82 | 3 | |
m5171135 | 0:2b8b56ac7a82 | 4 | #ifndef AIGAMOZU_CONTROL_PACKETS |
m5171135 | 0:2b8b56ac7a82 | 5 | #define AIGAMOZU_CONTROL_PACKETS |
m5171135 | 0:2b8b56ac7a82 | 6 | |
m5171135 | 2:3f2d4f53ceed | 7 | #define MANUAL_COMMAND_LENGTH 17 |
m5171135 | 2:3f2d4f53ceed | 8 | #define REQUEST_COMMNAD_LENGTH 11 |
m5171135 | 2:3f2d4f53ceed | 9 | #define CHANGE_MODE_COMMAND_LENGTH 12 |
kityann | 17:a6fa8cc96d94 | 10 | #define RECEIVE_STATUS_COMMNAD_LENGTH 64 |
m5171135 | 0:2b8b56ac7a82 | 11 | |
m5171135 | 7:200ce5c1f486 | 12 | #define vertex2D vector2D |
m5171135 | 7:200ce5c1f486 | 13 | |
kityann | 13:a5bc425540a7 | 14 | enum COMMAND_TYPE {MANUAL = 'M', STATUS_REQUEST = 'S', CHANGE_MODE = 'C', RECEIVE_STATUS = 'R',RECEIVE_KALMAN = 'K'}; |
m5171135 | 4:04dadf67ecb6 | 15 | enum MODE {STANDBY_MODE = 0, MANUAL_MODE = 1,AUTO_MODE = 2,AUTO_GPS_MODE = 3}; |
m5171135 | 6:f164a716be46 | 16 | enum STATUS {GPS_AVAIL = 0, GPS_UNAVAIL = 1,GPS_OUT_AREA = 2}; |
m5171135 | 0:2b8b56ac7a82 | 17 | |
m5171135 | 0:2b8b56ac7a82 | 18 | union TEST_T{ |
m5171135 | 0:2b8b56ac7a82 | 19 | long a; |
m5171135 | 0:2b8b56ac7a82 | 20 | uint8_t b[4]; |
m5171135 | 0:2b8b56ac7a82 | 21 | }; |
m5171135 | 7:200ce5c1f486 | 22 | |
kityann | 17:a6fa8cc96d94 | 23 | union UNION_double_char{ |
kityann | 17:a6fa8cc96d94 | 24 | double double_value; |
kityann | 17:a6fa8cc96d94 | 25 | uint8_t char_value[8]; |
kityann | 17:a6fa8cc96d94 | 26 | }; |
kityann | 17:a6fa8cc96d94 | 27 | |
m5171135 | 7:200ce5c1f486 | 28 | struct vector2D{ |
m5171135 | 7:200ce5c1f486 | 29 | double x; |
m5171135 | 7:200ce5c1f486 | 30 | double y; |
m5171135 | 7:200ce5c1f486 | 31 | }; |
m5171135 | 7:200ce5c1f486 | 32 | |
m5171135 | 0:2b8b56ac7a82 | 33 | class AigamozuControlPackets{ |
m5171135 | 0:2b8b56ac7a82 | 34 | |
m5171135 | 6:f164a716be46 | 35 | //--------PUBLIC-----------// |
m5171135 | 0:2b8b56ac7a82 | 36 | public: |
m5171135 | 5:3f51eeb5aedc | 37 | AigamozuControlPackets(VNH5019 agzSheild); |
m5171135 | 5:3f51eeb5aedc | 38 | |
m5171135 | 6:f164a716be46 | 39 | Ticker eachModeInt; |
m5171135 | 0:2b8b56ac7a82 | 40 | |
m5171135 | 2:3f2d4f53ceed | 41 | uint8_t* packetData; |
m5171135 | 2:3f2d4f53ceed | 42 | //Create Packet: Controller/Base -> Robot |
m5171135 | 2:3f2d4f53ceed | 43 | void createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR); |
s1200058 | 9:4f675487f06b | 44 | void createRequestCommand(uint8_t fromID,uint8_t toID); |
m5171135 | 2:3f2d4f53ceed | 45 | void createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode); |
m5171135 | 0:2b8b56ac7a82 | 46 | |
m5171135 | 2:3f2d4f53ceed | 47 | //Create Packet: Robot -> Controller/Base |
kityann | 17:a6fa8cc96d94 | 48 | void createReceiveStatusCommand(uint8_t fromID,uint8_t toID,double latitude,double longitude,double latitudeKalman,double longitudeKalman, |
kityann | 17:a6fa8cc96d94 | 49 | double covarLati,double covarLongi); |
m5171135 | 2:3f2d4f53ceed | 50 | //uint8_t* createAckPacket(uint8_t fromID,uint8_t toID); |
m5171135 | 0:2b8b56ac7a82 | 51 | |
m5171135 | 2:3f2d4f53ceed | 52 | //using create packet |
m5171135 | 2:3f2d4f53ceed | 53 | uint8_t* getPacketData(); |
m5171135 | 2:3f2d4f53ceed | 54 | int getPacketLength(); |
m5171135 | 2:3f2d4f53ceed | 55 | |
m5171135 | 2:3f2d4f53ceed | 56 | //Change Mode: |
m5171135 | 2:3f2d4f53ceed | 57 | bool changeMode(uint8_t *buf); |
m5171135 | 5:3f51eeb5aedc | 58 | |
m5171135 | 5:3f51eeb5aedc | 59 | //Check Command Type using xbee buffer; |
m5171135 | 0:2b8b56ac7a82 | 60 | uint8_t checkCommnadType(uint8_t* buf); |
m5171135 | 0:2b8b56ac7a82 | 61 | |
s1200058 | 9:4f675487f06b | 62 | //Change Speed |
m5171135 | 5:3f51eeb5aedc | 63 | void changeSpeed(uint8_t* buf); |
m5171135 | 5:3f51eeb5aedc | 64 | |
m5171135 | 0:2b8b56ac7a82 | 65 | MODE nowMode; |
m5171135 | 0:2b8b56ac7a82 | 66 | STATUS nowStatus; |
m5171135 | 4:04dadf67ecb6 | 67 | int packetLength; |
m5171135 | 4:04dadf67ecb6 | 68 | |
m5171135 | 4:04dadf67ecb6 | 69 | |
kityann | 13:a5bc425540a7 | 70 | void reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL); |
kityann | 13:a5bc425540a7 | 71 | void reNewBasePointKalman(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL); |
kityann | 11:4d71c9cc3b4a | 72 | void reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL); |
kityann | 11:4d71c9cc3b4a | 73 | void reNewBasePoint(int id, long latitudeH,long latitudeL,long longitudeH,long longitudeL); |
kityann | 17:a6fa8cc96d94 | 74 | |
kityann | 17:a6fa8cc96d94 | 75 | double get_agzPoint_lati(); |
kityann | 17:a6fa8cc96d94 | 76 | double get_agzPoint_longi(); |
kityann | 17:a6fa8cc96d94 | 77 | double get_agzPointKalman_lati(); |
kityann | 17:a6fa8cc96d94 | 78 | double get_agzPointKalman_longi(); |
kityann | 17:a6fa8cc96d94 | 79 | double get_agzCov_lati(); |
kityann | 17:a6fa8cc96d94 | 80 | double get_agzCov_longi(); |
kityann | 13:a5bc425540a7 | 81 | |
kityann | 17:a6fa8cc96d94 | 82 | void set_agzCov(double cov_lati,double cov_longi); |
m5171135 | 6:f164a716be46 | 83 | //--------PRIVATE-----------// |
m5171135 | 0:2b8b56ac7a82 | 84 | private: |
m5171135 | 5:3f51eeb5aedc | 85 | // |
m5171135 | 5:3f51eeb5aedc | 86 | VNH5019 _agzSheild; |
m5171135 | 5:3f51eeb5aedc | 87 | |
m5171135 | 5:3f51eeb5aedc | 88 | void manualMode(); |
m5171135 | 5:3f51eeb5aedc | 89 | int manualCount; |
m5171135 | 5:3f51eeb5aedc | 90 | |
m5171135 | 5:3f51eeb5aedc | 91 | //Auto Type 1 -> Random |
m5171135 | 5:3f51eeb5aedc | 92 | void randomAuto(); |
m5171135 | 5:3f51eeb5aedc | 93 | int randomCount; |
m5171135 | 5:3f51eeb5aedc | 94 | |
m5171135 | 5:3f51eeb5aedc | 95 | //Auto Type 2 -> GPS |
m5171135 | 5:3f51eeb5aedc | 96 | void gpsAuto(); |
m5171135 | 7:200ce5c1f486 | 97 | |
kityann | 17:a6fa8cc96d94 | 98 | vector2D agzPoint;//自分自身の位置の情報(double型) |
kityann | 13:a5bc425540a7 | 99 | vector2D agzPointKalman; |
kityann | 17:a6fa8cc96d94 | 100 | vector2D agzCov;//カルマンフィルタの共分散 |
m5171135 | 7:200ce5c1f486 | 101 | vector2D sub_vector( const vector2D& a, const vector2D& b ); |
m5171135 | 7:200ce5c1f486 | 102 | bool checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P); |
m5171135 | 7:200ce5c1f486 | 103 | |
kityann | 17:a6fa8cc96d94 | 104 | vector2D basePoint[4];//ベースの位置の情報(double型) |
kityann | 13:a5bc425540a7 | 105 | vector2D basePointKalman[4]; |
m5171135 | 7:200ce5c1f486 | 106 | |
m5171135 | 0:2b8b56ac7a82 | 107 | }; |
m5171135 | 0:2b8b56ac7a82 | 108 | |
m5171135 | 0:2b8b56ac7a82 | 109 | #endif |