00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00030 #ifndef _NAV200_H
00031 #define _NAV200_H
00032
00033 #include <libplayercore/playercore.h>
00034 #include <sys/types.h>
00035 #include <sys/stat.h>
00036 #if !defined (WIN32)
00037 #include <sys/time.h>
00038 #include <termios.h>
00039 #include <strings.h>
00040 #include <unistd.h>
00041 #endif
00042 #include <fcntl.h>
00043 #include <stdio.h>
00044 #include <stdlib.h>
00045 #include <errno.h>
00046 #include <string.h>
00047 #include <pthread.h>
00048 #include <math.h>
00049
00050
00051 #if defined (WIN32)
00052 typedef unsigned int ssize_t;
00053 #endif
00054
00055 #define STX 0x02
00056 #define MAXLEN 255
00057 #define BUFFER_SIZE 256
00058 #define HEADER_SIZE 4
00059 #define FOOTER_SIZE 1
00060
00061 typedef struct Nav200Command
00062 {
00063 uint8_t header;
00064 uint8_t length;
00065 uint8_t mode;
00066 uint8_t function;
00067 uint8_t data [MAXLEN-HEADER_SIZE-FOOTER_SIZE+1];
00068 int dataLength;
00069 uint8_t BCC;
00070 }Nav200Command;
00071
00072
00073
00074
00075
00076
00077
00078 typedef struct PositionXY
00079 {
00080 int x;
00081 int y;
00082 }PositionXY;
00083
00084
00085 typedef struct ReflectorData
00086 {
00087 uint8_t layer;
00088 uint8_t number;
00089 PositionXY pos;
00090 }ReflectorData;
00091
00092
00093 typedef struct LaserPos
00094 {
00095 PositionXY pos;
00096 short orientation;
00097 uint8_t quality;
00098 uint8_t number;
00099 }LaserPos;
00100
00101 typedef struct ErrorBytes
00102 {
00103 uint8_t F0;
00104 uint8_t F1;
00105 uint8_t F2;
00106 uint8_t F3;
00107 }ErrorBytes;
00108
00109
00110
00111
00112 class Nav200
00113 {
00114 public:
00115
00116 friend class SickNAV200;
00117 Nav200();
00118 ~Nav200();
00119
00120 int Initialise(Driver* device, Device* opaque, player_devaddr_t opaque_id);
00121 int Terminate();
00122
00123 int ProcessData();
00124
00125
00126 bool EnterStandby();
00127 int GetVersionNumber();
00128 char* GetVersionString();
00129 short GetDeviceSerial();
00130 bool rotateDirection(uint8_t direction);
00131 bool GetReflectorPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
00132 bool ChangeReflectorPosition(uint8_t layer, uint8_t number, int newX, int newY);
00133 bool InsertReflectorPosition(uint8_t layer, uint8_t number, int X, int Y);
00134 bool DeleteReflectorPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
00135
00136
00137 int GetReflectorRadius(uint8_t layer);
00138 bool SetReflectorRadius(uint8_t layer, uint8_t radius);
00139
00140
00141 bool EnterMapping();
00142 int StartMapping(uint8_t layer, int X, int Y, short orientation, uint8_t radius);
00143 int StartMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y, short orientation, uint8_t radius);
00144 int StartNegativeMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y, short orientation, uint8_t radius);
00145 bool MappingPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
00146
00147
00148 bool EnterPositioning();
00149 bool EnterPositioningInput(uint8_t NumberOfMeasurements);
00150 bool GetPositionAuto(LaserPos & laserPosition);
00151 bool GetPositionSpeed(short speedX, short speedY, LaserPos & laserPosition);
00152 bool GetPositionSpeedVelocity(short speedX, short speedY, short velocity, LaserPos & laserPosition);
00153 bool GetPositionSpeedVelocityAbsolute(short speedX, short speedY, short velocity, LaserPos & laserPosition);
00154 bool ChangeLayer(uint8_t layer);
00155 bool ChangeLayerDefPosition(uint8_t layer, int X, int Y, short orientation);
00156 bool SetActionRadii(int min, int max);
00157 bool SelectNearest(uint8_t N_nearest);
00158
00159
00160 bool EnterUpload();
00161 bool GetUploadTrans(uint8_t layer, ReflectorData & reflector);
00162
00163 bool EnterDownload();
00164 bool DownloadReflector(uint8_t layer, uint8_t number, int X, int Y);
00165
00166
00167 protected:
00168
00169
00170
00171
00172 uint8_t receivedBuffer[BUFFER_SIZE];
00173 int bytesReceived;
00174 Nav200Command packet;
00175 ErrorBytes error;
00176
00177 void PrintErrorMsg(void);
00178
00179 int ReadFromNav200(int timeout_usec=5000000);
00180 int WriteCommand(char mode, char function, int dataLength, uint8_t * data);
00181 uint8_t CreateCRC(uint8_t* data, ssize_t len);
00182
00183
00184 Driver *sn200;
00185
00186
00187 Device *opaque;
00188 player_devaddr_t opaque_id;
00189
00190 };
00191
00192
00193
00194 #endif