File content as of revision 19:13b24b50800e:

#include "mbed.h"
#include "VNH5019.h"

#ifndef AIGAMOZU_CONTROL_PACKETS
#define AIGAMOZU_CONTROL_PACKETS

#define MANUAL_COMMAND_LENGTH 17
#define REQUEST_COMMNAD_LENGTH 11
#define CHANGE_MODE_COMMAND_LENGTH 12
#define RECEIVE_STATUS_COMMNAD_LENGTH 64

#define vertex2D vector2D

#define BASENUMBER 10

enum COMMAND_TYPE {MANUAL = 'M', STATUS_REQUEST = 'S', CHANGE_MODE = 'C', RECEIVE_STATUS = 'R',RECEIVE_KALMAN = 'K'};
enum MODE {STANDBY_MODE = 0, MANUAL_MODE = 1,AUTO_MODE = 2,AUTO_GPS_MODE = 3};
enum STATUS {GPS_AVAIL = 0, GPS_UNAVAIL = 1,GPS_OUT_AREA = 2};

union TEST_T{
    long a;
    uint8_t b[4];
    };

union UNION_double_char{
    double double_value;
    uint8_t char_value[8];
    };

struct vector2D{
    double x;
    double y;
    };

class AigamozuControlPackets{
    
    //--------PUBLIC-----------//
    public:
    AigamozuControlPackets(VNH5019 agzSheild);
    
    Ticker eachModeInt;
    
    uint8_t* packetData;
    //Create Packet: Controller/Base -> Robot 
    void createManualCommad(uint8_t fromID,uint8_t toID,uint8_t directionL,uint8_t pwmL,uint8_t directionR, uint8_t pwmR);
    void createRequestCommand(uint8_t fromID,uint8_t toID);
    void createChangeModeCommand(uint8_t fromID,uint8_t toID,uint8_t,MODE mode);
    
    //Create Packet: Robot -> Controller/Base 
    void createReceiveStatusCommand(uint8_t fromID,uint8_t toID,double latitude,double longitude,double latitudeKalman,double longitudeKalman,
                                    double covarLati,double covarLongi);
    //uint8_t* createAckPacket(uint8_t fromID,uint8_t toID);
    
    //using create packet
    uint8_t* getPacketData();
    int getPacketLength();
    
    //Change Mode: 
    bool changeMode(uint8_t *buf); 
    
    //Check Command Type using xbee buffer;
    uint8_t checkCommnadType(uint8_t* buf);
    
    //Change Speed
    void changeSpeed(uint8_t* buf);
    
    MODE nowMode;
    STATUS nowStatus;
    int packetLength;
    
    
    void reNewRobotPointKalman(long latitudeH,long latitudeL,long longitudeH,long longitudeL);
    void reNewBasePointKalman(int id, uint8_t *latitude,uint8_t *longitude);
    void reNewRobotPoint(long latitudeH,long latitudeL,long longitudeH,long longitudeL);
    void reNewBasePoint(int id, uint8_t *latitude,uint8_t *longitude);
    
    double get_agzPoint_lati();
    double get_agzPoint_longi();
    double get_agzPointKalman_lati();
    double get_agzPointKalman_longi();
    double get_agzCov_lati();
    double get_agzCov_longi();
    double get_basePoint_lati(int);
    double get_basePoint_longi(int);
    double get_basePointKalman_lati(int);
    double get_basePointKalman_longi(int);
    
    void set_agzCov(double cov_lati,double cov_longi);
    //--------PRIVATE-----------//
    private:
    //
    VNH5019 _agzSheild;
    
    void manualMode();
    int manualCount;
    
    //Auto Type 1 -> Random
    void randomAuto();
    int randomCount;
    
    //Auto Type 2 -> GPS 
    void gpsAuto();
    
    vector2D agzPoint;//自分自身の位置の情報（double型）
    vector2D agzPointKalman;
    vector2D agzCov;//カルマンフィルタの共分散
    vector2D sub_vector( const vector2D& a, const vector2D& b );
    bool checkGpsHit( vertex2D A, vertex2D B, vertex2D C, vertex2D P);
    
    vector2D basePoint[BASENUMBER];//ベースの位置の情報（double型）
    vector2D basePointKalman[BASENUMBER];
    
    };
    
#endif