Haruna Nakazawa / AigamozuControlPackets

20150512 Nakazawa

Dependencies:   VNH5019

AigamozuControlPackets.h@1:6d7c854c927d, 2015-05-12 (annotated)

Committer:
s1210160
Date:
Tue May 12 10:50:56 2015 +0000
Revision:
1:6d7c854c927d
Parent:
0:806e9a434e81 
20150512 Nakazawa

Who changed what in which revision?

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