Haruna Nakazawa / AigamozuControlPackets_Nakazawa

20150512 Nakazawa

Dependencies:   VNH5019

Fork of AigamozuControlPackets by aigamozu

AigamozuControlPackets.h@18:fa82a475ab95, 2015-05-12 (annotated)

Committer:
s1210160
Date:
Tue May 12 10:19:32 2015 +0000
Revision:
18:fa82a475ab95
Parent:
17:a6fa8cc96d94 
20150512 Nakazawa

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 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