Defines | Functions | Variables
epos.cpp File Reference
#include "libepos/epos.h"
Include dependency graph for epos.cpp:

Go to the source code of this file.

Defines

#define E_ANS   0x00
 EPOS code to indicate an answer frame
#define E_BIT00   0x0001
 bit code: ready to switch on
#define E_BIT01   0x0002
 bit code: switched on
#define E_BIT02   0x0004
 bit code: operation enable
#define E_BIT03   0x0008
 bit code: FAULT
#define E_BIT04   0x0010
 bit code: voltage enabled
#define E_BIT05   0x0020
 bit code: quick stop
#define E_BIT06   0x0040
 bit code: switch on disable
#define E_BIT07   0x0080
 bit code: WARNING
#define E_BIT08   0x0100
 bit code: offset current measured (?)
#define E_BIT09   0x0200
 bit code: Remote (?)
#define E_BIT10   0x0400
 bit code: Target reached
#define E_BIT11   0x0800
 bit code: NOT USED
#define E_BIT12   0x1000
 bit code: OpMode specific
#define E_BIT13   0x2000
 bit code: OpMode specific, some error
#define E_BIT14   0x4000
 bit code: refresh cycle of power stage
#define E_BIT15   0x8000
 bit code: position referenced to home position
#define E_CURRMOD   -3
 EPOS operation mode: current mode.
#define E_DIAGMOD   -4
 EPOS operation mode: diagnostics mode.
#define E_FAIL   0x46
 EPOS answer code to indicate a failure
#define E_HOMING   6
 EPOS operation mode: homing.
#define E_HWERR   0x06060000
 Error code: access failed due to an hardware error.
#define E_INTINCOMP   0x06040047
 Error code: general internal incompatibility in the device.
#define E_MASTERENCMOD   -5
 EPOS operation mode:internal.
#define E_NMTSTATE   0x0f00ffc0
 Error code: wrong NMT state.
#define E_NOACCES   0x06010000
 Error code: Unsupported access to an object.
#define E_NODEID   0x0f00fb9
 Error code: error in Node-ID.
#define E_NOERR   0x00000000
 Error code: no error.
#define E_NSERV   0x0f00ffbc
 Error code: device not in service mode.
#define E_OK   0x4f
 EPOS answer code for all fine
#define E_ONOTEX   0x06020000
 Error code: object does not exist.
#define E_OUTMEM   0x05040005
 Error code: out of memory.
#define E_PARAMINCOMP   0x06040043
 Error code: general parameter incompatibility.
#define E_PARHIGH   0x06090031
 Error code: value of parameter written is too high.
#define E_PARLOW   0x06090032
 Error code: value of parameter written is too low.
#define E_PARREL   0x06090036
 Error code: maximum value is less than minimum value.
#define E_PASSWD   0x0f00ffbe
 Error code: password incorrect.
#define E_POSMOD   -1
 EPOS operation mode: position mode.
#define E_PRAGNEX   0x06090030
 Error code: value range of parameter exeeded.
#define E_PROFPOS   1
 EPOS operation mode: profile position mode.
#define E_PROFVEL   3
 EPOS operation mode: profile velocity mode.
#define E_READONLY   0x06010002
 Error code: Attempt to write a read-only object.
#define E_RS232   0x0f00ffbf
 Error code: rs232 command illegeal.
#define E_STARTPOS_HOMING   -200000
#define E_STEPDIRECMOD   -6
 EPOS operation mode:internal.
#define E_SUBINEX   0x06090011
 Error code: subindex does not exist.
#define E_VELMOD   -2
 EPOS operation mode: velocity mode.
#define E_WRITEONLY   0x06010001
 Error code: Attempt to read a write-only object.

Functions

int bitcmp (WORD a, WORD b)
 compare two 16bit bitmasks, return 1 (true) or 0 (false)
WORD CalcFieldCRC (WORD *pDataArray, WORD numberOfWords)
 Checksum calculation; copied from EPOS Communication Guide, p.8.
