Revision: 8819
http://playerstage.svn.sourceforge.net/playerstage/?rev=8819&view=rev
Author: jpgr87
Date: 2010-07-08 03:21:37 +0000 (Thu, 08 Jul 2010)
Log Message:
-----------
Applied patch #2948822: hemisson driver
Modified Paths:
--------------
code/player/trunk/server/drivers/mixed/CMakeLists.txt
Added Paths:
-----------
code/player/trunk/server/drivers/mixed/hemisson/
code/player/trunk/server/drivers/mixed/hemisson/CMakeLists.txt
code/player/trunk/server/drivers/mixed/hemisson/hemisson.cc
code/player/trunk/server/drivers/mixed/hemisson/hemisson_serial.cc
code/player/trunk/server/drivers/mixed/hemisson/hemisson_serial.h
Modified: code/player/trunk/server/drivers/mixed/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/mixed/CMakeLists.txt 2010-07-08
02:56:35 UTC (rev 8818)
+++ code/player/trunk/server/drivers/mixed/CMakeLists.txt 2010-07-08
03:21:37 UTC (rev 8819)
@@ -6,6 +6,7 @@
ADD_SUBDIRECTORY (erratic)
ADD_SUBDIRECTORY (evolution)
ADD_SUBDIRECTORY (garcia)
+ADD_SUBDIRECTORY (hemisson)
ADD_SUBDIRECTORY (irobot)
ADD_SUBDIRECTORY (khepera)
ADD_SUBDIRECTORY (mricp)
Added: code/player/trunk/server/drivers/mixed/hemisson/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/mixed/hemisson/CMakeLists.txt
(rev 0)
+++ code/player/trunk/server/drivers/mixed/hemisson/CMakeLists.txt
2010-07-08 03:21:37 UTC (rev 8819)
@@ -0,0 +1,7 @@
+PLAYERDRIVER_OPTION (hemisson build_hemisson ON)
+PLAYERDRIVER_REJECT_OS (hemisson build_hemisson PLAYER_OS_WIN)
+PLAYERDRIVER_REQUIRE_HEADER (hemisson build_hemisson sys/ioctl.h sys/types.h)
+PLAYERDRIVER_REQUIRE_HEADER (hemisson build_hemisson sys/stat.h sys/types.h)
+PLAYERDRIVER_REQUIRE_HEADER (hemisson build_hemisson sys/time.h sys/types.h)
+PLAYERDRIVER_REQUIRE_HEADER (hemisson build_hemisson sys/select.h sys/types.h)
+PLAYERDRIVER_ADD_DRIVER (hemisson build_hemisson SOURCES hemisson.cc
hemisson_serial.cc)
Added: code/player/trunk/server/drivers/mixed/hemisson/hemisson.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/hemisson/hemisson.cc
(rev 0)
+++ code/player/trunk/server/drivers/mixed/hemisson/hemisson.cc 2010-07-08
03:21:37 UTC (rev 8819)
@@ -0,0 +1,481 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2000
+ * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+/* Copyright (C) 2010
+ * Paul Osmialowski
+ * Copyright (C) 2004
+ * Toby Collett, University of Auckland Robotics Group
+ */
+
+/** @ingroup drivers */
+/** @{ */
+/** @defgroup driver_hemisson hemisson
+ * @brief K-Team Hemisson mobile robot
+
+The hemisson driver is used to interface to the K-Team hemisson robot.
+
+This driver is experimental and should be treated with caution. At
+this point it supports the @ref interface_position2d and
+...@ref interface_ranger interfaces.
+
+This driver was tested with RS-232C cable link (for example /dev/ttyS0)
+and bluetooth radio link (/dev/rfcomm0 or /dev/ttyUB0).
+
+...@par Compile-time dependencies
+
+- none
+
+...@par Provides
+
+- @ref interface_position2d
+- @ref interface_ranger
+
+...@par Requires
+
+- none
+
+...@par Configuration file options
+
+- port (string)
+ - Default: "/dev/rfcomm0"
+ - Serial port used to communicate with the robot.
+- sleep_nsec (integer)
+ - Default: 100000000
+ - timespec value for nanosleep().
+- init_motor_state (integer)
+ - Default: 0
+ - Initial motor state.
+- speed_factor (float)
+ - Default: 18.0
+ - Speed scale factor.
+- aspeed_factor (float)
+ - Default: 16.0
+ - Angular speed scale factor.
+- publish_ranges (integer)
+ - Default: 8
+ - Vaild values: 1..8
+ - Number of ranger scans to publish.
+- set_stall (integer)
+ - Default: 0
+ - If set to non-zero, stall field of provided position2d interface
+ will be filled according to sensor readings.
+- stall_threshold (float)
+ - Default: 0.025
+ - Distance below this value causes robot to be stalled.
+
+Since initialization routine takes some time to do its job, it may be
+worth to set alwayson to 1, as some libplayerc client programs may
+refuse to work due to communication timeout at the startup.
+
+...@par Example
+
+...@verbatim
+driver
+(
+ name "hemisson"
+ provides ["position2d:0" "ranger:0"]
+ port "/dev/ttyS1"
+ alwayson 1
+)
+...@endverbatim
+
+...@author Paul Osmialowski based on Khepera driver by Toby Collett
+*/
+/** @} */
+
+#include "hemisson_serial.h"
+
+#include <stddef.h>
+#include <string.h>
+#include <time.h>
+#include <math.h>
+#include <assert.h>
+#include <pthread.h>
+
+#include <libplayercore/playercore.h>
+
+#define EPS 0.00000000001
+
+#define HEMISSON_RANGES 8
+#define HEMISSON_WIDTH 0.11
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+// Convert degrees to radians
+#ifndef DTOR
+#define DTOR(d) ((d) * (M_PI) / 180.0)
+#endif
+
+#ifndef RTOD
+#define RTOD(r) ((r) * 180.0 / M_PI)
+#endif
+
+class Hemisson : public ThreadedDriver
+{
+public:
+ Hemisson(ConfigFile * cf, int section);
+ virtual ~Hemisson();
+
+ virtual void Main();
+
+ virtual int MainSetup();
+ virtual void MainQuit();
+ virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr,
void * data);
+private:
+ HemissonSerial * serial;
+ int debug;
+ player_devaddr_t position2d_addr;
+ player_devaddr_t ranger_addr;
+ const char * port;
+ int sleep_nsec;
+ int init_motor_state;
+ int motor_state;
+ double speed_factor;
+ double aspeed_factor;
+ int publish_ranges;
+ int set_stall;
+ double stall_threshold;
+ int stalled;
+ int prev_speed[2];
+ double prev_vel;
+ static double btm(int i);
+};
+
+Hemisson::Hemisson(ConfigFile * cf, int section)
+ : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
+{
+ this->serial = NULL;
+ this->debug = 0;
+ memset(&(this->position2d_addr), 0, sizeof(player_devaddr_t));
+ memset(&(this->ranger_addr), 0, sizeof(player_devaddr_t));
+ this->port = NULL;
+ this->sleep_nsec = 0;
+ this->init_motor_state = 0;
+ this->motor_state = 0;
+ this->speed_factor = 0.0;
+ this->publish_ranges = 0;
+ this->set_stall = 0;
+ this->stall_threshold = 0.0;
+ this->stalled = 0;
+ this->prev_speed[0] = 0;
+ this->prev_speed[1] = 0;
+ this->prev_vel = 0.0;
+ if (cf->ReadDeviceAddr(&(this->position2d_addr), section, "provides",
PLAYER_POSITION2D_CODE, -1, NULL))
+ {
+ this->SetError(-1);
+ return;
+ }
+ if (this->AddInterface(this->position2d_addr))
+ {
+ this->SetError(-1);
+ return;
+ }
+ if (cf->ReadDeviceAddr(&(this->ranger_addr), section, "provides",
PLAYER_RANGER_CODE, -1, NULL))
+ {
+ this->SetError(-1);
+ return;
+ }
+ if (this->AddInterface(this->ranger_addr))
+ {
+ this->SetError(-1);
+ return;
+ }
+ this->port = cf->ReadString(section, "port", HEMISSON_DEFAULT_SERIAL_PORT);
+ if (!(this->port))
+ {
+ this->SetError(-1);
+ return;
+ }
+ if (!(strlen(this->port) > 0))
+ {
+ this->SetError(-1);
+ return;
+ }
+ this->debug = cf->ReadInt(section, "debug", 0);
+ this->sleep_nsec = cf->ReadInt(section, "sleep_nsec", 100000000);
+ if ((this->sleep_nsec) < 0)
+ {
+ this->SetError(-1);
+ return;
+ }
+ this->init_motor_state = cf->ReadInt(section, "init_motor_state", 0);
+ this->speed_factor = cf->ReadFloat(section, "speed_factor", 18.0);
+ this->aspeed_factor = cf->ReadFloat(section, "aspeed_factor", 16.0);
+ this->publish_ranges = cf->ReadInt(section, "publish_ranges", 8);
+ if (((this->publish_ranges) < 1) || ((this->publish_ranges) >
HEMISSON_RANGES))
+ {
+ PLAYER_ERROR("Invalid number of ranges to publish");
+ this->SetError(-1);
+ return;
+ }
+ this->set_stall = cf->ReadInt(section, "set_stall", 0);
+ if ((this->set_stall) && (this->publish_ranges < 6))
+ {
+ PLAYER_ERROR("Stall detection needs at least 6 ir ranger sensors");
+ this->SetError(-1);
+ return;
+ }
+ this->stall_threshold = cf->ReadFloat(section, "stall_threshold", 0.025);
+ if ((this->stall_threshold) < 0.0)
+ {
+ this->SetError(-1);
+ return;
+ }
+}
+
+Hemisson::~Hemisson()
+{
+ if (this->serial) delete this->serial;
+ this->serial = NULL;
+}
+
+int Hemisson::MainSetup()
+{
+ try
+ {
+ this->serial = new HemissonSerial(this->debug, this->port);
+ } catch (...)
+ {
+ this->serial = NULL;
+ }
+ if (!(this->serial)) return -1;
+ return 0;
+}
+
+void Hemisson::MainQuit()
+{
+ if (this->serial) delete this->serial;
+ this->serial = NULL;
+}
+
+void Hemisson::Main()
+{
+ struct timespec tspec;
+ double rrval[HEMISSON_RANGES];
+ player_ranger_data_range_t ranges = {
static_cast<uint32_t>(this->publish_ranges), rrval };
+ int rval[HEMISSON_RANGES];
+ player_position2d_data_t pose;
+ int i;
+
+ this->motor_state = this->init_motor_state;
+ this->prev_speed[0] = -1000;
+ this->prev_speed[1] = -1000;
+ this->prev_vel = 0.0;
+ this->stalled = 0;
+ for (;;)
+ {
+ pthread_testcancel();
+ this->ProcessMessages();
+ pthread_testcancel();
+ if (!(this->serial->HemissonCommand('N', 0, NULL, HEMISSON_RANGES, rval)))
+ {
+ assert((ranges.ranges_count) ==
static_cast<uint32_t>(this->publish_ranges));
+ assert(ranges.ranges);
+ for (i = 0; i < (this->publish_ranges); i++)
+ {
+ ranges.ranges[i] = Hemisson::btm(rval[i]);
+ }
+ if (this->set_stall)
+ {
+ if ((this->prev_vel) > EPS)
+ {
+ if (((ranges.ranges[0]) < (this->stall_threshold)) ||
((ranges.ranges[1]) < (this->stall_threshold)) || ((ranges.ranges[2]) <
(this->stall_threshold))) this->stalled = !0;
+ else this->stalled = 0;
+ } else if ((this->prev_vel) < (-EPS))
+ {
+ if ((ranges.ranges[5]) < (this->stall_threshold)) this->stalled = !0;
+ else this->stalled = 0;
+ }
+ }
+ this->Publish(this->ranger_addr, PLAYER_MSGTYPE_DATA,
PLAYER_RANGER_DATA_RANGE, reinterpret_cast<void *>(&ranges));
+ }
+ pthread_testcancel();
+ memset(&(pose), 0, sizeof pose);
+ pose.stall = (this->set_stall) ? (this->stalled) : 0;
+ this->Publish(this->position2d_addr, PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE, reinterpret_cast<void *>(&pose));
+ pthread_testcancel();
+ if (this->sleep_nsec > 0)
+ {
+ tspec.tv_sec = 0;
+ tspec.tv_nsec = this->sleep_nsec;
+ nanosleep(&tspec, NULL);
+ }
+ }
+}
+
+int Hemisson::ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr,
void * data)
+{
+ player_ranger_geom_t ranger_geom;
+ player_pose3d_t ranger_poses[HEMISSON_RANGES];
+ player_bbox3d_t ranger_sizes[HEMISSON_RANGES];
+ player_ranger_config_t ranger_config;
+ player_position2d_geom_t position_geom;
+ player_position2d_power_config_t * position_power;
+ player_position2d_cmd_vel_t * position_cmd;
+ int i, rotvel, speed[2];
+ double d;
+
+ assert(hdr);
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_RANGER_REQ_GET_GEOM, this->ranger_addr))
+ {
+ memset(&ranger_geom, 0, sizeof ranger_geom);
+ ranger_geom.size.sw = HEMISSON_WIDTH;
+ ranger_geom.size.sl = 0.13;
+ ranger_geom.size.sh = 0.006;
+ ranger_geom.element_poses_count =
static_cast<uint32_t>(this->publish_ranges);
+ ranger_geom.element_poses = ranger_poses;
+ assert(ranger_geom.element_poses);
+ memset(&(ranger_geom.element_poses[0]), 0, sizeof
ranger_geom.element_poses[0]);
+ ranger_geom.element_poses[0].px = 0.06;
+ ranger_geom.element_poses[0].py = 0.0;
+ ranger_geom.element_poses[0].pz = 0.0;
+ ranger_geom.element_poses[0].pyaw = DTOR(0.0);
+ memset(&(ranger_geom.element_poses[1]), 0, sizeof
ranger_geom.element_poses[1]);
+ ranger_geom.element_poses[1].px = 0.055;
+ ranger_geom.element_poses[1].py = -0.04;
+ ranger_geom.element_poses[1].pz = 0.0;
+ ranger_geom.element_poses[1].pyaw = DTOR(-45.0);
+ memset(&(ranger_geom.element_poses[2]), 0, sizeof
ranger_geom.element_poses[2]);
+ ranger_geom.element_poses[2].px = 0.055;
+ ranger_geom.element_poses[2].py = 0.04;
+ ranger_geom.element_poses[2].pz = 0.0;
+ ranger_geom.element_poses[2].pyaw = DTOR(45.0);
+ memset(&(ranger_geom.element_poses[3]), 0, sizeof
ranger_geom.element_poses[3]);
+ ranger_geom.element_poses[3].px = 0.0;
+ ranger_geom.element_poses[3].py = -0.04;
+ ranger_geom.element_poses[3].pz = 0.0;
+ ranger_geom.element_poses[3].pyaw = DTOR(-90.0);
+ memset(&(ranger_geom.element_poses[4]), 0, sizeof
ranger_geom.element_poses[4]);
+ ranger_geom.element_poses[4].px = 0.0;
+ ranger_geom.element_poses[4].py = 0.04;
+ ranger_geom.element_poses[4].pz = 0.0;
+ ranger_geom.element_poses[4].pyaw = DTOR(90.0);
+ memset(&(ranger_geom.element_poses[5]), 0, sizeof
ranger_geom.element_poses[5]);
+ ranger_geom.element_poses[5].px = -0.06;
+ ranger_geom.element_poses[5].py = 0.0;
+ ranger_geom.element_poses[5].pz = 0.0;
+ ranger_geom.element_poses[5].pyaw = DTOR(180.0);
+ memset(&(ranger_geom.element_poses[6]), 0, sizeof
ranger_geom.element_poses[6]);
+ ranger_geom.element_poses[6].ppitch = DTOR(90.0);
+ memset(&(ranger_geom.element_poses[7]), 0, sizeof
ranger_geom.element_poses[7]);
+ ranger_geom.element_poses[7].ppitch = DTOR(-90.0);
+ ranger_geom.element_sizes_count =
static_cast<uint32_t>(this->publish_ranges);
+ ranger_geom.element_sizes = ranger_sizes;
+ assert(ranger_geom.element_sizes);
+ for (i = 0; i < HEMISSON_RANGES; i++)
+ {
+ memset(&(ranger_geom.element_sizes[i]), 0, sizeof
ranger_geom.element_sizes[i]);
+ ranger_geom.element_sizes[i].sw = 0.003;
+ ranger_geom.element_sizes[i].sl = 0.007;
+ ranger_geom.element_sizes[i].sh = 0.006;
+ }
+ this->Publish(this->ranger_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_RANGER_REQ_GET_GEOM, reinterpret_cast<void *>(&ranger_geom));
+ return 0;
+ }
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_RANGER_REQ_GET_CONFIG, this->ranger_addr))
+ {
+ // as in sonartoranger.cc: no config for this device, so send back a pile
of zeroes
+ memset(&ranger_config, 0, sizeof ranger_config);
+ this->Publish(this->ranger_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_RANGER_REQ_GET_CONFIG, reinterpret_cast<void *>(&ranger_config));
+ return 0;
+ }
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_GET_GEOM, this->position2d_addr))
+ {
+ memset(&position_geom, 0, sizeof position_geom);
+ position_geom.size.sw = HEMISSON_WIDTH;
+ position_geom.size.sl = 0.13;
+ position_geom.size.sh = 0.05;
+ this->Publish(this->position2d_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_GET_GEOM, reinterpret_cast<void *>(&position_geom));
+ return 0;
+ }
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_MOTOR_POWER, this->position2d_addr))
+ {
+ assert(data);
+ position_power = reinterpret_cast<player_position2d_power_config_t
*>(data);
+ assert(position_power);
+ this->motor_state = position_power->state;
+ this->Publish(this->position2d_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_MOTOR_POWER);
+ return 0;
+ }
+ if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL, this->position2d_addr))
+ {
+ assert(data);
+ position_cmd = reinterpret_cast<player_position2d_cmd_vel_t *>(data);
+ assert(position_cmd);
+ d = (position_cmd->vel.px) * (this->speed_factor);
+ i = static_cast<int>(d);
+ if (d > EPS)
+ {
+ if (static_cast<double>(i) < d) i++;
+ } else if (d < (-EPS))
+ {
+ if (static_cast<double>(i) > d) i--;
+ } else i = 0;
+ speed[0] = i;
+ speed[1] = i;
+ if (fabs(position_cmd->vel.pa) > EPS)
+ {
+ rotvel = static_cast<int>(RTOD(position_cmd->vel.pa) *
(this->aspeed_factor) * M_PI * (HEMISSON_WIDTH * 10.0) / 360.0);
+ speed[0] -= rotvel;
+ speed[1] += rotvel;
+ }
+ if (this->debug) PLAYER_WARN4("vel: %.4f, %.4f - speed: %d, %d",
position_cmd->vel.px, position_cmd->vel.pa, speed[0], speed[1]);
+ if (speed[0] > 9) speed[0] = 9;
+ if (speed[0] < -9) speed[0] = -9;
+ if (speed[1] > 9) speed[1] = 9;
+ if (speed[1] < -9) speed[1] = -9;
+ if (this->motor_state)
+ {
+ if (((this->prev_speed[0]) != speed[0]) || ((this->prev_speed[1]) !=
speed[1]))
+ {
+ this->prev_speed[0] = speed[0];
+ this->prev_speed[1] = speed[1];
+ this->serial->HemissonCommand('D', 2, speed, 0, NULL);
+ }
+ this->prev_vel = position_cmd->vel.px;
+ }
+ return 0;
+ }
+ return -1;
+}
+
+double Hemisson::btm(int i)
+{
+ if (i > 250) return EPS;
+ if (i > 100) return 0.01;
+ if (i > 50) return 0.02 - (static_cast<double>(i) / 100000.0);
+ if (i > 20) return 0.03 - (static_cast<double>(i) / 10000.0);
+ return 0.08 - (static_cast<double>(i) / 1000.0);
+}
+
+Driver * Hemisson_Init(ConfigFile * cf, int section)
+{
+ return reinterpret_cast<Driver *>(new Hemisson(cf, section));
+}
+
+void hemisson_Register(DriverTable * table)
+{
+ table->AddDriver("hemisson", Hemisson_Init);
+}
+
Added: code/player/trunk/server/drivers/mixed/hemisson/hemisson_serial.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/hemisson/hemisson_serial.cc
(rev 0)
+++ code/player/trunk/server/drivers/mixed/hemisson/hemisson_serial.cc
2010-07-08 03:21:37 UTC (rev 8819)
@@ -0,0 +1,570 @@
+/*
+ * Serial communication helper class for Hemisson robot driver
+ * Copyright (C) 2010 Paul Osmialowski
+ * Based on Minicom code released on the same license
+ * Minicom is Copyright (C) 1991,1992,1993,1994,1995,1996
+ * Miquel van Smoorenburg.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+#include "hemisson_serial.h"
+
+#include <stddef.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <sys/ioctl.h>
+#include <sys/select.h>
+#include <time.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define FAIL -1
+
+#ifndef CRTSCTS
+#ifdef IHFLOW
+#ifdef OHFLOW
+#define CRTSCTS ((IHFLOW) | (OHFLOW))
+#endif
+#endif
+#endif
+
+HemissonSerial::HemissonSerial(int debug, const char * port, const char *
rate) : debug(debug)
+{
+ int beep[1];
+ fd = -1;
+ parity = 0;
+ ttybuf[0] = 0;
+ struct timespec ts;
+
+ // open the serial port
+ fd = serial_open(port);
+ if (fd < 0)
+ {
+ fprintf(stderr, "Could not open serial device %s\n", port);
+ return;
+ }
+ m_setparms(fd, rate, "N", "8", "1", 0, 0);
+ tcflush(fd, TCIOFLUSH);
+ // communication sanity check
+ printf("Hemisson> %s\n", m_gets(fd, HEMISSON_SERIAL_TIMEOUT_USECS));
+ printf("Hemisson> %s\n", m_gets(fd, HEMISSON_SERIAL_TIMEOUT_USECS));
+ // clear the input buffer in case junk data is on the port
+ HemissonCommand('B', 0, NULL, 0, NULL);
+ tcflush(fd, TCIFLUSH);
+ // try a test command
+ beep[0] = 1;
+ HemissonCommand('H', 1, beep, 0, NULL);
+ ts.tv_sec = 0;
+ ts.tv_nsec = 500000000;
+ nanosleep(&ts, NULL);
+ beep[0] = 0;
+ HemissonCommand('H', 1, beep, 0, NULL);
+}
+
+HemissonSerial::~HemissonSerial()
+{
+ if (fd > 0)
+ {
+ serial_close(fd);
+ }
+}
+
+int HemissonSerial::WriteInts(char command, int Count, int * Values)
+{
+ int i;
+
+ if (fd < 0) return -1;
+ buffer[0] = command;
+ buffer[1] = '\0';
+ int length;
+ if (Values) for (i = 0; i < Count; ++ i)
+ {
+ length = strlen(buffer);
+ snprintf(&buffer[length],HEMISSON_BUFFER_LEN - length -
2,",%d",Values[i]);
+ }
+ if (debug)
+ {
+ printf(">>>> %s\n", buffer); fflush(stdout);
+ }
+ m_puts(fd, buffer);
+ return 0;
+}
+
+#define MAX_RETRIES 5
+// read ints from Hemisson
+int HemissonSerial::ReadInts(char Header, int Count, int * Values)
+{
+ int i;
+
+ buffer[0] = 0;
+ if (fd < 0) return -1;
+ for (i = 0; i < MAX_RETRIES; i++)
+ {
+ strncpy(buffer, m_gets(fd, HEMISSON_SERIAL_TIMEOUT_USECS),
HEMISSON_BUFFER_LEN);
+ if (debug)
+ {
+ printf("[%d] %s\n", i, buffer); fflush(stdout);
+ }
+ if (buffer[0]) break;
+ m_puts(fd, "B");
+ }
+ if (buffer[0] != Header) return -1;
+ char * pos = &buffer[2];
+ if (Values) for (i = 0; i < Count; i++)
+ {
+ Values[i] = strtol(pos, &pos, 10);
+ pos++;
+ }
+ return 0;
+}
+
+int HemissonSerial::HemissonCommand(char command, int InCount, int * InValues,
int OutCount, int * OutValues)
+{
+ struct timespec ts;
+ int ret2;
+
+ WriteInts(command,InCount,InValues);
+ ts.tv_sec = 0;
+ ts.tv_nsec = 50000000;
+ nanosleep(&ts, NULL);
+ ret2 = ReadInts(command + 32, OutCount, OutValues);
+ ts.tv_sec = 0;
+ ts.tv_nsec = 50000000;
+ nanosleep(&ts, NULL);
+ return ret2;
+}
+
+char * HemissonSerial::m_gets(int fd, int tmout)
+{
+ int n, f, q;
+ struct timeval tv;
+ fd_set fds;
+ char * buf;
+
+ buf = ttybuf; f = 0; q = 0;
+
+ for (;;)
+ {
+ buf[0]=0;
+ // tv.tv_sec = tmout / 1000;
+ // tv.tv_usec = (tmout % 1000) * 1000L;
+ tv.tv_sec = 0;
+ tv.tv_usec = tmout;
+
+ FD_ZERO(&fds);
+ FD_SET(fd, &fds);
+
+ if (select(fd + 1, &fds, NULL, NULL, &tv) > 0)
+ {
+ if (FD_ISSET(fd, &fds) > 0)
+ {
+ n = read(fd, buf, 1);
+ if (n > 0)
+ {
+ if (q)
+ {
+ buf++;
+ buf[0] = 0;
+ if ((parity == 'M') || (parity == 'S')) ttybuf[0] &= 0x7f;
+ return ttybuf;
+ }
+ switch(buf[0])
+ {
+ case 10:
+ buf[0] = 0;
+ if ((parity == 'M') || (parity == 'S')) ttybuf[0] &= 0x7f;
+ return ttybuf;
+ case 13:
+ buf[0] = 0;
+ break;
+ case 16:
+ buf[0] = 0;
+ q = !0;
+ break;
+ default:
+ buf++; f++;
+ if (f >= TTYBUFFLEN)
+ {
+ buf[0] = 0;
+ if ((parity == 'M') || (parity == 'S')) ttybuf[0] &= 0x7f;
+ return ttybuf;
+ }
+ }
+ }
+ }
+ } else break;
+ }
+ if ((parity == 'M') || (parity == 'S')) ttybuf[0] &= 0x7f;
+ return ttybuf;
+}
+
+int HemissonSerial::m_getchar(int fd, int tmout)
+{
+ int n;
+ struct timeval tv;
+ fd_set fds;
+
+ // tv.tv_sec = tmout / 1000;
+ // tv.tv_usec = (tmout % 1000) * 1000L;
+ tv.tv_sec = 0;
+ tv.tv_usec = tmout;
+
+ FD_ZERO(&fds);
+ FD_SET(fd, &fds);
+
+ if (select(fd + 1, &fds, NULL, NULL, &tv) > 0)
+ {
+ if (FD_ISSET(fd, &fds) > 0)
+ {
+ n = read(fd, ttybuf, 1);
+ if (n > 0) return static_cast<int>(static_cast<unsigned
char>(ttybuf[0]));
+ }
+ }
+ return -1;
+}
+
+/*
+ * Send a string to the modem.
+ */
+void HemissonSerial::m_puts(int fd, const char * s)
+{
+ struct timespec ts;
+ char c;
+
+ while (*s)
+ {
+ if (*s == '^' && (*(s + 1)))
+ {
+ s++;
+ if (*s == '^')
+ c = *s;
+ else
+ c = (*s) & 31;
+ } else c = *s;
+ if (c == '~')
+ {
+ ts.tv_sec = 1;
+ ts.tv_nsec = 0;
+ nanosleep(&ts, NULL);
+ } else write(fd, &c, 1);
+ s++;
+ }
+ c = 13;
+ write(fd, &c, 1);
+}
+
+void HemissonSerial::m_putchar(int fd, int chr)
+{
+ char c;
+ struct timespec ts;
+
+ c = static_cast<char>(chr);
+ if (c == '~')
+ {
+ ts.tv_sec = 1;
+ ts.tv_nsec = 0;
+ nanosleep(&ts, NULL);
+ } else write(fd, &c, 1);
+}
+
+/* Set hardware flow control. */
+void HemissonSerial::m_sethwf(int fd, int on)
+{
+ struct termios tty;
+
+ tcgetattr(fd, &tty);
+ if (on)
+ tty.c_cflag |= CRTSCTS;
+ else
+ tty.c_cflag &= ~CRTSCTS;
+ tcsetattr(fd, TCSANOW, &tty);
+}
+
+/* Set RTS line. Sometimes dropped. Linux specific? */
+void HemissonSerial::m_setrts(int fd)
+{
+#if defined(TIOCM_RTS) && defined(TIOCMGET)
+ int mcs=0;
+
+ ioctl(fd, TIOCMGET, &mcs);
+ mcs |= TIOCM_RTS;
+ ioctl(fd, TIOCMSET, &mcs);
+#endif
+}
+
+/*
+ * Drop DTR line and raise it again.
+ */
+void HemissonSerial::m_dtrtoggle(int fd, int sec)
+{
+ /* Posix - set baudrate to 0 and back */
+ struct termios tty, old;
+ struct timespec ts;
+
+ tcgetattr(fd, &tty);
+ tcgetattr(fd, &old);
+ cfsetospeed(&tty, B0);
+ cfsetispeed(&tty, B0);
+ tcsetattr(fd, TCSANOW, &tty);
+ if (sec > 0)
+ {
+ ts.tv_sec = sec;
+ ts.tv_nsec = 0;
+ nanosleep(&ts, NULL);
+ tcsetattr(fd, TCSANOW, &old);
+ }
+}
+
+/*
+ * Send a break
+ */
+void HemissonSerial::m_break(int fd)
+{
+ tcsendbreak(fd, 0);
+}
+
+/*
+ * Get the dcd status
+ */
+int HemissonSerial::m_getdcd(int fd)
+{
+#ifdef TIOCMGET
+ int mcs=0;
+
+ ioctl(fd, TIOCMGET, &mcs);
+ return (mcs & TIOCM_CAR) ? 1 : 0;
+#else
+ fd = fd;
+ return 0; /* Impossible!! (eg. Coherent) */
+#endif
+}
+
+/*
+ * Save the state of a port
+ */
+void HemissonSerial::m_savestate(int fd)
+{
+ tcgetattr(fd, &savetty);
+#ifdef TIOCMGET
+ ioctl(fd, TIOCMGET, &m_word);
+#endif
+}
+
+/*
+ * Restore the state of a port
+ */
+void HemissonSerial::m_restorestate(int fd)
+{
+ tcsetattr(fd, TCSANOW, &savetty);
+#ifdef TIOCMSET
+ ioctl(fd, TIOCMSET, &m_word);
+#endif
+}
+
+/*
+ * Set the line status so that it will not kill our process
+ * if the line hangs up.
+ */
+/*ARGSUSED*/
+void HemissonSerial::m_nohang(int fd)
+{
+ struct termios sgg;
+
+ tcgetattr(fd, &sgg);
+ sgg.c_cflag |= CLOCAL;
+ tcsetattr(fd, TCSANOW, &sgg);
+}
+
+/*
+ * Set hangup on close on/off.
+ */
+void HemissonSerial::m_hupcl(int fd, int on)
+{
+ struct termios sgg;
+
+ tcgetattr(fd, &sgg);
+ if (on) sgg.c_cflag |= HUPCL;
+ else sgg.c_cflag &= ~HUPCL;
+ tcsetattr(fd, TCSANOW, &sgg);
+}
+
+/*
+ * Flush the buffers
+ */
+void HemissonSerial::m_flush(int fd)
+{
+/* Should I Posixify this, or not? */
+#ifdef TCFLSH
+ ioctl(fd, TCFLSH, 2);
+#endif
+#ifdef TIOCFLUSH
+ ioctl(fd, TIOCFLUSH, NULL);
+#endif
+}
+
+/*
+ * Set baudrate, parity and number of bits.
+ */
+void HemissonSerial::m_setparms(int fd, const char * baudr, const char * par,
const char * bits, const char * stopb, int hwf, int swf)
+{
+ int spd = -1;
+ int newbaud;
+ int bit = bits[0];
+
+ struct termios tty;
+
+ tcgetattr(fd, &tty);
+
+ /* We generate mark and space parity ourself. */
+ parity = par[0];
+ if (bit == '7' && (par[0] == 'M' || par[0] == 'S'))
+ bit = '8';
+
+ /* Check if 'baudr' is really a number */
+ if ((newbaud = (atol(baudr) / 100)) == 0 && baudr[0] != '0') newbaud = -1;
+
+ switch(newbaud) {
+ case 0: spd = B0; break;
+ case 3: spd = B300; break;
+ case 6: spd = B600; break;
+ case 12: spd = B1200; break;
+ case 24: spd = B2400; break;
+ case 48: spd = B4800; break;
+ case 96: spd = B9600; break;
+#ifdef B19200
+ case 192: spd = B19200; break;
+#else /* B19200 */
+#ifdef EXTA
+ case 192: spd = EXTA; break;
+#else /* EXTA */
+ case 192: spd = B9600; break;
+#endif /* EXTA */
+#endif /* B19200 */
+#ifdef B38400
+ case 384: spd = B38400; break;
+#else /* B38400 */
+#ifdef EXTB
+ case 384: spd = EXTB; break;
+#else /* EXTB */
+ case 384: spd = B9600; break;
+#endif /* EXTB */
+#endif /* B38400 */
+#ifdef B57600
+ case 576: spd = B57600; break;
+#endif
+#ifdef B115200
+ case 1152: spd = B115200; break;
+#endif
+#ifdef B230400
+ case 2304: spd = B230400; break;
+#endif
+ }
+ if (spd != -1) {
+ cfsetospeed(&tty, (speed_t)spd);
+ cfsetispeed(&tty, (speed_t)spd);
+ }
+ switch (bit) {
+ case '5':
+ tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS5;
+ break;
+ case '6':
+ tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS6;
+ break;
+ case '7':
+ tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS7;
+ break;
+ case '8':
+ default:
+ tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;
+ break;
+ }
+ /* Set into raw, no echo mode */
+ tty.c_iflag = IGNBRK;
+ tty.c_lflag = 0;
+ tty.c_oflag = 0;
+ tty.c_cflag |= CLOCAL | CREAD;
+#ifdef _DCDFLOW
+ tty.c_cflag &= ~CRTSCTS;
+#endif
+ tty.c_cc[VMIN] = 1;
+ tty.c_cc[VTIME] = 5;
+
+ if (swf)
+ tty.c_iflag |= IXON | IXOFF;
+ else
+ tty.c_iflag &= ~(IXON|IXOFF|IXANY);
+
+ tty.c_cflag &= ~(PARENB | PARODD);
+ if (par[0] == 'E')
+ tty.c_cflag |= PARENB;
+ else if (par[0] == 'O')
+ tty.c_cflag |= (PARENB | PARODD);
+
+ if (stopb[0] == '2')
+ tty.c_cflag |= CSTOPB;
+ else
+ tty.c_cflag &= ~CSTOPB;
+
+ tcsetattr(fd, TCSANOW, &tty);
+
+ m_setrts(fd);
+
+#ifndef _DCDFLOW
+ m_sethwf(fd, hwf);
+#endif
+}
+
+int HemissonSerial::serial_open(const char * devname)
+{
+ int f, portfd;
+ struct stat stt;
+
+ ttybuf[0] = 0;
+ ttybuf[TTYBUFFLEN] = 0;
+ if (stat(devname, &stt)==-1)
+ {
+ fprintf(stderr, "Cannot stat device: %s\n", devname);
+ return FAIL;
+ }
+ portfd=open(devname, O_RDWR|O_NDELAY);
+ if (portfd<0)
+ {
+ fprintf(stderr, "Cannot open device: %s\n", devname);
+ return FAIL;
+ }
+ f = fcntl (portfd, F_GETFL, 0);
+ fcntl(portfd, F_SETFL, f & ~O_NDELAY);
+ m_savestate(portfd);
+ m_nohang(portfd);
+ m_hupcl(portfd, 1);
+ m_flush(portfd);
+ return portfd;
+}
+
+void HemissonSerial::serial_close(int fd)
+{
+ if (fd >= 0)
+ {
+ m_restorestate(fd);
+ close(fd);
+ }
+}
Added: code/player/trunk/server/drivers/mixed/hemisson/hemisson_serial.h
===================================================================
--- code/player/trunk/server/drivers/mixed/hemisson/hemisson_serial.h
(rev 0)
+++ code/player/trunk/server/drivers/mixed/hemisson/hemisson_serial.h
2010-07-08 03:21:37 UTC (rev 8819)
@@ -0,0 +1,85 @@
+/*
+ * Serial communication helper class for Hemisson robot driver
+ * Copyright (C) 2010 Paul Osmialowski
+ * Based on Minicom code released on the same license
+ * Minicom is Copyright (C) 1991,1992,1993,1994,1995,1996
+ * Miquel van Smoorenburg.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+#ifndef HEMISSON_SERIAL_H
+#define HEMISSON_SERIAL_H
+
+#include <stddef.h>
+#include <termios.h>
+
+#define HEMISSON_BAUDRATE "115200"
+#define HEMISSON_DEFAULT_SERIAL_PORT "/dev/rfcomm0"
+#define HEMISSON_BUFFER_LEN 255
+#define HEMISSON_SERIAL_TIMEOUT_USECS 100000
+
+#define TTYBUFFLEN 255
+#define TTYBUFFSIZE (TTYBUFFLEN + 1)
+
+class HemissonSerial
+{
+public:
+ HemissonSerial(int debug = 0, const char * port =
HEMISSON_DEFAULT_SERIAL_PORT, const char * rate = HEMISSON_BAUDRATE);
+ virtual ~HemissonSerial();
+
+ bool Open() { return fd >0; };
+ int HemissonCommand(char command, int InCount, int * InValues, int
OutCount, int * OutValues);
+
+protected:
+ // serial port descriptor
+ int fd;
+ // read/write buffer
+ char buffer[HEMISSON_BUFFER_LEN + 1];
+
+ int WriteInts(char command, int Count = 0, int * Values = NULL);
+ int ReadInts(char Header, int Count = 0, int * Values = NULL);
+
+ int debug;
+
+private:
+ char ttybuf[TTYBUFFSIZE];
+ char parity;
+ struct termios savetty;
+ int m_word;
+
+ char * m_gets(int fd, int tmout);
+ int m_getchar(int fd, int tmout);
+ void m_puts(int fd, const char * s);
+ void m_putchar(int fd, int chr);
+ void m_dtrtoggle(int fd, int sec);
+ void m_break(int fd);
+ int m_getdcd(int fd);
+ void m_flush(int fd);
+ void m_setparms(int fd, const char * baudr, const char * par, const
char * bits, const char * stopb, int hwf, int swf);
+ int serial_open(const char * devname);
+ void serial_close(int fd);
+
+ void m_sethwf(int fd, int on);
+ void m_setrts(int fd);
+ void m_savestate(int fd);
+ void m_restorestate(int fd);
+ void m_nohang(int fd);
+ void m_hupcl(int fd, int on);
+};
+
+#endif
+
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
This SF.net email is sponsored by Sprint
What will you do first with EVO, the first 4G phone?
Visit sprint.com/first -- http://p.sf.net/sfu/sprint-com-first
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
