Update of /cvsroot/playerstage/code/player/server/drivers/mixed/irobot/create
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv32201/server/drivers/mixed/irobot/create
Added Files:
Tag: release-2-0-patches
.cvsignore Makefile.am create_comms.c create_comms.h
create_driver.cc
Log Message:
backported create driver from HEAD
--- NEW FILE: .cvsignore ---
Makefile
Makefile.in
.deps
*.la
.libs
*.lo
--- NEW FILE: create_comms.c ---
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2006 -
* Brian Gerkey
*
*
* 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 <errno.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <termios.h>
#include <math.h>
#include <stdio.h>
#include <unistd.h>
#include <netinet/in.h>
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <libplayercore/playercommon.h>
#include <libplayercore/playerconfig.h>
#include <replace/replace.h>
//#include <sys/poll.h>
#include "create_comms.h"
#define DTOR(d) ((d) * M_PI / 180)
create_comm_t*
create_create(const char* serial_port)
{
create_comm_t* r;
r = calloc(1,sizeof(create_comm_t));
assert(r);
r->fd = -1;
r->mode = CREATE_MODE_OFF;
r->oa = r->ox = r->oy = 0;
strncpy(r->serial_port,serial_port,sizeof(r->serial_port)-1);
return(r);
}
void
create_destroy(create_comm_t* r)
{
free(r);
}
int
create_open(create_comm_t* r, unsigned char fullcontrol)
{
struct termios term;
int flags;
if(r->fd >= 0)
{
puts("create connection already open!");
return(-1);
}
printf("Opening connection to Create on %s...", r->serial_port);
fflush(stdout);
// Open it. non-blocking at first, in case there's no create
if((r->fd = open(r->serial_port,
O_RDWR | O_NONBLOCK, S_IRUSR | S_IWUSR )) < 0 )
{
perror("create_open():open():");
return(-1);
}
if(tcflush(r->fd, TCIFLUSH) < 0 )
{
perror("create_open():tcflush():");
close(r->fd);
r->fd = -1;
return(-1);
}
if(tcgetattr(r->fd, &term) < 0 )
{
perror("create_open():tcgetattr():");
close(r->fd);
r->fd = -1;
return(-1);
}
cfmakeraw(&term);
cfsetispeed(&term, B57600);
cfsetospeed(&term, B57600);
if(tcsetattr(r->fd, TCSAFLUSH, &term) < 0 )
{
perror("create_open():tcsetattr():");
close(r->fd);
r->fd = -1;
return(-1);
}
if(create_init(r, fullcontrol) < 0)
{
puts("failed to initialize connection");
close(r->fd);
r->fd = -1;
return(-1);
}
if(create_get_sensors(r, 10000) < 0)
{
puts("create_open():failed to get data");
close(r->fd);
r->fd = -1;
return(-1);
}
r->oa = r->ox = r->oy = 0;
// We know the robot is there; switch to blocking
// ok, we got data, so now set NONBLOCK, and continue
if((flags = fcntl(r->fd, F_GETFL)) < 0)
{
perror("create_open():fcntl():");
close(r->fd);
r->fd = -1;
return(-1);
}
if(fcntl(r->fd, F_SETFL, flags ^ O_NONBLOCK) < 0)
{
perror("create_open():fcntl()");
close(r->fd);
r->fd = -1;
return(-1);
}
puts("Done.");
return(0);
}
int
create_init(create_comm_t* r, unsigned char fullcontrol)
{
unsigned char cmdbuf[1];
//usleep(CREATE_DELAY_MODECHANGE_MS * 1e3);
cmdbuf[0] = CREATE_OPCODE_START;
if(write(r->fd, cmdbuf, 1) < 0)
{
perror("create_init():write():");
return(-1);
}
r->mode = CREATE_MODE_PASSIVE;
usleep(CREATE_DELAY_MODECHANGE_MS * 1e3);
if (fullcontrol)
{
cmdbuf[0] = CREATE_OPCODE_FULL;
if(write(r->fd, cmdbuf, 1) < 0)
{
perror("create_init():write():");
return(-1);
}
r->mode = CREATE_MODE_FULL;
}
else
{
cmdbuf[0] = CREATE_OPCODE_SAFE;
if(write(r->fd, cmdbuf, 1) < 0)
{
perror("create_init():write():");
return(-1);
}
r->mode = CREATE_MODE_SAFE;
}
usleep(100000);
return(0);
}
int
create_close(create_comm_t* r)
{
//unsigned char cmdbuf[1];
create_set_speeds(r, 0.0, 0.0);
usleep(CREATE_DELAY_MODECHANGE_MS * 1e3);
if(close(r->fd) < 0)
{
perror("create_close():close():");
return(-1);
}
else
return(0);
}
int
create_set_speeds(create_comm_t* r, double tv, double rv)
{
unsigned char cmdbuf[5];
int16_t tv_mm, rad_mm;
//printf("tv: %.3lf rv: %.3lf\n", tv, rv);
tv_mm = (int16_t)rint(tv * 1e3);
tv_mm = MAX(tv_mm, -CREATE_TVEL_MAX_MM_S);
tv_mm = MIN(tv_mm, CREATE_TVEL_MAX_MM_S);
if(rv == 0)
{
// Special case: drive straight
rad_mm = 0x8000;
}
else if(tv == 0)
{
// Special cases: turn in place
if(rv > 0)
rad_mm = 1;
else
rad_mm = -1;
tv_mm = (int16_t)rint(CREATE_AXLE_LENGTH * fabs(rv) * 1e3);
}
else
{
// General case: convert rv to turn radius
rad_mm = (int16_t)rint(tv_mm / rv);
// The robot seems to turn very slowly with the above
rad_mm /= 2;
//printf("real rad_mm: %d\n", rad_mm);
rad_mm = MAX(rad_mm, -CREATE_RADIUS_MAX_MM);
rad_mm = MIN(rad_mm, CREATE_RADIUS_MAX_MM);
}
//printf("tv_mm: %d rad_mm: %d\n", tv_mm, rad_mm);
cmdbuf[0] = CREATE_OPCODE_DRIVE;
cmdbuf[1] = (unsigned char)(tv_mm >> 8);
cmdbuf[2] = (unsigned char)(tv_mm & 0xFF);
cmdbuf[3] = (unsigned char)(rad_mm >> 8);
cmdbuf[4] = (unsigned char)(rad_mm & 0xFF);
//printf("set_speeds: %X %X %X %X %X\n",
//cmdbuf[0], cmdbuf[1], cmdbuf[2], cmdbuf[3], cmdbuf[4]);
if(write(r->fd, cmdbuf, 5) < 0)
{
perror("create_set_speeds():write():");
return(-1);
}
else
return(0);
}
int
create_get_sensors(create_comm_t* r, int timeout)
{
struct pollfd ufd[1];
unsigned char cmdbuf[2];
unsigned char databuf[CREATE_SENSOR_PACKET_SIZE];
int retval;
int numread;
int totalnumread;
//int i;
cmdbuf[0] = CREATE_OPCODE_SENSORS;
/* Zero to get all sensor data */
cmdbuf[1] = 0;
if(write(r->fd, cmdbuf, 2) < 0)
{
perror("create_get_sensors():write():");
return(-1);
}
ufd[0].fd = r->fd;
ufd[0].events = POLLIN;
totalnumread = 0;
while(totalnumread < sizeof(databuf))
{
retval = poll(ufd,1,timeout);
if(retval < 0)
{
if(errno == EINTR)
continue;
else
{
perror("create_get_sensors():poll():");
return(-1);
}
}
else if(retval == 0)
{
printf("create_get_sensors: poll timeout\n");
return(-1);
}
else
{
if((numread =
read(r->fd,databuf+totalnumread,sizeof(databuf)-totalnumread)) < 0)
{
perror("create_get_sensors():read()");
return(-1);
}
else
{
totalnumread += numread;
/*printf("read %d bytes; buffer so far:\n", numread);
for(i=0;i<totalnumread;i++)
printf("%x ", databuf[i]);
puts("");
*/
}
}
}
create_parse_sensor_packet(r, databuf, (size_t)totalnumread);
return(0);
}
int
create_parse_sensor_packet(create_comm_t* r, unsigned char* buf, size_t buflen)
{
unsigned char flag;
int16_t signed_int;
uint16_t unsigned_int;
double dist, angle;
int idx;
if(buflen != CREATE_SENSOR_PACKET_SIZE)
{
puts("create_parse_sensor_packet():packet is wrong size");
return(-1);
}
idx = 0;
/* Bumps, wheeldrops */
flag = buf[idx++];
r->bumper_right = (flag >> 0) & 0x01;
r->bumper_left = (flag >> 1) & 0x01;
r->wheeldrop_right = (flag >> 2) & 0x01;
r->wheeldrop_left = (flag >> 3) & 0x01;
r->wheeldrop_caster = (flag >> 4) & 0x01;
r->wall = buf[idx++] & 0x01;
r->cliff_left = buf[idx++] & 0x01;
r->cliff_frontleft = buf[idx++] & 0x01;
r->cliff_frontright = buf[idx++] & 0x01;
r->cliff_right = buf[idx++] & 0x01;
r->virtual_wall = buf[idx++] & 0x01;
flag = buf[idx++];
r->overcurrent_sidebrush = (flag >> 0) & 0x01;
r->overcurrent_vacuum = (flag >> 1) & 0x01;
r->overcurrent_mainbrush = (flag >> 2) & 0x01;
r->overcurrent_driveright = (flag >> 3) & 0x01;
r->overcurrent_driveleft = (flag >> 4) & 0x01;
// The next two bytes are unused (used to be dirt detector left and right)
idx += 2;
r->remote_opcode = buf[idx++];
flag = buf[idx++];
r->button_max = (flag >> 0) & 0x01;
r->button_clean = (flag >> 1) & 0x01;
r->button_spot = (flag >> 2) & 0x01;
r->button_power = (flag >> 3) & 0x01;
memcpy(&signed_int, buf+idx, 2);
idx += 2;
signed_int = (int16_t)ntohs((uint16_t)signed_int);
dist = signed_int / 1e3;
memcpy(&signed_int, buf+idx, 2);
idx += 2;
signed_int = (int16_t)ntohs((uint16_t)signed_int);
angle = DTOR(signed_int);
/* First-order odometric integration */
r->oa = NORMALIZE(r->oa + angle);
r->ox += dist * cos(r->oa);
r->oy += dist * sin(r->oa);
r->charging_state = buf[idx++];
memcpy(&unsigned_int, buf+idx, 2);
idx += 2;
unsigned_int = ntohs(unsigned_int);
r->voltage = unsigned_int / 1e3;
memcpy(&signed_int, buf+idx, 2);
idx += 2;
signed_int = (int16_t)ntohs((uint16_t)signed_int);
r->current = signed_int / 1e3;
r->temperature = (char)(buf[idx++]);
memcpy(&unsigned_int, buf+idx, 2);
idx += 2;
unsigned_int = ntohs(unsigned_int);
r->charge = unsigned_int / 1e3;
memcpy(&unsigned_int, buf+idx, 2);
idx += 2;
unsigned_int = ntohs(unsigned_int);
r->capacity = unsigned_int / 1e3;
return(0);
}
void
create_print(create_comm_t* r)
{
printf("mode: %d\n", r->mode);
printf("position: %.3lf %.3lf %.3lf\n", r->ox, r->oy, r->oa);
printf("bumpers: l:%d r:%d\n", r->bumper_left, r->bumper_right);
printf("wall: %d virtual wall: %d\n", r->wall, r->virtual_wall);
printf("wheeldrops: c:%d l:%d r:%d\n",
r->wheeldrop_caster, r->wheeldrop_left, r->wheeldrop_right);
printf("cliff: l:%d fl:%d fr:%d r:%d\n",
r->cliff_left, r->cliff_frontleft, r->cliff_frontright,
r->cliff_right);
printf("overcurrent: dl:%d dr:%d mb:%d sb:%d v:%d\n",
r->overcurrent_driveleft, r->overcurrent_driveright,
r->overcurrent_mainbrush, r->overcurrent_sidebrush,
r->overcurrent_vacuum);
printf("dirt: l:%d r:%d\n", r->dirtdetector_left, r->dirtdetector_right);
printf("remote opcode: %d\n", r->remote_opcode);
printf("buttons: p:%d s:%d c:%d m:%d\n",
r->button_power, r->button_spot, r->button_clean, r->button_max);
printf("charging state: %d\n", r->charging_state);
printf("battery: voltage:%.3lf current:%.3lf temp:%.3lf charge:%.3lf
capacity:%.3f\n",
r->voltage, r->current, r->temperature, r->charge, r->capacity);
}
int create_set_song(create_comm_t* r, unsigned char songNumber, unsigned char
songLength, unsigned char *notes, unsigned char *noteLengths)
{
int size = 2*songLength+3;
unsigned char cmdbuf[size];
unsigned char i;
cmdbuf[0] = CREATE_OPCODE_SONG;
cmdbuf[1] = songNumber;
cmdbuf[2] = songLength;
for (i=0; i < songLength; i++)
{
cmdbuf[3+(2*i)] = notes[i];
cmdbuf[3+(2*i)+1] = noteLengths[i];
}
if(write(r->fd, cmdbuf, size) < 0)
{
perror("create_set_song():write():");
return(-1);
}
else
return(0);
}
int
create_play_song(create_comm_t *r, unsigned char songNumber)
{
unsigned char cmdbuf[2];
cmdbuf[0] = CREATE_OPCODE_PLAY;
cmdbuf[1] = songNumber;
if(write(r->fd, cmdbuf, 2) < 0)
{
perror("create_set_song():write():");
return(-1);
}
else
return(0);
}
int
create_vacuum(create_comm_t *r, int state)
{
unsigned char cmdbuf[2];
cmdbuf[0] = CREATE_OPCODE_MOTORS;
cmdbuf[1] = state;
if (write(r->fd, cmdbuf, 2) < 0)
{
perror("create_vacuum():write():");
return -1;
}
return 0;
}
int
create_set_leds(create_comm_t *r, uint8_t dirt_detect, uint8_t max, uint8_t
clean, uint8_t spot, uint8_t status, uint8_t power_color, uint8_t
power_intensity )
{
unsigned char cmdbuf[5];
cmdbuf[0] = CREATE_OPCODE_LEDS;
cmdbuf[1] = dirt_detect | max<<1 | clean<<2 | spot<<3 | status<<4;
cmdbuf[2] = power_color;
cmdbuf[3] = power_intensity;
printf("Set LEDS[%d][%d][%d]\n",cmdbuf[1], cmdbuf[2], cmdbuf[3]);
if (write(r->fd, cmdbuf, 4) < 0)
{
perror("create_set_leds():write():");
return -1;
}
return 0;
}
int
create_run_demo(create_comm_t *r, uint8_t num)
{
unsigned char cmdbuf[2];
cmdbuf[0] = CREATE_OPCODE_DEMO;
cmdbuf[1] = num;
if (write(r->fd, cmdbuf, 2) < 0)
{
perror("create_run_demo():write():");
return -1;
}
}
--- NEW FILE: create_comms.h ---
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2006 -
* Brian Gerkey
*
*
* 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 CREATE_COMMS_H
#define CREATE_COMMS_H
#ifdef __cplusplus
extern "C" {
#endif
#include <limits.h>
/* command opcodes */
#define CREATE_OPCODE_START 128
#define CREATE_OPCODE_BAUD 129
#define CREATE_OPCODE_SAFE 131
#define CREATE_OPCODE_FULL 132
#define CREATE_OPCODE_SPOT 134
#define CREATE_OPCODE_COVER 135
#define CREATE_OPCODE_DEMO 136
#define CREATE_OPCODE_DRIVE 137
#define CREATE_OPCODE_MOTORS 138
#define CREATE_OPCODE_LEDS 139
#define CREATE_OPCODE_SONG 140
#define CREATE_OPCODE_PLAY 141
#define CREATE_OPCODE_SENSORS 142
#define CREATE_OPCODE_COVERDOCK 143
#define CREATE_OPCODE_PWM_MOTORS 144
#define CREATE_OPCODE_DRIVE_WHEELS 145
#define CREATE_OPCODE_DIGITAL_OUTPUTS 147
#define CREATE_OPCODE_STREAM 148
#define CREATE_OPCODE_QUERY_LIST 149
#define CREATE_OPCODE_DO_STREAM 150
#define CREATE_OPCODE_SEND_IR_CHAR 151
#define CREATE_OPCODE_SCRIPT 152
#define CREATE_OPCODE_PLAY_SCRIPT 153
#define CREATE_OPCODE_SHOW_SCRIPT 154
#define CREATE_OPCODE_WAIT_TIME 155
#define CREATE_OPCODE_WAIT_DISTANCE 156
#define CREATE_OPCODE_WAIT_ANGLE 157
#define CREATE_OPCODE_WAIT_EVENT 158
#define CREATE_DELAY_MODECHANGE_MS 20
#define CREATE_MODE_OFF 0
#define CREATE_MODE_PASSIVE 1
#define CREATE_MODE_SAFE 2
#define CREATE_MODE_FULL 3
#define CREATE_TVEL_MAX_MM_S 500
#define CREATE_RADIUS_MAX_MM 2000
#define CREATE_SENSOR_PACKET_SIZE 26
#define CREATE_CHARGING_NOT 0
#define CREATE_CHARGING_RECOVERY 1
#define CREATE_CHARGING_CHARGING 2
#define CREATE_CHARGING_TRICKLE 3
#define CREATE_CHARGING_WAITING 4
#define CREATE_CHARGING_ERROR 5
#define CREATE_AXLE_LENGTH 0.258
#define CREATE_DIAMETER 0.33
#define CREATE_BUMPER_XOFFSET 0.05
#ifndef MIN
#define MIN(a,b) ((a < b) ? (a) : (b))
#endif
#ifndef MAX
#define MAX(a,b) ((a > b) ? (a) : (b))
#endif
#ifndef NORMALIZE
#define NORMALIZE(z) atan2(sin(z), cos(z))
#endif
typedef struct
{
/* Serial port to which the robot is connected */
char serial_port[PATH_MAX];
/* File descriptor associated with serial connection (-1 if no valid
* connection) */
int fd;
/* Current operation mode; one of CREATE_MODE_* */
unsigned char mode;
/* Integrated odometric position [m m rad] */
double ox, oy, oa;
/* Various Boolean flags */
int bumper_left, bumper_right;
unsigned char wheeldrop_caster, wheeldrop_left, wheeldrop_right;
unsigned char wall;
unsigned char cliff_left, cliff_frontleft, cliff_frontright, cliff_right;
unsigned char virtual_wall;
unsigned char overcurrent_driveleft, overcurrent_driveright;
unsigned char overcurrent_mainbrush, overcurrent_sidebrush;
unsigned char overcurrent_vacuum;
unsigned char dirtdetector_right, dirtdetector_left;
unsigned char remote_opcode;
unsigned char button_power, button_spot, button_clean, button_max;
/* One of CREATE_CHARGING_* */
unsigned char charging_state;
/* Volts */
double voltage;
/* Amps */
double current;
/* degrees C */
double temperature;
/* Ah */
double charge;
/* Capacity */
double capacity;
} create_comm_t;
create_comm_t* create_create(const char* serial_port);
void create_destroy(create_comm_t* r);
int create_open(create_comm_t* r, unsigned char fullcontrol);
int create_init(create_comm_t* r, unsigned char fullcontrol);
int create_close(create_comm_t* r);
int create_set_speeds(create_comm_t* r, double tv, double rv);
int create_parse_sensor_packet(create_comm_t* r,
unsigned char* buf, size_t buflen);
int create_get_sensors(create_comm_t* r, int timeout);
void create_print(create_comm_t* r);
int create_set_song(create_comm_t* r, unsigned char songNumber,
unsigned char songLength, unsigned char *notes,
unsigned char *noteLengths);
int create_play_song(create_comm_t *r, unsigned char songNumber);
int create_vacuum(create_comm_t *r, int state);
int create_set_leds(create_comm_t *r, uint8_t dirt_detect, uint8_t max,
uint8_t clean, uint8_t spot, uint8_t status,
uint8_t power_color, uint8_t power_intensity );
int create_run_demo(create_comm_t *r, uint8_t num);
#ifdef __cplusplus
}
#endif
#endif
--- NEW FILE: Makefile.am ---
noinst_LTLIBRARIES =
if INCLUDE_CREATE
noinst_LTLIBRARIES += libcreate.la
endif
AM_CPPFLAGS = -Wall -I$(top_srcdir)
libcreate_la_SOURCES = create_comms.h create_comms.c create_driver.cc
--- NEW FILE: create_driver.cc ---
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2006 -
* Brian Gerkey
*
*
* 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
*
*/
/** @ingroup drivers */
/** @{ */
/** @defgroup driver_create create
@brief iRobot Create
Newer versions of the iRobot Create vaccum robot can be controlled by an
external computer over a serial line. This driver supports control of
these robots.
Note that the serial port on top of the Create operates at 5V, not the
RS232 standard of 12V. This means that you cannot just plug a plain
old serial cable between the Create and your PC's serial port. You need
to put a level-shifter in between them. Or you if have a computer that
exposes serial lines at "logic level," (e.g., the Gumstix), you can use
them directly. Check out <a href="http://www.irobot.com/hacker";>iRobot's
hacker site</a> for more information, including the pinout on the Create's
serial port. The <a href="http://create.pbwiki.com";>Create Wiki</a>
has a howto on building an appropriate serial cable.
@par Compile-time dependencies
- none
@par Provides
The create driver provides the following device interfaces:
- @ref interface_position2d
- This interface returns odometry data (PLAYER_POSITION2D_DATA_STATE),
and accepts velocity commands (PLAYER_POSITION2D_CMD_VEL).
- @ref interface_power
- This interface returns battery levels (PLAYER_POWER_DATA_STATE).
- @ref interface_bumper
- This interface returns bumper data (PLAYER_BUMPER_DATA_STATE).
- @ref interface_opaque
- This driver supports programming song, playing songs, setting the LEDs,
and running demo scripts.
- Play song data format in bytes: [0][song_number]
- Program song data format in bytes:
[1][song_number][length(n)][note_1][length_note_1]...[note_n][length_note_n].
- Set LEDS format in bytes:
[2][dirt_dectect(0/1)][max_bool(0/1)][clean(0/1)][spot(0/1)][status(0=off,1=red,2=green,3=amber)][power_color(0-255)][power_intensity(0-255)]
- Run a demo script in bytes: [3][demo number]
@par Supported configuration requests
- PLAYER_POSITION2D_REQ_GET_GEOM
- PLAYER_BUMPER_GET_GEOM
@par Configuration file options
- port (string)
- Default: "/dev/ttyS0"
- Serial port used to communicate with the robot.
- safe (integer)
- Default: 1
- Nonzero to keep the robot in "safe" mode (the robot will stop if
the wheeldrop or cliff sensors are triggered), zero for "full" mode
@par Example
@verbatim
driver
(
name "create"
provides ["position2d:0" "power:0" "bumper:0" "ir:0" "opaque:0"]
port "/dev/ttyS2"
safe 1
)
@endverbatim
@todo
- Add support for IRs, vacuum motors, etc.
- Recover from a cliff/wheeldrop sensor being triggered in safe mode;
the robot goes into passive mode when this happens, which right now
requires Player to be restarted
@author Brian Gerkey
*/
/** @} */
#include <unistd.h>
#include <stdlib.h>
#include <libplayercore/playercore.h>
#include "create_comms.h"
#define CYCLE_TIME_US 200000
class Create : public Driver
{
public:
Create(ConfigFile* cf, int section);
~Create();
int Setup();
int Shutdown();
// MessageHandler
int ProcessMessage(MessageQueue * resp_queue,
player_msghdr * hdr,
void * data);
private:
// Main function for device thread.
virtual void Main();
// Serial port where the create is
const char* serial_port;
// full control or not
bool safe;
player_devaddr_t position_addr;
player_devaddr_t power_addr;
player_devaddr_t bumper_addr;
player_devaddr_t ir_addr;
player_devaddr_t opaque_addr;
player_devaddr_t gripper_addr;
player_opaque_data_t *cpdata;
// The underlying create object
create_comm_t* create_dev;
};
// a factory creation function
Driver* Create_Init(ConfigFile* cf, int section)
{
return((Driver*)(new Create(cf, section)));
}
// a driver registration function
void Create_Register(DriverTable* table)
{
table->AddDriver("create", Create_Init);
}
Create::Create(ConfigFile* cf, int section)
: Driver(cf,section,true,PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
{
memset(&this->position_addr,0,sizeof(player_devaddr_t));
memset(&this->power_addr,0,sizeof(player_devaddr_t));
memset(&this->bumper_addr,0,sizeof(player_devaddr_t));
memset(&this->ir_addr,0,sizeof(player_devaddr_t));
memset(&this->opaque_addr,0,sizeof(player_devaddr_t));
//memset(&this->gripper_addr,0,sizeof(player_devaddr_t));
this->cpdata = (player_opaque_data_t*)malloc(sizeof(player_opaque_data_t));
// Do we create a position interface?
if(cf->ReadDeviceAddr(&(this->position_addr), section, "provides",
PLAYER_POSITION2D_CODE, -1, NULL) == 0)
{
if(this->AddInterface(this->position_addr) != 0)
{
this->SetError(-1);
return;
}
}
// Do we create a power interface?
if(cf->ReadDeviceAddr(&(this->power_addr), section, "provides",
PLAYER_POWER_CODE, -1, NULL) == 0)
{
if(this->AddInterface(this->power_addr) != 0)
{
this->SetError(-1);
return;
}
}
// Do we create a bumper interface?
if(cf->ReadDeviceAddr(&(this->bumper_addr), section, "provides",
PLAYER_BUMPER_CODE, -1, NULL) == 0)
{
if(this->AddInterface(this->bumper_addr) != 0)
{
this->SetError(-1);
return;
}
}
// Do we create a IR interface?
if(cf->ReadDeviceAddr(&(this->ir_addr), section, "provides",
PLAYER_IR_CODE, -1, NULL) == 0)
{
if(this->AddInterface(this->ir_addr) != 0)
{
this->SetError(-1);
return;
}
}
// Do we create a Opaque interface?
if(cf->ReadDeviceAddr(&(this->opaque_addr), section, "provides",
PLAYER_OPAQUE_CODE, -1, NULL) == 0)
{
if(this->AddInterface(this->opaque_addr) != 0)
{
this->SetError(-1);
return;
}
}
// Do we create a gripper interface?
if(cf->ReadDeviceAddr(&(this->gripper_addr), section, "provides",
PLAYER_GRIPPER_CODE, -1, NULL) == 0)
{
if(this->AddInterface(this->gripper_addr) != 0)
{
this->SetError(-1);
return;
}
}
this->serial_port = cf->ReadString(section, "port", "/dev/ttyS0");
this->safe = cf->ReadInt(section, "safe", 1);
this->create_dev = NULL;
}
Create::~Create()
{
free (this->cpdata);
}
int
Create::Setup()
{
this->create_dev = create_create(this->serial_port);
if(create_open(this->create_dev, !this->safe) < 0)
{
create_destroy(this->create_dev);
this->create_dev = NULL;
PLAYER_ERROR("failed to connect to create");
return(-1);
}
this->StartThread();
return(0);
}
int
Create::Shutdown()
{
this->StopThread();
if(create_close(this->create_dev))
{
PLAYER_ERROR("failed to close create connection");
}
create_destroy(this->create_dev);
this->create_dev = NULL;
return(0);
}
void
Create::Main()
{
for(;;)
{
this->ProcessMessages();
if(create_get_sensors(this->create_dev, -1) < 0)
{
PLAYER_ERROR("failed to get sensor data from create");
create_close(this->create_dev);
return;
}
////////////////////////////
// Update position2d data
player_position2d_data_t posdata;
memset(&posdata,0,sizeof(posdata));
posdata.pos.px = this->create_dev->ox;
posdata.pos.py = this->create_dev->oy;
posdata.pos.pa = this->create_dev->oa;
this->Publish(this->position_addr, NULL,
PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
(void*)&posdata, sizeof(posdata), NULL);
////////////////////////////
// Update power data
player_power_data_t powerdata;
memset(&powerdata,0,sizeof(powerdata));
powerdata.volts = this->create_dev->voltage;
powerdata.watts = this->create_dev->voltage * this->create_dev->current;
powerdata.joules = this->create_dev->charge;
powerdata.percent = 100.0 *
(this->create_dev->charge / this->create_dev->capacity);
powerdata.charging =
(this->create_dev->charging_state == CREATE_CHARGING_NOT) ? 0 : 1;
powerdata.valid = (PLAYER_POWER_MASK_VOLTS |
PLAYER_POWER_MASK_WATTS |
PLAYER_POWER_MASK_JOULES |
PLAYER_POWER_MASK_PERCENT |
PLAYER_POWER_MASK_CHARGING);
this->Publish(this->power_addr, NULL,
PLAYER_MSGTYPE_DATA, PLAYER_POWER_DATA_STATE,
(void*)&powerdata, sizeof(powerdata), NULL);
////////////////////////////
// Update bumper data
player_bumper_data_t bumperdata;
memset(&bumperdata,0,sizeof(bumperdata));
bumperdata.bumpers_count = 2;
bumperdata.bumpers[0] = this->create_dev->bumper_left;
bumperdata.bumpers[1] = this->create_dev->bumper_right;
this->Publish(this->bumper_addr, NULL,
PLAYER_MSGTYPE_DATA, PLAYER_BUMPER_DATA_STATE,
(void*)&bumperdata);
////////////////////////////
// Update IR data
player_ir_data_t irdata;
memset(&irdata,0,sizeof(irdata));
irdata.ranges_count = 11;
irdata.ranges[0] = (float)this->create_dev->wall;
irdata.ranges[1] = (float)this->create_dev->cliff_left;
irdata.ranges[2] = (float)this->create_dev->cliff_frontleft;
irdata.ranges[3] = (float)this->create_dev->cliff_frontright;
irdata.ranges[4] = (float)this->create_dev->cliff_right;
irdata.ranges[5] = (float)this->create_dev->virtual_wall;
irdata.ranges[6] = (float)this->create_dev->dirtdetector_right;
irdata.ranges[7] = (float)this->create_dev->dirtdetector_left;
irdata.ranges[8] = (float)this->create_dev->wheeldrop_caster;
irdata.ranges[9] = (float)this->create_dev->wheeldrop_left;
irdata.ranges[10] = (float)this->create_dev->wheeldrop_right;
this->Publish(this->ir_addr,NULL,
PLAYER_MSGTYPE_DATA, PLAYER_IR_DATA_RANGES,
(void*)&irdata);
////////////////////////////
// Update Gripper data
player_gripper_data_t gripperdata;
memset(&gripperdata,0,sizeof(gripperdata));
gripperdata.state=this->create_dev->overcurrent_vacuum;
gripperdata.beams=this->create_dev->dirtdetector_right+this->create_dev->dirtdetector_left;
this->Publish(this->gripper_addr, NULL,
PLAYER_MSGTYPE_DATA,
PLAYER_GRIPPER_DATA_STATE,
(void*) &gripperdata, sizeof(gripperdata), NULL);
////////////////////////////
// Update Opaque-Control data
memset(this->cpdata,0,sizeof(cpdata));
this->cpdata->data_count=5;
this->cpdata->data[0] = this->create_dev->button_max;
this->cpdata->data[1] = this->create_dev->button_clean;
this->cpdata->data[2] = this->create_dev->button_spot;
this->cpdata->data[3] = this->create_dev->button_power;
this->cpdata->data[4] = this->create_dev->remote_opcode;
this->Publish(this->opaque_addr,NULL,
PLAYER_MSGTYPE_DATA,PLAYER_OPAQUE_DATA_STATE,
(void*)this->cpdata, sizeof(*this->cpdata), NULL);
usleep(CYCLE_TIME_US);
}
}
int
Create::ProcessMessage(MessageQueue * resp_queue,
player_msghdr * hdr,
void * data)
{
if(Message::MatchMessage(hdr,
PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL,
this->position_addr))
{
// get and send the latest motor command
player_position2d_cmd_vel_t position_cmd;
position_cmd = *(player_position2d_cmd_vel_t*)data;
PLAYER_MSG2(2,"sending motor commands %f:%f",
position_cmd.vel.px,
position_cmd.vel.pa);
if(create_set_speeds(this->create_dev,
position_cmd.vel.px,
position_cmd.vel.pa) < 0)
{
PLAYER_ERROR("failed to set speeds to create");
}
return(0);
}
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_MOTOR_POWER,
this->position_addr))
{
this->Publish(this->position_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER);
return 0;
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_GET_GEOM,
this->position_addr))
{
/* Return the robot geometry. */
player_position2d_geom_t geom={{0}};
// Assume that it turns about its geometric center, so leave as zeros
geom.size.sl = CREATE_DIAMETER;
geom.size.sw = CREATE_DIAMETER;
this->Publish(this->position_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_GET_GEOM,
(void*)&geom, sizeof(geom), NULL);
return(0);
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
PLAYER_BUMPER_GET_GEOM,
this->bumper_addr))
{
player_bumper_geom_t geom;
geom.bumper_def_count = 2;
geom.bumper_def[0].pose.px = 0.0;
geom.bumper_def[0].pose.py = 0.0;
geom.bumper_def[0].pose.pa = 0.0;
geom.bumper_def[0].length = 0.0;
geom.bumper_def[0].radius = CREATE_DIAMETER/2.0;
geom.bumper_def[1].pose.px = 0.0;
geom.bumper_def[1].pose.py = 0.0;
geom.bumper_def[1].pose.pa = 0.0;
geom.bumper_def[1].length = 0.0;
geom.bumper_def[1].radius = CREATE_DIAMETER/2.0;
this->Publish(this->bumper_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_BUMPER_GET_GEOM,
(void*)&geom);
return(0);
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
PLAYER_IR_POSE,
this->ir_addr))
{
player_ir_pose poses;
poses.poses_count = 11;
// TODO: Fill in proper values
for (int i=0; i<11; i++)
{
poses.poses[i].px = 0.0;
poses.poses[i].py = 0.0;
poses.poses[i].pa = 0.0;
}
this->Publish(this->ir_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_IR_POSE,
(void*)&poses);
return(0);
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,
PLAYER_OPAQUE_CMD,
this->opaque_addr))
{
player_opaque_data_t opaque_data;
opaque_data = *(player_opaque_data_t*)data;
// Play Command
if (opaque_data.data[0] == 0 )
{
uint8_t song_index;
song_index = opaque_data.data[1];
create_play_song(this->create_dev, song_index);
}
// Program song command
else if (opaque_data.data[0] == 1)
{
uint8_t index = opaque_data.data[1];
uint8_t length = opaque_data.data[2];
uint8_t notes[length];
uint8_t note_lengths[length];
for (unsigned int i=0; i<length; i++)
{
notes[i] = opaque_data.data[3+i*2];
note_lengths[i] = opaque_data.data[4+i*2];
}
create_set_song(this->create_dev, index, length,
notes, note_lengths);
}
// Set the LEDs
else if (opaque_data.data[0] == 2)
{
uint8_t dirt_detect = opaque_data.data[1] == 0 ? 0 : 1;
uint8_t max = opaque_data.data[2] == 0 ? 0 : 1;
uint8_t clean = opaque_data.data[3] == 0 ? 0 : 1;
uint8_t spot = opaque_data.data[4] == 0 ? 0 : 1;
uint8_t status = opaque_data.data[5];
uint8_t power_color = opaque_data.data[6];
uint8_t power_intensity = opaque_data.data[7];
if (status > 3)
status = 3;
if (create_set_leds(this->create_dev, dirt_detect, max, clean, spot,
status, power_color, power_intensity) < 0)
{
PLAYER_ERROR("failed to set create leds");
return -1;
}
}
// Run a demo script
else if (opaque_data.data[0] == 3)
{
uint8_t demo_num = opaque_data.data[1];
if (create_run_demo(this->create_dev, demo_num)<0)
{
PLAYER_ERROR("failed to run create demo");
return -1;
}
}
return 0;
}
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_GRIPPER_CMD_STATE,
this->gripper_addr))
{
player_gripper_cmd_t* gcmd = (player_gripper_cmd_t*)data;
if(gcmd->cmd)
create_vacuum(this->create_dev,7);
else
create_vacuum(this->create_dev,0);
return 0;
}
else
{
return(-1);
}
}
-------------------------------------------------------------------------
SF.Net email is sponsored by:
Check out the new SourceForge.net Marketplace.
It's the best place to buy or sell services for
just about anything Open Source.
http://sourceforge.net/services/buy/index.php
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