int changeEPOSstate (int state)
 change EPOS state ==> firmware spec 8.1.3
int checkEPOS ()
int checkEPOSerror ()
 check global variable E_error for EPOS error code
int checkEPOSstate ()
 check EPOS status, return state according to firmware spec 8.1.1
void checkPtr (void *ptr)
 exit(-1) if ptr == NULL
int closeEPOS ()
int doHoming (int method, long int start)
 does a homing move. Give homing mode (see firmware 9.3) and start position
int InitiateSegmentedRead (WORD index, BYTE subindex)
 Read Object from EPOS memory, firmware definition 6.3.1.2.
int monitorHomingStatus ()
 as monitorStatus(), but also waits for Homing Attained' signal
int monitorStatus ()
 reads position, velocity and current and displays them in an endless loop. Returns after target position has been reached
int moveAbsolute (long int steps)
 set OpMode to ProfilePosition and make absolute movement
int moveRelative (long int steps)
 set OpMode to ProfilePosition and make relative movement
int moveVelocity (int32_t steps)
 set OpMode to ProfileVelocity and make movement
int openEPOS (char *dev)
int openTCPEPOS (char *ip, short unsigned port)
int printEPOScontrolword (WORD s)
 pretty-print Controlword
int printEPOSstate ()
 pretty-print EPOS state
int printEPOSstatusword (WORD s)
 pretty-print Statusword
int readActualCurrent (short int *val)
int readActualPosition (long *pos)
 read actual position; 14.1.62
int readActualVelocity (long *val)
 read actual position; 14.1.68
int readAnswer (WORD **ptr)
 int readAnswer(WORD **ptr) - read an answer frame from EPOS
int readBYTE (BYTE *c)
 read a single BYTE from EPOS, timeout implemented
int readControlword (WORD *w)
 read EPOS control word (firmware spec 14.1.57)
int readDemandPosition (long *pos)
 read actual position; 14.1.61
int readDemandVelocity (long *val)
 read actual position; 14.1.67
int readDeviceName (char *str)
 ask for device name, device name is placed in 'name' (string must be big enough, NO CHECKING!!)
int readDInputPolarity (WORD *w)
 read digital input polarity mask
int ReadObject (WORD index, BYTE subindex, WORD **answer)
 Read Object from EPOS memory, firmware definition 6.3.1.1.
int readOpMode ()
 read and returns EPOS mode of operation -- 14.1.60 here, RETURN(0) MEANS ERROR! '-1' is a valid OpMode, but 0 is not!
int readPositionWindow (unsigned long int *pos)
 read position window; 14.1.64
int readRS232timeout ()
 ask for RS232 timeout; firmware spec 14.1.35
int readStatusword (WORD *status)
 read Statusword; 14.1.58
int readSWversion ()
 example from EPOS com. guide: ask EPOS for software version
int readTargetPosition (long *val)
 read target position; 14.1.70
int readWORD (WORD *w)
 read a single WORD from EPOS, timeout implemented
int SegmentRead (WORD **ptr)
 int SegmentRead(WORD **ptr) - read data segment of the object initiated with 'InitiateSegmentedRead()'
int sendCom (WORD *frame)
 send command to EPOS, taking care of all neccessary 'ack' and checksum tests
int set_speed_profile (unsigned int max_velocity, unsigned int acceleration, unsigned int deceleration, bool trapezoidal)
int setHomePolarity (int pol)
 set home switch polarity -- firmware spec 14.1.47
int setOpMode (int m)
 set EPOS mode of operation -- 14.1.59
int waitForTarget (unsigned int t)
 waits for positoning to finish, argument is timeout in seconds. give timeout==0 to disable timeout
int writeBYTE (BYTE *c)
 write a single BYTE to EPOS
int WriteObject (WORD index, BYTE subindex, WORD data[2])
int WriteObject (WORD index, BYTE subindex, WORD *data)
int writePositionWindow (unsigned long int val)
 write position window; 14.1.64
