aigamozu / AigamozuControlPackets

to Mineta

Dependencies:   VNH5019

Dependents:   agz_base_ver2 agz_base_ver2 get_GPS_data_ver1 aigamozu_program_ver2 ... more

AigamozuControlPackets.h@26:195c054a0148, 2015-05-15 (annotated)

Committer:
s1200058
Date:
Fri May 15 09:23:45 2015 +0000
Revision:
26:195c054a0148
Parent:
25:119fd6c9ae80 
2015/05/15

Who changed what in which revision?

UserRevisionLine numberNew 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 19:13b24b50800e 14 #define BASENUMBER 10
kityann 19:13b24b50800e 15
kityann 13:a5bc425540a7 16 enum COMMAND_TYPE {MANUAL = 'M', STATUS_REQUEST = 'S', CHANGE_MODE = 'C', RECEIVE_STATUS = 'R',RECEIVE_KALMAN = 'K'};
m5171135 4:04dadf67ecb6 17 enum MODE {STANDBY_MODE = 0, MANUAL_MODE = 1,AUTO_MODE = 2,AUTO_GPS_MODE = 3};
m5171135 6:f164a716be46 18 enum STATUS {GPS_AVAIL = 0, GPS_UNAVAIL = 1,GPS_OUT_AREA = 2};
m5171135 0:2b8b56ac7a82 19
m5171135 0:2b8b56ac7a82 20 union TEST_T{
m5171135 0:2b8b56ac7a82 21 long a;
m5171135 0:2b8b56ac7a82 22 uint8_t b[4];
m5171135 0:2b8b56ac7a82 23 };
m5171135 7:200ce5c1f486 24
kityann 17:a6fa8cc96d94 25 union UNION_double_char{
kityann 17:a6fa8cc96d94 26 double double_value;
kityann 17:a6fa8cc96d94 27 uint8_t char_value[8];
kityann 17:a6fa8cc96d94 28 };
s1200058 26:195c054a0148 29
s1200058 26:195c054a0148 30 union UNION_int_char{
s1200058 26:195c054a0148 31 int int_value;
s1200058 26:195c054a0148 32 uint8_t char_value[8];
s1200058 26:195c054a0148 33 };
kityann 17:a6fa8cc96d94 34
m5171135 7:200ce5c1f486 35 struct vector2D{
m5171135 7:200ce5c1f486 36 double x;
m5171135 7:200ce5c1f486 37 double y;
m5171135 7:200ce5c1f486 38 };
m5171135 7:200ce5c1f486 39
m5171135 0:2b8b56ac7a82 40 class AigamozuControlPackets{
m5171135 0:2b8b56ac7a82 41
m5171135 6:f164a716be46 42 //--------PUBLIC-----------//
m5171135 0:2b8b56ac7a82 43 public:
m5171135 5:3f51eeb5aedc 44 AigamozuControlPackets(VNH5019 agzSheild);
m5171135 5:3f51eeb5aedc 45
m5171135 6:f164a716be46 46 Ticker eachModeInt;
m5171135 0:2b8b56ac7a82 47
m5171135 2:3f2d4f53ceed 48 uint8_t* packetData;
m5171135 2:3f2d4f53ceed 49 //Create Packet: Controller/Base -> Robot
m5171135 2:3f2d4f53ceed 50 void createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR);
s1200058 9:4f675487f06b 51 void createRequestCommand(uint8_t fromID,uint8_t toID);
m5171135 2:3f2d4f53ceed 52 void createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode);
m5171135 0:2b8b56ac7a82 53
m5171135 2:3f2d4f53ceed 54 //Create Packet: Robot -> Controller/Base
kityann 17:a6fa8cc96d94 55 void createReceiveStatusCommand(uint8_t fromID,uint8_t toID,double latitude,double longitude,double latitudeKalman,double longitudeKalman,
kityann 17:a6fa8cc96d94 56 double covarLati,double covarLongi);
m5171135 2:3f2d4f53ceed 57 //uint8_t* createAckPacket(uint8_t fromID,uint8_t toID);
m5171135 0:2b8b56ac7a82 58
m5171135 2:3f2d4f53ceed 59 //using create packet
m5171135 2:3f2d4f53ceed 60 uint8_t* getPacketData();
m5171135 2:3f2d4f53ceed 61 int getPacketLength();
m5171135 2:3f2d4f53ceed 62
m5171135 2:3f2d4f53ceed 63 //Change Mode:
m5171135 2:3f2d4f53ceed 64 bool changeMode(uint8_t *buf);
m5171135 5:3f51eeb5aedc 65
m5171135 5:3f51eeb5aedc 66 //Check Command Type using xbee buffer;
m5171135 0:2b8b56ac7a82 67 uint8_t checkCommnadType(uint8_t* buf);
m5171135 0:2b8b56ac7a82 68
s1200058 9:4f675487f06b 69 //Change Speed
m5171135 5:3f51eeb5aedc 70 void changeSpeed(uint8_t* buf);
m5171135 5:3f51eeb5aedc 71
m5171135 0:2b8b56ac7a82 72 MODE nowMode;
m5171135 0:2b8b56ac7a82 73 STATUS nowStatus;
m5171135 4:04dadf67ecb6 74 int packetLength;
m5171135 4:04dadf67ecb6 75
m5171135 4:04dadf67ecb6 76
kityann 13:a5bc425540a7 77 void reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL);
kityann 19:13b24b50800e 78 void reNewBasePointKalman(int id, uint8_t *latitude,uint8_t *longitude);
kityann 11:4d71c9cc3b4a 79 void reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL);
kityann 19:13b24b50800e 80 void reNewBasePoint(int id, uint8_t *latitude,uint8_t *longitude);
kityann 17:a6fa8cc96d94 81
kityann 17:a6fa8cc96d94 82 double get_agzPoint_lati();
kityann 17:a6fa8cc96d94 83 double get_agzPoint_longi();
kityann 17:a6fa8cc96d94 84 double get_agzPointKalman_lati();
kityann 17:a6fa8cc96d94 85 double get_agzPointKalman_longi();
kityann 17:a6fa8cc96d94 86 double get_agzCov_lati();
kityann 17:a6fa8cc96d94 87 double get_agzCov_longi();
kityann 19:13b24b50800e 88 double get_basePoint_lati(int);
kityann 19:13b24b50800e 89 double get_basePoint_longi(int);
kityann 19:13b24b50800e 90 double get_basePointKalman_lati(int);
kityann 19:13b24b50800e 91 double get_basePointKalman_longi(int);
s1200058 26:195c054a0148 92 int get_base_status(int);
kityann 19:13b24b50800e 93
s1200058 20:fec2d6dec897 94 void set_agzAutoGPS();
s1200058 20:fec2d6dec897 95 void set_agzKalmanGPS();
s1200058 20:fec2d6dec897 96
kityann 17:a6fa8cc96d94 97 void set_agzCov(double cov_lati,double cov_longi);
s1200058 25:119fd6c9ae80 98 void set_base_status(int, uint8_t *);
s1200058 20:fec2d6dec897 99
s1200058 20:fec2d6dec897 100 //Auto Type 2 -> GPS
s1200058 20:fec2d6dec897 101 bool gpsAuto();
m5171135 6:f164a716be46 102 //--------PRIVATE-----------//
m5171135 0:2b8b56ac7a82 103 private:
m5171135 5:3f51eeb5aedc 104 //
m5171135 5:3f51eeb5aedc 105 VNH5019 _agzSheild;
m5171135 5:3f51eeb5aedc 106
m5171135 5:3f51eeb5aedc 107 void manualMode();
m5171135 5:3f51eeb5aedc 108 int manualCount;
m5171135 5:3f51eeb5aedc 109
m5171135 5:3f51eeb5aedc 110 //Auto Type 1 -> Random
m5171135 5:3f51eeb5aedc 111 void randomAuto();
s1200058 20:fec2d6dec897 112 int randomCount;
m5171135 7:200ce5c1f486 113
kityann 17:a6fa8cc96d94 114 vector2D agzPoint;//自分自身の位置の情報（double型）
kityann 13:a5bc425540a7 115 vector2D agzPointKalman;
kityann 17:a6fa8cc96d94 116 vector2D agzCov;//カルマンフィルタの共分散
m5171135 7:200ce5c1f486 117 vector2D sub_vector( const vector2D& a, const vector2D& b );
m5171135 7:200ce5c1f486 118 bool checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P);
m5171135 7:200ce5c1f486 119
kityann 19:13b24b50800e 120 vector2D basePoint[BASENUMBER];//ベースの位置の情報（double型）
kityann 19:13b24b50800e 121 vector2D basePointKalman[BASENUMBER];
m5171135 7:200ce5c1f486 122
s1200058 26:195c054a0148 123 int base_status[12];
s1200058 25:119fd6c9ae80 124
m5171135 0:2b8b56ac7a82 125 };
m5171135 0:2b8b56ac7a82 126
m5171135 0:2b8b56ac7a82 127 #endif