diff --git a/atrvjr_drivers/rflex/include/rflex/rflex-info.h b/atrvjr_drivers/rflex/include/rflex/rflex-info.h new file mode 100644 index 0000000..c1f13c4 --- /dev/null +++ b/atrvjr_drivers/rflex/include/rflex/rflex-info.h @@ -0,0 +1,103 @@ +// Original file from player's rflex driver +#ifndef RFLEX_INFO_H1 +#define RFLEX_INFO_H1 + +#ifndef TRUE +#define TRUE 1 +#endif + +#ifndef FALSE +#define FALSE 0 +#endif + +#define MAX_NAME_LENGTH 256 +#define MAX_COMMAND_LENGTH 256 +#define MAX_ACMD_SIZE 48 +#define MAX_NUM_LOOPS 30 +#define MAX_BUFFER_LENGTH 4096 + +#define EPSILON 0.0001 + +#define TIMEOUT -1 +#define WRONG 0 +#define OK 1 + +#define B_STX 0x02 +#define B_ETX 0x03 +#define B_ESC 0x1b + +#define STD_TRANS_TORQUE 30000 +#define STD_ROT_ACC 100000 +#define STD_ROT_TORQUE 35000 + +#define SYS_PORT 1 +#define MOT_PORT 2 +#define JSTK_PORT 3 +#define SONAR_PORT 4 +#define DIO_PORT 5 +#define IR_PORT 6 + +#define SYS_LCD_DUMP 0 +#define SYS_STATUS 1 + +#define MOT_AXIS_GET_SYSTEM 0 +#define MOT_AXIS_GET_MODEL 1 +#define MOT_AXIS_GET_TARGET 2 +#define MOT_AXIS_SET_LIMITS 3 +#define MOT_AXIS_GET_LIMITS 4 +#define MOT_AXIS_SET_POS_LIMITS 5 +#define MOT_AXIS_GET_POS_LIMITS 6 +#define MOT_AXIS_SET_DIR 7 +#define MOT_AXIS_SET_POS 8 +#define MOT_AXIS_GET_MODE 9 +#define MOT_SET_DEFAULTS 10 +#define MOT_BRAKE_SET 11 +#define MOT_BRAKE_RELEASE 12 +#define MOT_SYSTEM_REPORT 33 +#define MOT_SYSTEM_REPORT_REQ 34 +#define MOT_GET_NAXES 65 +#define MOT_SET_GEARING 66 +#define MOT_GET_GEARING 67 +#define MOT_MOTOR_SET_MODE 68 +#define MOT_MOTOR_GET_MODE 69 +#define MOT_MOTOR_SET_PARMS 70 +#define MOT_MOTOR_GET_PARMS 71 +#define MOT_MOTOR_SET_LIMITS 72 +#define MOT_MOTOR_GET_LIMITS 73 +#define MOT_MOTOR_GET_DATA 74 +#define MOT_AXIS_SET_PARMS 75 +#define MOT_AXIS_GET_PARMS 76 +#define MOT_AXIS_SET_PWM_LIMIT 77 +#define MOT_AXIS_GET_PWM_LIMIT 78 +#define MOT_AXIS_SET_PWM 79 +#define MOT_AXIS_GET_PWM 80 + +#define SONAR_RUN 0 +#define SONAR_GET_UPDATE 1 +#define SONAR_REPORT 2 + +#define DIO_REPORTS_REQ 0 +#define DIO_REPORT 1 +#define DIO_GET_UPDATE 2 +#define DIO_UPDATE 3 +#define DIO_SET 4 + +#define IR_RUN 0 +#define IR_REPORT 1 + +#define JSTK_GET_STATE 0 + +enum PARITY_TYPE { N, E, O }; + +typedef struct { + char ttyport[MAX_NAME_LENGTH]; + int baud; + enum PARITY_TYPE parity; + int fd; + int databits; + int stopbits; + int hwf; + int swf; +} RFLEX_Device; + +#endif diff --git a/atrvjr_drivers/rflex/include/rflex/rflex-player.hh b/atrvjr_drivers/rflex/include/rflex/rflex-player.hh new file mode 100644 index 0000000..c6ae983 --- /dev/null +++ b/atrvjr_drivers/rflex/include/rflex/rflex-player.hh @@ -0,0 +1,177 @@ +/* + * Functions ripped straight from player's rflex driver to fix the issue + * with port initialization. + */ + +#ifndef RFLEX_PLAYER_HH_ +#define RFLEX_PLAYER_HH_ + +#include +#include +#include + +// From rflex-io.cc +int iParity(enum PARITY_TYPE par) { + if (par == N) + return (IGNPAR); + else + return (INPCK); +} + +int iSoftControl(int flowcontrol) { + if (flowcontrol) + return (IXON); + else + return (IXOFF); +} + +int cDataSize(int numbits) { + switch (numbits) { + case 5: + return (CS5); + break; + case 6: + return (CS6); + break; + case 7: + return (CS7); + break; + case 8: + return (CS8); + break; + default: + return (CS8); + break; + } +} + +int cStopSize(int numbits) { + if (numbits == 2) { + return (CSTOPB); + } else { + return (0); + } +} + +int cFlowControl(int flowcontrol) { + if (flowcontrol) { + return (CRTSCTS); + } else { + return (CLOCAL); + } +} + +int cParity(enum PARITY_TYPE par) { + if (par != N) { + if (par == O) { + return (PARENB | PARODD); + } else { + return (PARENB); + } + } else { + return (0); + } +} + +int cBaudrate(int baudrate) { + switch (baudrate) { + case 0: + return (B0); + break; + case 300: + return (B300); + break; + case 600: + return (B600); + break; + case 1200: + return (B1200); + break; + case 2400: + return (B2400); + break; + case 4800: + return (B4800); + break; + case 9600: + return (B9600); + break; + case 19200: + return (B19200); + break; + case 38400: + return (B38400); + break; + case 57600: + return (B57600); + break; + case 115200: + return (B115200); + break; +#ifdef B230400 + case 230400: + return (B230400); + break; +#endif + + default: + return (B9600); + break; + } + +} + +void DEVICE_set_params(RFLEX_Device dev) { + struct termios ctio; + + tcgetattr(dev.fd, &ctio); /* save current port settings */ + + ctio.c_iflag = iSoftControl(dev.swf) | iParity(dev.parity); + ctio.c_oflag = 0; + ctio.c_cflag = CREAD | cFlowControl(dev.hwf || dev.swf) + | cParity(dev.parity) | cDataSize(dev.databits) + | cStopSize(dev.stopbits); + + ctio.c_lflag = 0; + ctio.c_cc[VTIME] = 0; /* inter-character timer unused */ + ctio.c_cc[VMIN] = 0; /* blocking read until 0 chars received */ + + cfsetispeed(&ctio, (speed_t) cBaudrate(dev.baud)); + cfsetospeed(&ctio, (speed_t) cBaudrate(dev.baud)); + + tcflush(dev.fd, TCIFLUSH); + tcsetattr(dev.fd, TCSANOW, &ctio); + +} + +int DEVICE_connect_port(RFLEX_Device *dev) { + if ((dev->fd = open((dev->ttyport), (O_RDWR | O_NOCTTY), 0)) < 0) { + return (-1); + } + DEVICE_set_params(*dev); + return (dev->fd); +} +// /rflex-io.h + +int rflex_open_connection(const char *device_name, int *fd) { + RFLEX_Device rdev; + + strncpy(rdev.ttyport, device_name, MAX_NAME_LENGTH); + rdev.baud = 115200; + rdev.databits = 8; + rdev.parity = N; + rdev.stopbits = 1; + rdev.hwf = 0; + rdev.swf = 0; + + printf("trying port %s\n", rdev.ttyport); + if (DEVICE_connect_port(&rdev) < 0) { + fprintf(stderr, "Can't open device %s\n", rdev.ttyport); + return -1; + } + + *fd = rdev.fd; + return 0; +} + +#endif /* RFLEX_PLAYER_HH_ */ diff --git a/atrvjr_drivers/rflex/src/rflex_driver.cc b/atrvjr_drivers/rflex/src/rflex_driver.cc index 6a9d098..e1bde75 100644 --- a/atrvjr_drivers/rflex/src/rflex_driver.cc +++ b/atrvjr_drivers/rflex/src/rflex_driver.cc @@ -34,6 +34,7 @@ #include #include // memcpy #include +#include //finds the sign of a value static long sgn( long val ) { @@ -85,8 +86,12 @@ RFLEX::RFLEX() { int RFLEX::initialize(const char* device_name) { // Open the port - fd = open(device_name, O_RDWR | O_NONBLOCK); - if (fd == -1) { + + // rflex_open_connection does initialization and the code below duplicates + // it. This is not nice and needs to be cleaned when possible. + // The function was copied from original player driver to fix control lag + // issue we had on ATRVjr. + if (rflex_open_connection(device_name, &fd) < 0) { fprintf(stderr,"Could not open serial port %s\n", device_name ); return -1; }