int writeWORD (WORD *w)
 write a single WORD to EPOS

Variables

DWORD E_error
 EPOS global error status.
static int ep = -1
 EPOS file descriptor.
char gMarker = 0
 global; for internal handling
int sp
 serial port file descriptor

Define Documentation

#define E_ANS   0x00

EPOS code to indicate an answer frame

Definition at line 59 of file epos.cpp.

#define E_BIT00   0x0001

bit code: ready to switch on

Definition at line 109 of file epos.cpp.

#define E_BIT01   0x0002

bit code: switched on

Definition at line 108 of file epos.cpp.

#define E_BIT02   0x0004

bit code: operation enable

Definition at line 107 of file epos.cpp.

#define E_BIT03   0x0008

bit code: FAULT

Definition at line 106 of file epos.cpp.

#define E_BIT04   0x0010

bit code: voltage enabled

Definition at line 105 of file epos.cpp.

#define E_BIT05   0x0020

bit code: quick stop

Definition at line 104 of file epos.cpp.

#define E_BIT06   0x0040

bit code: switch on disable

Definition at line 103 of file epos.cpp.

#define E_BIT07   0x0080

bit code: WARNING

Definition at line 102 of file epos.cpp.

#define E_BIT08   0x0100

bit code: offset current measured (?)

Definition at line 101 of file epos.cpp.

#define E_BIT09   0x0200

bit code: Remote (?)

Definition at line 100 of file epos.cpp.

#define E_BIT10   0x0400

bit code: Target reached

Definition at line 99 of file epos.cpp.

#define E_BIT11   0x0800

bit code: NOT USED

Definition at line 98 of file epos.cpp.

#define E_BIT12   0x1000

bit code: OpMode specific

Definition at line 97 of file epos.cpp.

#define E_BIT13   0x2000

bit code: OpMode specific, some error

Definition at line 96 of file epos.cpp.

#define E_BIT14   0x4000

bit code: refresh cycle of power stage

Definition at line 95 of file epos.cpp.

#define E_BIT15   0x8000

bit code: position referenced to home position

Definition at line 94 of file epos.cpp.

#define E_CURRMOD   -3

EPOS operation mode: current mode.

Definition at line 122 of file epos.cpp.

#define E_DIAGMOD   -4

EPOS operation mode: diagnostics mode.

Definition at line 123 of file epos.cpp.

#define E_FAIL   0x46

EPOS answer code to indicate a failure

Definition at line 58 of file epos.cpp.

#define E_HOMING   6

EPOS operation mode: homing.

Definition at line 115 of file epos.cpp.

#define E_HWERR   0x06060000

Error code: access failed due to an hardware error.

Definition at line 77 of file epos.cpp.

#define E_INTINCOMP   0x06040047

Error code: general internal incompatibility in the device.

Definition at line 76 of file epos.cpp.

#define E_MASTERENCMOD   -5

EPOS operation mode:internal.

Definition at line 124 of file epos.cpp.

#define E_NMTSTATE   0x0f00ffc0

Error code: wrong NMT state.

Definition at line 85 of file epos.cpp.

#define E_NOACCES   0x06010000

Error code: Unsupported access to an object.

Definition at line 72 of file epos.cpp.

#define E_NODEID   0x0f00fb9

Error code: error in Node-ID.

Definition at line 89 of file epos.cpp.

#define E_NOERR   0x00000000

Error code: no error.

Definition at line 68 of file epos.cpp.

#define E_NSERV   0x0f00ffbc

Error code: device not in service mode.

Definition at line 88 of file epos.cpp.

#define E_OK   0x4f

EPOS answer code for all fine

Definition at line 57 of file epos.cpp.

#define E_ONOTEX   0x06020000

Error code: object does not exist.

Definition at line 69 of file epos.cpp.

#define E_OUTMEM   0x05040005

Error code: out of memory.

Definition at line 71 of file epos.cpp.

#define E_PARAMINCOMP   0x06040043

