nav200.h

00001 
00009 #ifndef _NAV200_H
00010 #define _NAV200_H
00011 
00012 #include <libplayercore/playercore.h>
00013 #include <sys/types.h>
00014 #include <sys/stat.h>
00015 #include <sys/time.h>
00016 #include <fcntl.h>
00017 #include <termios.h>
00018 #include <stdio.h>
00019 #include <strings.h>
00020 #include <unistd.h>
00021 #include <stdlib.h>
00022 #include <errno.h>
00023 #include <string.h>
00024 #include <pthread.h>
00025 #include <math.h>
00026 //#include <stdint.h>
00027 
00028 
00029 #define STX     0x02
00030 #define MAXLEN  255
00031 #define BUFFER_SIZE 256
00032 #define HEADER_SIZE 4
00033 #define FOOTER_SIZE 1
00034 
00035 typedef struct Nav200Command
00036 {
00037         uint8_t header;
00038         uint8_t length;
00039         uint8_t mode;
00040         uint8_t function;
00041         uint8_t data [MAXLEN-HEADER_SIZE-FOOTER_SIZE+1];
00042         int dataLength;
00043         uint8_t BCC;
00044 }Nav200Command;
00045 
00046 // typedef struct ReflectorInfo
00047 // {
00048 //   uint8_t layer;
00049 //   uint8_t number;
00050 // }ReflectorInfo;
00051 
00052 typedef struct PositionXY
00053 {//position is in mm
00054   int x;
00055   int y;
00056 }PositionXY;
00057 
00058 
00059 typedef struct ReflectorData
00060 {
00061   uint8_t layer;
00062   uint8_t number; // reflector number
00063   PositionXY pos;
00064 }ReflectorData;
00065 
00066 
00067 typedef struct LaserPos
00068 {
00069   PositionXY pos; // position of the laser scanner
00070   short orientation;
00071   uint8_t quality;
00072   uint8_t number; // number of reflectors used
00073 }LaserPos;
00074 
00075 typedef struct ErrorBytes
00076 {
00077   uint8_t F0; // function byte of the last command
00078   uint8_t F1; // error class
00079   uint8_t F2; // error group
00080   uint8_t F3; // error specification
00081 }ErrorBytes;
00082 
00083 
00084 
00085 
00086 class Nav200
00087 {
00088 public:
00089 
00090   friend class SickNAV200;
00091   Nav200();
00092   ~Nav200();
00093 
00094   int Initialise(Driver* device, Device* opaque, player_devaddr_t opaque_id);
00095   int Terminate();
00096 
00097   int ProcessData();
00098 
00099   // standby mode
00100   bool EnterStandby();
00101   int GetVersionNumber();
00102   char* GetVersionString(); //String pointer return is only valid till the next request to Nav200
00103   short GetDeviceSerial();
00104   bool rotateDirection(uint8_t direction);
00105   bool GetReflectorPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
00106   bool ChangeReflectorPosition(uint8_t layer, uint8_t number, int newX, int newY);
00107   bool InsertReflectorPosition(uint8_t layer, uint8_t number, int X, int Y);
00108   bool DeleteReflectorPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
00109 
00110   // read and set reflector radii
00111   int GetReflectorRadius(uint8_t layer);
00112   bool SetReflectorRadius(uint8_t layer, uint8_t radius);
00113 
00114   // mapping mode
00115   bool EnterMapping();
00116   int StartMapping(uint8_t layer, int X, int Y, short orientation, uint8_t radius);
00117   int StartMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y, short orientation, uint8_t radius);
00118   int StartNegativeMappingMeasurement(uint8_t layer, uint8_t scans, int X, int Y, short orientation, uint8_t radius);
00119   bool MappingPosition(uint8_t layer, uint8_t number, PositionXY & reflector);
00120 
00121   // positioning mode
00122   bool EnterPositioning();
00123   bool EnterPositioningInput(uint8_t NumberOfMeasurements);
00124   bool GetPositionAuto(LaserPos & laserPosition);
00125   bool GetPositionSpeed(short speedX, short speedY, LaserPos & laserPosition);
00126   bool GetPositionSpeedVelocity(short speedX, short speedY, short velocity, LaserPos & laserPosition);
00127   bool GetPositionSpeedVelocityAbsolute(short speedX, short speedY, short velocity, LaserPos & laserPosition);
00128   bool ChangeLayer(uint8_t layer);
00129   bool ChangeLayerDefPosition(uint8_t layer, int X, int Y, short orientation);
00130   bool SetActionRadii(int min, int max);
00131   bool SelectNearest(uint8_t N_nearest);
00132 
00133   // upload mode
00134   bool EnterUpload();
00135   bool GetUploadTrans(uint8_t layer, ReflectorData & reflector);
00136   // download mode
00137   bool EnterDownload();
00138   bool DownloadReflector(uint8_t layer, uint8_t number, int X, int Y);
00139 
00140 
00141 protected:
00142   // serial port descriptor
00143   int fd;
00144   struct termios oldtio;
00145 
00146   uint8_t receivedBuffer[BUFFER_SIZE];
00147   int bytesReceived;
00148   Nav200Command packet;
00149   ErrorBytes error;
00150 
00151   void PrintErrorMsg(void);
00152 
00153   int ReadFromNav200(int timeout_usec=5000000);
00154   int WriteCommand(char mode, char function, int dataLength, uint8_t * data);
00155   uint8_t CreateCRC(uint8_t* data, ssize_t len);
00156   
00157   // SickNav200 Driver info
00158   Driver *sn200;
00159   
00160   // Opaque info - for setting filter
00161   Device *opaque;
00162   player_devaddr_t opaque_id;
00163   
00164 };
00165 
00166 
00167 
00168 #endif
Last updated 12 September 2005 21:38:45