Revision: 9067
http://playerstage.svn.sourceforge.net/playerstage/?rev=9067&view=rev
Author: jpgr87
Date: 2011-07-24 18:34:54 +0000 (Sun, 24 Jul 2011)
Log Message:
-----------
Small fixes to roomba 500 odometry, fixes to roomba and create set/reset
odometry commands
Modified Paths:
--------------
code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc
code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c
code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.h
code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc
Modified: code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc
2011-07-23 00:00:51 UTC (rev 9066)
+++ code/player/trunk/server/drivers/mixed/irobot/create/create_driver.cc
2011-07-24 18:34:54 UTC (rev 9067)
@@ -140,9 +140,6 @@
// full control or not
bool safe;
-
- // offsets for position2d odom
- double x_offset, y_offset, a_offset;
player_devaddr_t position_addr;
player_devaddr_t power_addr;
@@ -251,9 +248,6 @@
this->serial_port = cf->ReadString(section, "port", "/dev/ttyS0");
this->safe = cf->ReadInt(section, "safe", 1);
this->create_dev = NULL;
- this->x_offset = 0;
- this->y_offset = 0;
- this->a_offset = 0;
}
Create::~Create()
@@ -307,9 +301,9 @@
player_position2d_data_t posdata;
memset(&posdata,0,sizeof(posdata));
- posdata.pos.px = this->create_dev->ox - this->x_offset;
- posdata.pos.py = this->create_dev->oy - this->y_offset;
- posdata.pos.pa = this->create_dev->oa - this->a_offset;
+ 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,
PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,
@@ -469,9 +463,9 @@
{
player_position2d_data_t *positionreq = (player_position2d_data_t*)data;
- this->x_offset = this->create_dev->ox - positionreq->pos.px;
- this->y_offset = this->create_dev->oy - positionreq->pos.py;
- this->a_offset = this->create_dev->oa - positionreq->pos.pa;
+ this->create_dev->ox = positionreq->pos.px;
+ this->create_dev->oy = positionreq->pos.py;
+ this->create_dev->oa = positionreq->pos.pa;
return 0;
}
@@ -479,9 +473,9 @@
PLAYER_POSITION2D_REQ_RESET_ODOM,
this->position_addr))
{
- this->x_offset = this->create_dev->ox;
- this->y_offset = this->create_dev->oy;
- this->a_offset = this->create_dev->oa;
+ this->create_dev->ox = 0;
+ this->create_dev->oy = 0;
+ this->create_dev->oa = 0;
return 0;
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
Modified: code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c
===================================================================
--- code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c
2011-07-23 00:00:51 UTC (rev 9066)
+++ code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c
2011-07-24 18:34:54 UTC (rev 9067)
@@ -41,7 +41,7 @@
#include "roomba_comms.h"
roomba_comm_t*
-roomba_create(const char* serial_port)
+roomba_create(const char* serial_port, unsigned int roomba_type)
{
roomba_comm_t* r;
@@ -50,6 +50,7 @@
r->fd = -1;
r->mode = ROOMBA_MODE_OFF;
strncpy(r->serial_port,serial_port,sizeof(r->serial_port)-1);
+ r->roomba_type = roomba_type;
return(r);
}
@@ -60,7 +61,7 @@
}
int
-roomba_open(roomba_comm_t* r, unsigned char fullcontrol, int roomba500)
+roomba_open(roomba_comm_t* r, unsigned char fullcontrol)
{
struct termios term;
int flags;
@@ -99,7 +100,7 @@
cfmakeraw(&term);
- if (roomba500)
+ if (r->roomba_type == ROOMBA_500)
{
cfsetispeed(&term, B115200);
cfsetospeed(&term, B115200);
@@ -398,15 +399,31 @@
r->button_spot = (flag >> 2) & 0x01;
r->button_power = (flag >> 3) & 0x01;
+ // Distance (in mm) since last poll
memcpy(&signed_int, buf+idx, 2);
idx += 2;
signed_int = (int16_t)ntohs((uint16_t)signed_int);
- dist = signed_int / 1e3;
+ if (r->roomba_type == ROOMBA_DISCOVERY)
+ {
+ dist = (double)signed_int / 1.0e3;
+ }
+ else // roomba500 readings come in backwards, and off by a factor of 10
maybe?
+ {
+ dist = (double)signed_int / 1.0e3 * -10.0;
+ }
+ // Angle since last reading
memcpy(&signed_int, buf+idx, 2);
idx += 2;
signed_int = (int16_t)ntohs((uint16_t)signed_int);
- angle = (2.0 * (signed_int / 1e3)) / ROOMBA_AXLE_LENGTH;
+ if (r->roomba_type == ROOMBA_DISCOVERY) // Difference between wheel
readings, in mm
+ {
+ angle = (2.0 * ((double)signed_int / 1.0e3)) / ROOMBA_AXLE_LENGTH;
+ }
+ else // Angle, in degrees
+ {
+ angle = (double)signed_int * M_PI / 180.0;
+ }
/* First-order odometric integration */
r->oa = NORMALIZE(r->oa + angle);
Modified: code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.h
===================================================================
--- code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.h
2011-07-23 00:00:51 UTC (rev 9066)
+++ code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.h
2011-07-24 18:34:54 UTC (rev 9067)
@@ -72,6 +72,9 @@
#define ROOMBA_BUMPER_XOFFSET 0.05
+#define ROOMBA_DISCOVERY 0
+#define ROOMBA_500 1
+
#ifndef MIN
#define MIN(a,b) ((a < b) ? (a) : (b))
#endif
@@ -93,6 +96,8 @@
unsigned char mode;
/* Integrated odometric position [m m rad] */
double ox, oy, oa;
+ /* Roomba type (there are differences between roomba discovery & 500
interfaces) */
+ unsigned int roomba_type;
/* Various Boolean flags */
int bumper_left, bumper_right;
@@ -121,9 +126,9 @@
double capacity;
} roomba_comm_t;
-roomba_comm_t* roomba_create(const char* serial_port);
+roomba_comm_t* roomba_create(const char* serial_port, unsigned int
roomba_type);
void roomba_destroy(roomba_comm_t* r);
-int roomba_open(roomba_comm_t* r, unsigned char fullcontrol, int roomba500);
+int roomba_open(roomba_comm_t* r, unsigned char fullcontrol);
int roomba_init(roomba_comm_t* r, unsigned char fullcontrol);
int roomba_close(roomba_comm_t* r);
int roomba_set_speeds(roomba_comm_t* r, double tv, double rv);
Modified: code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc
2011-07-23 00:00:51 UTC (rev 9066)
+++ code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc
2011-07-24 18:34:54 UTC (rev 9067)
@@ -154,9 +154,6 @@
bool bumplocked;
bool roomba500;
-
- // Offsets for setting & resetting position2d odometry
- double x_offset, y_offset, a_offset;
player_devaddr_t position_addr;
player_devaddr_t power_addr;
@@ -269,18 +266,18 @@
this->bumplock = cf->ReadInt(section, "bumplock", 0);
this->roomba500 = cf->ReadBool(section, "roomba500", 0);
this->bumplocked = false;
- this->x_offset = 0;
- this->y_offset = 0;
- this->a_offset = 0;
this->roomba_dev = NULL;
}
int
Roomba::MainSetup()
{
- this->roomba_dev = roomba_create(this->serial_port);
+ if (this->roomba500)
+ this->roomba_dev = roomba_create(this->serial_port, ROOMBA_500);
+ else
+ this->roomba_dev = roomba_create(this->serial_port, ROOMBA_DISCOVERY);
- if(roomba_open(this->roomba_dev, !this->safe, this->roomba500) < 0)
+ if(roomba_open(this->roomba_dev, !this->safe) < 0)
{
roomba_destroy(this->roomba_dev);
this->roomba_dev = NULL;
@@ -339,9 +336,9 @@
player_position2d_data_t posdata;
memset(&posdata,0,sizeof(posdata));
- posdata.pos.px = this->roomba_dev->ox - this->x_offset;
- posdata.pos.py = this->roomba_dev->oy - this->y_offset;
- posdata.pos.pa = this->roomba_dev->oa - this->a_offset;
+ posdata.pos.px = this->roomba_dev->ox;
+ posdata.pos.py = this->roomba_dev->oy;
+ posdata.pos.pa = this->roomba_dev->oa;
posdata.stall = static_cast<uint8_t>(this->bumplocked);
this->Publish(this->position_addr,
@@ -533,9 +530,9 @@
{
player_position2d_data_t *positionreq = (player_position2d_data_t*)data;
- this->x_offset = this->roomba_dev->ox - positionreq->pos.px;
- this->y_offset = this->roomba_dev->oy - positionreq->pos.py;
- this->a_offset = this->roomba_dev->oa - positionreq->pos.pa;
+ this->roomba_dev->ox = positionreq->pos.px;
+ this->roomba_dev->oy = positionreq->pos.py;
+ this->roomba_dev->oa = positionreq->pos.pa;
return 0;
}
@@ -543,9 +540,9 @@
PLAYER_POSITION2D_REQ_RESET_ODOM,
this->position_addr))
{
- this->x_offset = this->roomba_dev->ox;
- this->y_offset = this->roomba_dev->oy;
- this->a_offset = this->roomba_dev->oa;
+ this->roomba_dev->ox = 0;
+ this->roomba_dev->oy = 0;
+ this->roomba_dev->oa = 0;
return 0;
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Magic Quadrant for Content-Aware Data Loss Prevention
Research study explores the data loss prevention market. Includes in-depth
analysis on the changes within the DLP market, and the criteria used to
evaluate the strengths and weaknesses of these DLP solutions.
http://www.accelacomm.com/jaw/sfnl/114/51385063/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