Error code: general parameter incompatibility.

Definition at line 75 of file epos.cpp.

#define E_PARHIGH   0x06090031

Error code: value of parameter written is too high.

Definition at line 79 of file epos.cpp.

#define E_PARLOW   0x06090032

Error code: value of parameter written is too low.

Definition at line 80 of file epos.cpp.

#define E_PARREL   0x06090036

Error code: maximum value is less than minimum value.

Definition at line 81 of file epos.cpp.

#define E_PASSWD   0x0f00ffbe

Error code: password incorrect.

Definition at line 87 of file epos.cpp.

#define E_POSMOD   -1

EPOS operation mode: position mode.

Definition at line 120 of file epos.cpp.

#define E_PRAGNEX   0x06090030

Error code: value range of parameter exeeded.

Definition at line 78 of file epos.cpp.

#define E_PROFPOS   1

EPOS operation mode: profile position mode.

Definition at line 117 of file epos.cpp.

#define E_PROFVEL   3

EPOS operation mode: profile velocity mode.

Definition at line 116 of file epos.cpp.

#define E_READONLY   0x06010002

Error code: Attempt to write a read-only object.

Definition at line 74 of file epos.cpp.

#define E_RS232   0x0f00ffbf

Error code: rs232 command illegeal.

Definition at line 86 of file epos.cpp.

#define E_STARTPOS_HOMING   -200000

starting point for (slow!) homing movement. If the zero point is not off too far, this will speed things up enormously!

Definition at line 50 of file epos.cpp.

#define E_STEPDIRECMOD   -6

EPOS operation mode:internal.

Definition at line 125 of file epos.cpp.

#define E_SUBINEX   0x06090011

Error code: subindex does not exist.

Definition at line 70 of file epos.cpp.

#define E_VELMOD   -2

EPOS operation mode: velocity mode.

Definition at line 121 of file epos.cpp.

#define E_WRITEONLY   0x06010001

Error code: Attempt to read a write-only object.

Definition at line 73 of file epos.cpp.


Function Documentation

int bitcmp ( WORD  a,
WORD  b 
)

compare two 16bit bitmasks, return 1 (true) or 0 (false)

Definition at line 2435 of file epos.cpp.

WORD CalcFieldCRC ( WORD pDataArray,
WORD  numberOfWords 
)

Checksum calculation; copied from EPOS Communication Guide, p.8.

Definition at line 2142 of file epos.cpp.

int changeEPOSstate ( int  state)

change EPOS state ==> firmware spec 8.1.3

Definition at line 667 of file epos.cpp.

int checkEPOS ( )

check whether the socket connection to EPOS is established

Return values:
0success
-1failure

Definition at line 365 of file epos.cpp.

int checkEPOSerror ( )

check global variable E_error for EPOS error code

Definition at line 1951 of file epos.cpp.

int checkEPOSstate ( )

check EPOS status, return state according to firmware spec 8.1.1

check EPOS state, firmware spec 8.1.1

Returns:
EPOS status as defined in firmware specification 8.1.1

Definition at line 508 of file epos.cpp.

void checkPtr ( void *  ptr)

exit(-1) if ptr == NULL

Definition at line 2443 of file epos.cpp.

int closeEPOS ( )

closes connection socket of EPOS device

Return values:
0success
-1failure

Definition at line 354 of file epos.cpp.

int doHoming ( int  method,
long int  start 
)

does a homing move. Give homing mode (see firmware 9.3) and start position

Definition at line 1468 of file epos.cpp.

int InitiateSegmentedRead ( WORD  index,
BYTE  subindex 
)

Read Object from EPOS memory, firmware definition 6.3.1.2.

NOT USED IN libEPOS so far -> untested!

Definition at line 2460 of file epos.cpp.

as monitorStatus(), but also waits for Homing Attained' signal

Definition at line 1844 of file epos.cpp.

int monitorStatus ( )

reads position, velocity and current and displays them in an endless loop. Returns after target position has been reached

