aigamozu / AigamozuControlPackets_2016

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 aigamozu

AigamozuControlPackets.h@48:ee5a6906273e, 2016-06-11 (annotated)

Committer:
s1200058
Date:
Sat Jun 11 03:40:16 2016 +0000
Revision:
48:ee5a6906273e
Parent:
41:2ec22c53aa26
Child:
50:3511be172d81
change turning time

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