Definition at line 1773 of file epos.cpp.

int moveAbsolute ( long int  steps)

set OpMode to ProfilePosition and make absolute movement

Definition at line 1652 of file epos.cpp.

int moveRelative ( long int  steps)

set OpMode to ProfilePosition and make relative movement

Definition at line 1605 of file epos.cpp.

int moveVelocity ( int32_t  steps)

set OpMode to ProfileVelocity and make movement

Definition at line 1711 of file epos.cpp.

int openEPOS ( char *  dev)

establish the connection to EPOS via RS232 connection

Parameters:
devstring describing the device on which the EPOS is connected to, e.g. "/dev/ttyS0"
Return values:
0success
-1failure

Definition at line 234 of file epos.cpp.

int openTCPEPOS ( char *  ip,
short unsigned  port 
)

establish the connection to EPOS via a TCP/IP tunneled RS232 connection

Parameters:
ipstring describing the IP address on which the EPOS is connected to, e.g. "192.168.1.100"
portshort unsigned int giving the TCP port number on the device 'IP'
Return values:
0success
-1failure

Definition at line 303 of file epos.cpp.

pretty-print Controlword

Definition at line 951 of file epos.cpp.

int printEPOSstate ( )

pretty-print EPOS state

Definition at line 639 of file epos.cpp.

pretty-print Statusword

pretty-print Statusword to stdout

Parameters:
sWORD variable holding the statusword

Definition at line 428 of file epos.cpp.

int readActualCurrent ( short int *  val)

read actual motor current, see firmware description 14.1.69

Parameters:
valpointer to short int where the actual motor current will be placed.
Return values:
0success
-1error

Definition at line 1271 of file epos.cpp.

int readActualPosition ( long *  pos)

read actual position; 14.1.62

read actual position; firmware description 14.1.62

Return values:
0success
<0some error, check with checkEPOSerror()

Definition at line 1108 of file epos.cpp.

int readActualVelocity ( long *  val)

read actual position; 14.1.68

Definition at line 1229 of file epos.cpp.

int readAnswer ( WORD **  ptr)

int readAnswer(WORD **ptr) - read an answer frame from EPOS

int readAnswer(WORD **ptr) - read an answer frame from EPOS

Parameters:
ptrWORD **ptr; pointer address where answer frame is placed.
Return values:
>0number of WORDs recieved from EPOS. ptr points now to answer frame.
<0failure; ptr points to NULL. Global E_error is also set to returnd EPOS ErrorCode

Definition at line 2252 of file epos.cpp.

int readBYTE ( BYTE c)

read a single BYTE from EPOS, timeout implemented

Definition at line 2058 of file epos.cpp.

int readControlword ( WORD w)

read EPOS control word (firmware spec 14.1.57)

Definition at line 921 of file epos.cpp.

int readDemandPosition ( long *  pos)

read actual position; 14.1.61

Definition at line 1073 of file epos.cpp.

int readDemandVelocity ( long *  val)

read actual position; 14.1.67

Definition at line 1198 of file epos.cpp.

int readDeviceName ( char *  str)

ask for device name, device name is placed in 'name' (string must be big enough, NO CHECKING!!)

readDeviceName: read manufactor device name string firmware

Parameters:
strpreviously allocated string, will be filled with device name
Return values:
0success
-1error

Definition at line 1349 of file epos.cpp.

int readDInputPolarity ( WORD w)

read digital input polarity mask

Definition at line 839 of file epos.cpp.

int ReadObject ( WORD  index,
BYTE  subindex,
WORD **  answer 
)

Read Object from EPOS memory, firmware definition 6.3.1.1.

Definition at line 2352 of file epos.cpp.

int readOpMode ( )

read and returns EPOS mode of operation -- 14.1.60 here, RETURN(0) MEANS ERROR! '-1' is a valid OpMode, but 0 is not!

read mode of operation --- 14.1.60

Returns:
RETURN(0) MEANS ERROR! -1 is a valid OpMode, but 0 is not!

Definition at line 1027 of file epos.cpp.

int readPositionWindow ( unsigned long int *  pos)

read position window; 14.1.64

Definition at line 1139 of file epos.cpp.

ask for RS232 timeout; firmware spec 14.1.35

Definition at line 1385 of file epos.cpp.

int readStatusword ( WORD status)

read Statusword; 14.1.58

read EPOS status word

Parameters:
statuspointer to WORD, the content of EPOS statusword will be placed there
Return values:
0success
-1failure

Definition at line 392 of file epos.cpp.

int readSWversion ( )

example from EPOS com. guide: ask EPOS for software version

firmware spec 14.1.33 returns software version as HEX **

Definition at line 805 of file epos.cpp.

int readTargetPosition ( long *  val)

read target position; 14.1.70

read EPOS target position; firmware description 14.1.70

Parameters:
valpointer to long int, will be filled with EPOS target position
Return values:
0success
-1error

Definition at line 1307 of file epos.cpp.

int readWORD ( WORD w)

read a single WORD from EPOS, timeout implemented

Definition at line 2101 of file epos.cpp.

int SegmentRead ( WORD **  ptr)

int SegmentRead(WORD **ptr) - read data segment of the object initiated with 'InitiateSegmentedRead()'

NOT USED IN libEPOS so far -> untested!

Return values:
>0means sucess. Number of WORDs recieved from EPOS will be returned (*ptr) points to answer frame
<0means failure: (*ptr) points to NULL

Definition at line 2491 of file epos.cpp.

int sendCom ( WORD frame)

send command to EPOS, taking care of all neccessary 'ack' and checksum tests

Definition at line 2175 of file epos.cpp.

int set_speed_profile ( unsigned int  max_velocity,
unsigned int  acceleration,
unsigned int  deceleration,
bool  trapezoidal 
)

Definition at line 1409 of file epos.cpp.

int setHomePolarity ( int  pol)

set home switch polarity -- firmware spec 14.1.47

Definition at line 874 of file epos.cpp.

int setOpMode ( int  m)

set EPOS mode of operation -- 14.1.59

Definition at line 1000 of file epos.cpp.

int waitForTarget ( unsigned int  t)

waits for positoning to finish, argument is timeout in seconds. give timeout==0 to disable timeout

Definition at line 1924 of file epos.cpp.

int writeBYTE ( BYTE c)

write a single BYTE to EPOS

Definition at line 2029 of file epos.cpp.

int WriteObject ( WORD  index,
BYTE  subindex,
WORD  data[2] 
)

6.3.2.1 WriteObject()

WORD *data is a pointer to a 2 WORDs array (== 4 BYTES) holding data to transmit

int WriteObject ( WORD  index,
BYTE  subindex,
WORD data 
)

Low-level function to write an object to EPOS memory. Is called by writing libEPOS functions.

Parameters:
indexWORD describing EPOS memory index for writing. See firmware documentation for valid values
subindexBYTE describing EPOS memory subindex for writing. See firmware documentation for valid values
datapointer to WORD array holding the data to be written to EPOS memory
Return values:
0success
-1error

Definition at line 2393 of file epos.cpp.

int writePositionWindow ( unsigned long int  val)

write position window; 14.1.64

Definition at line 1169 of file epos.cpp.

int writeWORD ( WORD w)

write a single WORD to EPOS

Definition at line 2043 of file epos.cpp.


Variable Documentation

EPOS global error status.

EPOS global error status

Definition at line 131 of file epos.cpp.

int ep = -1 [static]

EPOS file descriptor.

Definition at line 204 of file epos.cpp.

char gMarker = 0

global; for internal handling

Definition at line 205 of file epos.cpp.

int sp

serial port file descriptor

Definition at line 130 of file epos.cpp.



epos_driver
Author(s): Tomasz Kucner , Martin Magnusson , Hakan Almqvist , Marcus Hauser
autogenerated on Fri Aug 28 2015 10:38:28