Update of /cvsroot/playerstage/code/player/server/drivers/mixed/erratic
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv762/server/drivers/mixed/erratic
Modified Files:
Tag: release-2-0-patches
Makefile.am erratic.cc erratic.h packet.cc packet.h
robot_params.cc robot_params.h
Added Files:
Tag: release-2-0-patches
motorpacket.cc motorpacket.h
Log Message:
merged various changes from HEAD
Index: erratic.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.cc,v
retrieving revision 1.1.2.2
retrieving revision 1.1.2.3
diff -C2 -d -r1.1.2.2 -r1.1.2.3
*** erratic.cc 7 Jun 2006 16:12:50 -0000 1.1.2.2
--- erratic.cc 22 Sep 2006 20:46:43 -0000 1.1.2.3
***************
*** 2,5 ****
--- 2,10 ----
/**
+ * Copyright (C) 2006
+ * Videre Design
+ * Copyright (C) 2000
+ * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
+ *
* Videre Erratic robot driver for Player
*
***************
*** 20,35 ****
/**
! * This driver is adapted from the p2os driver of player 1.6. Major changes
! * throughout, as well as removed many uneccessary interfaces. In
particular,
! * communication with robot is threaded and fully responsive.
**/
// This must be first per pthread reference
#include <pthread.h>
- #ifdef HAVE_CONFIG_H
- #include "config.h"
- #endif
-
#include <fcntl.h>
#include <stdio.h>
--- 25,165 ----
/**
! * This driver is adapted from the p2os driver of player 1.6.
**/
+ /** @ingroup drivers */
+ /** @{ */
+ /** @defgroup driver_erratic erratic
+ * @brief Erratic
+
+ This driver talks to the embedded computer in the Erratic robot, which
+ mediates communication to the devices of the robot.
+
+ @par Compile-time dependencies
+
+ - none
+
+ @par Provides
+
+ The erratic driver provides the following device interfaces, some of
+ them named:
+
+ - "odometry" @ref interface_position2d
+ - This interface returns odometry data, and accepts velocity commands.
+
+ - @ref interface_power
+ - Returns the current battery voltage (12 V when fully charged).
+
+ - @ref interface_aio
+ - Returns data from analog input pins
+
+ - @ref interface_ir
+ - Returns ranges from IR sensors, assuming they're connected to the analog
input pins
+
+ @par Supported configuration requests
+
+ - "odometry" @ref interface_position2d :
+ - PLAYER_POSITION_SET_ODOM_REQ
+ - PLAYER_POSITION_MOTOR_POWER_REQ
+ - PLAYER_POSITION_RESET_ODOM_REQ
+ - PLAYER_POSITION_GET_GEOM_REQ
+ - PLAYER_POSITION_VELOCITY_MODE_REQ
+ - @ref interface_ir :
+ - PLAYER_SONAR_GET_GEOM_REQ
+
+ @par Configuration file options
+
+ - port (string)
+ - Default: "/dev/ttyS0"
+ - direct_wheel_vel_control (integer)
+ - Default: 0
+ - Send direct wheel velocity commands to Erratic (as opposed to sending
+ translational and rotational velocities and letting Erratic smoothly
+ achieve them).
+ - max_trans_vel (length)
+ - Default: 0.5 m/s
+ - Maximum translational velocity
+ - max_rot_vel (angle)
+ - Default: 100 deg/s
+ - Maximum rotational velocity
+ - trans_acc (length)
+ - Default: 0
+ - Maximum translational acceleration, in length/sec/sec; nonnegative.
+ Zero means use the robot's default value.
+ - trans_decel (length)
+ - Default: trans_acc
+ - Maximum translational deceleration, in length/sec/sec; nonpositive.
+ Zero means use the robot's default value.
+ - rot_acc (angle)
+ - Default: 0
+ - Maximum rotational acceleration, in angle/sec/sec; nonnegative.
+ Zero means use the robot's default value.
+ - rot_decel (angle)
+ - Default: rot_acc
+ - Maximum rotational deceleration, in angle/sec/sec; nonpositive.
+ Zero means use the robot's default value.
+ - pid_trans_p (integer)
+ - Default: -1
+ - Translational PID setting; proportional gain.
+ Negative means use the robot's default value.
+ - pid_trans_d (integer)
+ - Default: -1
+ - Translational PID setting; derivative gain.
+ Negative means use the robot's default value.
+ - pid_rot_p (integer)
+ - Default: -1
+ - Rotational PID setting; proportional gain.
+ Negative means use the robot's default value.
+ - pid_rot_d (integer)
+ - Default: -1
+ - Rotational PID setting; derivative gain.
+ Negative means use the robot's default value.
+ - motor_pwm_frequency (integer)
+ - Default: -1
+ - Frequency of motor PWM.
+ Bounds determined by robot.
+ Negative means use the robot's default value.
+ - motor_pwm_max_on (float)
+ - Default: 1
+ - Maximum motor duty cycle.
+ - save_settings_in_robot (integer)
+ - Default: 0
+ - A value of 1 installs current settings as default values in the robot.
+
+
+ @par Example
+
+ @verbatim
+ driver
+ (
+ name "erratic"
+ plugin "erratic"
+
+ provides [ "position2d:0"
+ "power:0"
+ "aio:0"
+ "ir:0" ]
+
+ port "/dev/ttyUSB0"
+
+ max_trans_vel 3
+ max_rot_vel 720
+
+ trans_acc 1
+ rot_acc 200
+
+ direct_wheel_vel_control 0
+ )
+ @endverbatim
+
+ @author Joakim Arfvidsson, Brian Gerkey, Kasper Stoy, James McKenna
+ */
+ /** @} */
+
+
+
// This must be first per pthread reference
#include <pthread.h>
#include <fcntl.h>
#include <stdio.h>
***************
*** 40,43 ****
--- 170,177 ----
#include "erratic.h"
+ #ifdef HAVE_CONFIG_H
+ #include "config.h"
+ #endif
+
bool debug_mode = FALSE;
***************
*** 53,57 ****
table->AddDriver("erratic", Erratic_Init);
}
-
#if 0
extern "C" {
--- 187,190 ----
***************
*** 77,81 ****
this->position_subscriptions = 0;
! this->aio_subscriptions = 0;
// intialise members
--- 210,214 ----
this->position_subscriptions = 0;
! this->aio_ir_subscriptions = 0;
// intialise members
***************
*** 106,109 ****
--- 239,250 ----
}
+ // Do we create an ir interface?
+ if(cf->ReadDeviceAddr(&(this->ir_id), section, "provides",
PLAYER_IR_CODE, -1, NULL) == 0) {
+ if(this->AddInterface(this->ir_id) != 0) {
+ this->SetError(-1);
+ return;
+ }
+ }
+
// build the table of robot parameters.
initialize_robot_params();
***************
*** 328,333 ****
param_idx = 0;
! // Create a packet and set initial odometry position (the SIP is
persistent)
! this->motor_packet = new erSIP(param_idx);
this->motor_packet->x_offset = 0;
this->motor_packet->y_offset = 0;
--- 469,474 ----
param_idx = 0;
! // Create a packet and set initial odometry position (the
ErraticMotorPacket is persistent)
! this->motor_packet = new ErraticMotorPacket(param_idx);
this->motor_packet->x_offset = 0;
this->motor_packet->y_offset = 0;
***************
*** 580,584 ****
if(Device::MatchDeviceAddress(id, this->aio_id))
! this->aio_subscriptions++;
}
--- 721,728 ----
if(Device::MatchDeviceAddress(id, this->aio_id))
! this->aio_ir_subscriptions++;
!
! if(Device::MatchDeviceAddress(id, this->ir_id))
! this->aio_ir_subscriptions++;
}
***************
*** 599,603 ****
if(Device::MatchDeviceAddress(id, this->aio_id))
! this->aio_subscriptions--;
}
--- 743,750 ----
if(Device::MatchDeviceAddress(id, this->aio_id))
! this->aio_ir_subscriptions--;
!
! if(Device::MatchDeviceAddress(id, this->ir_id))
! this->aio_ir_subscriptions--;
}
***************
*** 621,625 ****
while ((error_code = packet.Receive(this->read_fd, 5000))) {
waited += 5;
! printf("Lost serial communication with Erratic - no
data received for %i seconds\n", waited);
}
--- 768,772 ----
while ((error_code = packet.Receive(this->read_fd, 5000))) {
waited += 5;
! printf("Lost serial communication with Erratic (%d) -
no data received for %i seconds\n", error_code, waited);
}
***************
*** 650,659 ****
break;
case (reply_e)ain:
erratic_data.aio.voltages_count =
packet.packet[4];
for (unsigned int i_voltage = 0; i_voltage <
erratic_data.aio.voltages_count ;i_voltage++) {
erratic_data.aio.voltages[i_voltage] =
(packet.packet[5+i_voltage*2]
! +
256*packet.packet[6+i_voltage*2]) * (1.0 / 1024.0);
}
PublishAIn();
break;
case (reply_e)debug:
--- 797,813 ----
break;
case (reply_e)ain:
+ // This data goes in two places, analog input
and ir rangers
erratic_data.aio.voltages_count =
packet.packet[4];
+ erratic_data.ir.voltages_count =
RobotParams[this->param_idx]->NumIR;
+ erratic_data.ir.ranges_count = 2;
for (unsigned int i_voltage = 0; i_voltage <
erratic_data.aio.voltages_count ;i_voltage++) {
erratic_data.aio.voltages[i_voltage] =
(packet.packet[5+i_voltage*2]
! +
256*packet.packet[6+i_voltage*2]) * (1.0 / 1024.0) * CPU_VOLTAGE;
! erratic_data.ir.voltages[i_voltage] =
(packet.packet[5+i_voltage*2]
! +
256*packet.packet[6+i_voltage*2]) * (1.0 / 1024.0) * CPU_VOLTAGE;
! erratic_data.ir.ranges[i_voltage] =
IRRangeFromVoltage(erratic_data.ir.voltages[i_voltage]);
}
PublishAIn();
+ PublishIR();
break;
case (reply_e)debug:
***************
*** 703,707 ****
if (packet) {
if (print_all_packets) {
! printf("Just about to send:\n");
packet->Print();
}
--- 857,861 ----
if (packet) {
if (print_all_packets) {
! printf("Just about to send: ");
packet->Print();
}
***************
*** 768,772 ****
ErraticPacket *packet = new ErraticPacket();
! command[0] = (command_e)ain;
command[1] = (argtype_e)argint;
command[2] = val ? 1 : 0;
--- 922,926 ----
ErraticPacket *packet = new ErraticPacket();
! command[0] = (command_e)set_analog;
command[1] = (argtype_e)argint;
command[2] = val ? 1 : 0;
***************
*** 777,780 ****
--- 931,939 ----
}
+ // This describes the IR hardware
+ float Erratic::IRRangeFromVoltage(float voltage) {
+ // This is values for the Sharp 2Y0A02, 150 cm ranger
+ return -.2475 + .1756 * voltage + .7455 / voltage - .0446 * voltage *
voltage;
+ }
***************
*** 784,788 ****
void Erratic::Main() {
int last_position_subscrcount=0;
! int last_aio_subscriptions=0;
for(;;)
--- 943,947 ----
void Erratic::Main() {
int last_position_subscrcount=0;
! int last_aio_ir_subscriptions=0;
for(;;)
***************
*** 810,821 ****
last_position_subscrcount = this->position_subscriptions;
// We'll ask the robot to enable analog packets if we just got
our
// first subscriber
! if(!last_aio_subscriptions && this->aio_subscriptions)
this->ToggleAIn(1);
! else if(last_aio_subscriptions && !(this->aio_subscriptions))
this->ToggleAIn(0);
! last_aio_subscriptions = this->aio_subscriptions;
!
--- 969,980 ----
last_position_subscrcount = this->position_subscriptions;
+
// We'll ask the robot to enable analog packets if we just got
our
// first subscriber
! if(!last_aio_ir_subscriptions && this->aio_ir_subscriptions)
this->ToggleAIn(1);
! else if(last_aio_ir_subscriptions &&
!(this->aio_ir_subscriptions))
this->ToggleAIn(0);
! last_aio_ir_subscriptions = this->aio_ir_subscriptions;
***************
*** 867,898 ****
NULL);
}
!
! // This publishes all data we have (this is dumb)
! void Erratic::PublishAllData() {
! // TODO: something smarter about timestamping.
!
! // put odometry data
! this->Publish(this->position_id, NULL,
! PLAYER_MSGTYPE_DATA,
! PLAYER_POSITION2D_DATA_STATE,
! (void*)&(this->erratic_data.position),
! sizeof(player_position2d_data_t),
! NULL);
!
! // put power data
! this->Publish(this->power_id, NULL,
PLAYER_MSGTYPE_DATA,
! PLAYER_POWER_DATA_STATE,
! (void*)&(this->erratic_data.power),
! sizeof(player_power_data_t),
NULL);
!
! // put aio data
! this->Publish(this->aio_id, NULL,
! PLAYER_MSGTYPE_DATA,
! PLAYER_AIO_DATA_STATE,
! (void*)&(this->erratic_data.aio),
! sizeof(player_aio_data_t),
! NULL);
}
--- 1026,1042 ----
NULL);
}
! void Erratic::PublishIR() {
! this->Publish(this->ir_id, NULL,
PLAYER_MSGTYPE_DATA,
! PLAYER_IR_DATA_RANGES,
! (void*)&(this->erratic_data.ir),
! sizeof(player_ir_data_t),
NULL);
! }
! void Erratic::PublishAllData() {
! this->PublishPosition2D();
! this->PublishPower();
! this->PublishAIn();
! this->PublishIR();
}
***************
*** 913,918 ****
// check for position config requests
if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
! PLAYER_POSITION2D_REQ_SET_ODOM,
! this->position_id))
{
if(hdr->size != sizeof(player_position2d_set_odom_req_t))
--- 1057,1062 ----
// check for position config requests
if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
! PLAYER_POSITION2D_REQ_SET_ODOM,
! this->position_id))
{
if(hdr->size != sizeof(player_position2d_set_odom_req_t))
***************
*** 924,948 ****
(player_position2d_set_odom_req_t*)data;
! this->motor_packet->x_offset =
((int)rint(set_odom_req->pose.px*1e3)) -
! this->motor_packet->xpos;
! this->motor_packet->y_offset =
((int)rint(set_odom_req->pose.py*1e3)) -
! this->motor_packet->ypos;
! this->motor_packet->angle_offset =
((int)rint(RTOD(set_odom_req->pose.pa))) -
! this->motor_packet->angle;
this->Publish(this->position_id, resp_queue,
! PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_SET_ODOM);
return(0);
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
! PLAYER_POSITION2D_REQ_MOTOR_POWER,
! this->position_id))
! {
! /* motor state change request
! * 1 = enable motors
! * 0 = disable motors (default)
! */
! if(hdr->size !=
sizeof(player_position2d_power_config_t))
! {
PLAYER_WARN("Arg to motor state change request wrong
size; ignoring");
return(-1);
--- 1068,1086 ----
(player_position2d_set_odom_req_t*)data;
! this->motor_packet->x_offset =
((int)rint(set_odom_req->pose.px*1e3))
! - this->motor_packet->xpos;
! this->motor_packet->y_offset =
((int)rint(set_odom_req->pose.py*1e3))
! - this->motor_packet->ypos;
! this->motor_packet->angle_offset =
((int)rint(RTOD(set_odom_req->pose.pa)))
! - this->motor_packet->angle;
this->Publish(this->position_id, resp_queue,
! PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_SET_ODOM);
return(0);
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
! PLAYER_POSITION2D_REQ_MOTOR_POWER,
! this->position_id)) {
! if(hdr->size != sizeof(player_position2d_power_config_t)) {
PLAYER_WARN("Arg to motor state change request wrong
size; ignoring");
return(-1);
***************
*** 953,969 ****
this->Publish(this->position_id, resp_queue,
! PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_MOTOR_POWER);
return(0);
! }
! else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
! PLAYER_POSITION2D_REQ_RESET_ODOM,
! this->position_id))
! {
! /* reset position to 0,0,0: no args */
! if(hdr->size != 0)
! {
PLAYER_WARN("Arg to reset position request is wrong
size; ignoring");
return(-1);
}
ResetRawPositions();
--- 1091,1105 ----
this->Publish(this->position_id, resp_queue,
! PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_POSITION2D_REQ_MOTOR_POWER);
return(0);
! } else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
! PLAYER_POSITION2D_REQ_RESET_ODOM,
! this->position_id)) {
! if(hdr->size != 0) {
PLAYER_WARN("Arg to reset position request is wrong
size; ignoring");
return(-1);
}
+
ResetRawPositions();
***************
*** 973,1010 ****
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
! PLAYER_POSITION2D_REQ_GET_GEOM,
! this->position_id))
! {
/* Return the robot geometry. */
! if(hdr->size != 0)
! {
PLAYER_WARN("Arg get robot geom is wrong size;
ignoring");
return(-1);
}
player_position2d_geom_t geom;
! // TODO: Figure out this rotation offset somehow; it's not
! // given in the Saphira parameters. For now, -0.1 is
! // about right for a Pioneer 2DX.
! geom.pose.px = -0.1;
geom.pose.py = 0.0;
geom.pose.pa = 0.0;
! // get dimensions from the parameter table
geom.size.sl = RobotParams[param_idx]->RobotLength / 1e3;
geom.size.sw = RobotParams[param_idx]->RobotWidth / 1e3;
this->Publish(this->position_id, 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_POSITION2D_REQ_VELOCITY_MODE,
! this->position_id))
! {
/* velocity control mode:
! * 0 = direct wheel velocity control (default)
! * 1 = separate translational and rotational control
! */
if(hdr->size !=
sizeof(player_position2d_velocity_mode_config_t))
{
--- 1109,1141 ----
}
else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
! PLAYER_POSITION2D_REQ_GET_GEOM,
! this->position_id)) {
/* Return the robot geometry. */
! if(hdr->size != 0) {
PLAYER_WARN("Arg get robot geom is wrong size;
ignoring");
return(-1);
}
player_position2d_geom_t geom;
!
! geom.pose.px = -RobotParams[param_idx]->RobotAxleOffset / 1e3;
geom.pose.py = 0.0;
geom.pose.pa = 0.0;
!
geom.size.sl = RobotParams[param_idx]->RobotLength / 1e3;
geom.size.sw = RobotParams[param_idx]->RobotWidth / 1e3;
this->Publish(this->position_id, 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_POSITION2D_REQ_VELOCITY_MODE,
! this->position_id)) {
/* velocity control mode:
! * 0 = direct wheel velocity control
! * 1 = separate translational and rotational control
! */
if(hdr->size !=
sizeof(player_position2d_velocity_mode_config_t))
{
***************
*** 1022,1026 ****
this->Publish(this->position_id, resp_queue,
! PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_VELOCITY_MODE);
return(0);
}
--- 1153,1174 ----
this->Publish(this->position_id, resp_queue,
! PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_POSITION2D_REQ_VELOCITY_MODE);
! return(0);
! }
!
! // Request for getting IR locations
! else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
! PLAYER_IR_POSE,
! this->ir_id)) {
! player_ir_pose_t pose;
! pose.poses_count = RobotParams[param_idx]->NumIR;
! for (uint16_t i = 0; i < pose.poses_count ;i++)
! pose.poses[i] = RobotParams[param_idx]->IRPose[i];
!
! this->Publish(this->ir_id, resp_queue,
! PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_IR_POSE,
! (void*)&pose, sizeof(pose), NULL);
return(0);
}
Index: erratic.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.h,v
retrieving revision 1.1.2.2
retrieving revision 1.1.2.3
diff -C2 -d -r1.1.2.2 -r1.1.2.3
*** erratic.h 7 Jun 2006 16:12:50 -0000 1.1.2.2
--- erratic.h 22 Sep 2006 20:46:43 -0000 1.1.2.3
***************
*** 2,7 ****
/**
! * Erratic Erratic robot driver for Player
! *
* 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
--- 2,12 ----
/**
! * Copyright (C) 2006
! * Videre Design
! * Copyright (C) 2000
! * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
! *
! * Videre Erratic robot driver for Player
! *
* 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
***************
*** 19,25 ****
**/
! #ifndef _P2OSDEVICE_H
! #define _P2OSDEVICE_H
#include <pthread.h>
--- 24,37 ----
**/
! #ifndef _ERRATICDEVICE_H
! #define _ERRATICDEVICE_H
+ #ifndef ERRATIC_VERSION
+ #define ERRATIC_VERSION "1.0b"
+ #endif
+
+ #ifndef ERRATIC_DATE
+ #define ERRATIC_DATE "2006-05-07"
+ #endif
#include <pthread.h>
***************
*** 35,41 ****
#include <stdint.h>
! // Version
! #define ERRATIC_VERSION "1.0b"
! #define ERRATIC_DATE "2006-05-07"
// Default max speeds
--- 47,51 ----
#include <stdint.h>
! #define CPU_VOLTAGE 3.5
// Default max speeds
***************
*** 105,116 ****
player_power_data_t power;
player_aio_data_t aio;
} __attribute__ ((packed)) player_erratic_data_t;
// this is here because we need the above typedef's before including it.
! #include "sip.h"
extern bool debug_mode;
! class erSIP;
class Erratic : public Driver
--- 115,127 ----
player_power_data_t power;
player_aio_data_t aio;
+ player_ir_data ir;
} __attribute__ ((packed)) player_erratic_data_t;
// this is here because we need the above typedef's before including it.
! #include "motorpacket.h"
extern bool debug_mode;
! class ErraticMotorPacket;
class Erratic : public Driver
***************
*** 122,131 ****
player_devaddr_t power_id;
player_devaddr_t aio_id;
int position_subscriptions;
! int aio_subscriptions;
! //erSIP* sippacket;
! erSIP *motor_packet;
pthread_mutex_t motor_packet_mutex;
--- 133,143 ----
player_devaddr_t power_id;
player_devaddr_t aio_id;
+ player_devaddr_t ir_id;
int position_subscriptions;
! int aio_ir_subscriptions;
! //ErraticMotorPacket* sippacket;
! ErraticMotorPacket *motor_packet;
pthread_mutex_t motor_packet_mutex;
***************
*** 133,138 ****
int Disconnect();
-
- int SendReceiveOLD(ErraticPacket* pkt, bool publish_data=true);
void ResetRawPositions();
void ToggleMotorPower(unsigned char val);
--- 145,148 ----
***************
*** 148,151 ****
--- 158,164 ----
void PublishPower();
void PublishAIn();
+ void PublishIR();
+
+ float IRRangeFromVoltage(float voltage);
void StartThreads();
***************
*** 158,166 ****
static void *ReceiveThreadDummy(void *driver);
- int direct_wheel_vel_control; // false -> separate trans and rot vel
-
int read_fd, write_fd;
const char* psos_serial_port;
- struct timeval lastblob_tv;
player_position2d_cmd_vel_t last_position_cmd;
--- 171,176 ----
***************
*** 175,178 ****
--- 185,190 ----
// Parameters
+ bool direct_wheel_vel_control;
+
bool print_all_packets;
bool print_status_summary;
Index: packet.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/packet.h,v
retrieving revision 1.1.2.2
retrieving revision 1.1.2.3
diff -C2 -d -r1.1.2.2 -r1.1.2.3
*** packet.h 7 Jun 2006 16:12:50 -0000 1.1.2.2
--- packet.h 22 Sep 2006 20:46:43 -0000 1.1.2.3
***************
*** 1,24 ****
! /*
! * Copyright (C) 2006
! * Videre Design
! * 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
! *
! */
#ifndef _PACKET_H
--- 1,25 ----
! /**
! * Copyright (C) 2006
! * Videre Design
! * Copyright (C) 2000
! * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
! *
! * Videre Erratic robot driver for Player
! *
! * 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 _PACKET_H
--- NEW FILE: motorpacket.h ---
/*
* 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
*
*/
/*
* $Id: motorpacket.h,v 1.1.2.1 2006/09/22 20:46:43 gerkey Exp $
*
* part of the P2OS parser. methods for filling and parsing server
* information packets (ErraticMotorPackets)
*/
#ifndef _ErraticMotorPacket_H
#define _ErraticMotorPacket_H
#include <limits.h>
#include "erratic.h"
class ErraticMotorPacket
{
private:
int PositionChange( unsigned short, unsigned short );
int param_idx; // index of our robot's data in the parameter table
public:
// these values are returned in every standard ErraticMotorPacket
bool lwstall, rwstall;
unsigned char status, battery;
unsigned short ptu, timer, rawxpos;
unsigned short rawypos;
short angle, lvel, rvel, control;
int xpos, ypos;
int x_offset,y_offset,angle_offset;
bool Parse( unsigned char *buffer, int length );
void Print();
void Fill(player_erratic_data_t* data);
ErraticMotorPacket(int idx)
{
param_idx = idx;
xpos = INT_MAX;
ypos = INT_MAX;
}
};
#endif
Index: packet.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/packet.cc,v
retrieving revision 1.1.2.2
retrieving revision 1.1.2.3
diff -C2 -d -r1.1.2.2 -r1.1.2.3
*** packet.cc 7 Jun 2006 16:12:50 -0000 1.1.2.2
--- packet.cc 22 Sep 2006 20:46:43 -0000 1.1.2.3
***************
*** 2,9 ****
/**
! * 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
--- 2,11 ----
/**
! * Copyright (C) 2006
! * Videre Design
* Copyright (C) 2000
* Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
! *
! * Videre Erratic robot driver for Player
*
* This program is free software; you can redistribute it and/or modify
***************
*** 28,31 ****
--- 30,35 ----
#include "packet.h"
#include <unistd.h>
+ #include <sys/select.h>
+ #include <sys/time.h>
#include <stdlib.h> /* for exit() */
//#include <sys/poll.h>
***************
*** 59,63 ****
if ( chksum == received_chksum )
! return(true);
if (debug_mode) {
--- 63,74 ----
if ( chksum == received_chksum )
! {
! if (debug_mode)
! {
! printf("Good packet: ");
! PrintHex();
! }
! return(true);
! }
if (debug_mode) {
***************
*** 90,100 ****
uint16_t cnt;
memset(packet,0,sizeof(packet));
struct pollfd readpoll;
! readpoll.fd = fd; readpoll.events = POLLIN | POLLERR | POLLHUP |
POLLNVAL; readpoll.revents = 0;
// This block will terminate or move on when there is data to read
if (wait) {
while (1) {
int8_t stuff = poll(&readpoll, 1, wait);
--- 101,144 ----
uint16_t cnt;
+ if (debug_mode)
+ printf("Check for packets in Receive()\n");
+
memset(packet,0,sizeof(packet));
struct pollfd readpoll;
! readpoll.fd = fd;
! readpoll.events = POLLIN | POLLPRI;
! readpoll.revents = 0;
+ fd_set read_set, error_set;
+ struct timeval tv;
+ #ifdef USE_SELECT
+ FD_ZERO(&read_set);
+ FD_ZERO(&error_set);
+ FD_SET(fd, &read_set);
+ FD_SET(fd, &error_set);
+ tv.tv_sec = 0;
+ tv.tv_usec = 0;
+ if (wait >= 1000)
+ tv.tv_sec = wait/1000;
+ else
+ tv.tv_usec = wait*1000;
+ #endif
+
// This block will terminate or move on when there is data to read
if (wait) {
while (1) {
+ #ifdef USE_SELECT
+ int ret = select(fd+1, &read_set, 0, &error_set, &tv);
+ if (ret)
+ {
+ if (debug_mode)
+ printf("Data waiting\n");
+ break;
+ }
+ else
+ if (debug_mode)
+ printf("No data\n");
+ #endif
+
int8_t stuff = poll(&readpoll, 1, wait);
***************
*** 126,133 ****
// Once we've started receiving a packet, we have a
tight timeout
while( cnt!=1 ) {
if (wait) {
while (1) {
! int8_t stuff = poll(&readpoll,
1, 3);
if (stuff < 0) {
--- 170,192 ----
// Once we've started receiving a packet, we have a
tight timeout
+ // Trouble with FTDI USB interface: needs longer timeout
while( cnt!=1 ) {
if (wait) {
while (1) {
!
! #ifdef USE_SELECT
! FD_ZERO(&read_set);
! FD_ZERO(&error_set);
! FD_SET(fd, &read_set);
! FD_SET(fd, &error_set);
! tv.tv_sec = 0;
! tv.tv_usec = 100*1000; // in
usec's
!
! int ret = select(fd+1,
&read_set, 0, &error_set, &tv);
! if (ret)
! break;
! #endif
!
! int8_t stuff = poll(&readpoll,
1, 100);
if (stuff < 0) {
***************
*** 152,155 ****
--- 211,216 ----
int newcnt = read( fd, &prefix[2], 1 );
+ if (debug_mode)
+ printf("Read %d byte: %02x\n", newcnt,
prefix[2]);
if (newcnt < 0 && errno == EAGAIN)
***************
*** 181,185 ****
if (wait) {
while (1) {
! int8_t stuff = poll(&readpoll, 1, 3);
if (stuff < 0) {
--- 242,259 ----
if (wait) {
while (1) {
! #ifdef USE_SELECT
! FD_ZERO(&read_set);
! FD_ZERO(&error_set);
! FD_SET(fd, &read_set);
! FD_SET(fd, &error_set);
! tv.tv_sec = 0;
! tv.tv_usec = 100*1000;
// in usec's
!
! int ret = select(fd+1, &read_set, 0,
&error_set, &tv);
! if (ret)
! break;
! #endif
!
! int8_t stuff = poll(&readpoll, 1, 100);
if (stuff < 0) {
***************
*** 203,206 ****
--- 277,290 ----
int newcnt = read( fd, &packet[3+cnt], prefix[2]-cnt );
+ if (debug_mode)
+ {
+ printf("Read %d bytes packet\n",
newcnt);
+ if (newcnt > 0)
+ {
+ for (int i=0; i<newcnt;
i++)
+ printf("%02x ",
packet[3+i]);
+ printf("\n");
+ }
+ }
if (newcnt < 0 && errno == EAGAIN)
--- NEW FILE: motorpacket.cc ---
// -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:1; -*-
/**
* Videre Erratic robot driver for Player
*
* 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 <stdio.h>
#include <limits.h>
#include <math.h> /* rint(3) */
#include <sys/types.h>
#include <netinet/in.h>
#include "motorpacket.h"
#include <stdlib.h> /* for abs() */
#include <unistd.h>
int ErraticMotorPacket::PositionChange( unsigned short from, unsigned short to
)
{
int diff1, diff2;
/* find difference in two directions and pick shortest */
if ( to > from ) {
diff1 = to - from;
diff2 = - ( from + 4096 - to );
}
else {
diff1 = to - from;
diff2 = 4096 - from + to;
}
if ( abs(diff1) < abs(diff2) )
return(diff1);
else
return(diff2);
}
void ErraticMotorPacket::Print()
{
printf("lwstall:%d rwstall:%d\n", lwstall, rwstall);
printf("status: 0x%x", status);
printf("battery: %d\n", battery);
printf("xpos: %d ypos:%d ptu:%hu timer:%hu\n", xpos, ypos, ptu, timer);
printf("angle: %d lvel: %d rvel: %d control: %d\n", angle, lvel, rvel,
control);
}
// Parses and absorbs a standard packet from the robot
bool ErraticMotorPacket::Parse( unsigned char *buffer, int length )
{
int cnt = 0, change;
unsigned short newxpos, newypos;
// Check type and length
if (length < 20) return false;
status = buffer[cnt];
cnt += sizeof(unsigned char);
// This driver does its own integration, and only cares for the lower bits of
the odometry updates
// Integers from robot are little-endian
newxpos = buffer[cnt] + 0x100*(buffer[cnt+1]&0x0F);
if (xpos!=INT_MAX) {
change = (int) rint(PositionChange( rawxpos, newxpos ) *
RobotParams[param_idx]->DistConvFactor);
//printf("xchange: %i ", change);
if (abs(change)>100)
;//PLAYER_WARN1("invalid odometry change [%d]; odometry values are
tainted", change);
else
xpos += change;
}
else {
//printf("xpos was INT_MAX\n");
xpos = 0;
}
rawxpos = newxpos;
cnt += 3;
newypos = buffer[cnt] + 0x100*(buffer[cnt+1]&0x0F);
if (ypos!=INT_MAX) {
change = (int) rint(PositionChange( rawypos, newypos ) *
RobotParams[param_idx]->DistConvFactor);
if (abs(change)>100)
;//PLAYER_WARN1("invalid odometry change [%d]; odometry values are
tainted", change);
else
ypos += change;
}
else
ypos = 0;
rawypos = newypos;
cnt += 3;
//if (debug_mode)
// printf("Just parsed, new xpos: %i ypos: %i\n", xpos, ypos);
angle = (short)
rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
M_PI/2048.0 * 180.0/M_PI);
cnt += sizeof(short);
lvel = (short)
rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
RobotParams[param_idx]->VelConvFactor);
cnt += sizeof(short);
rvel = (short)
rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
RobotParams[param_idx]->VelConvFactor);
cnt += sizeof(short);
battery = buffer[cnt];
cnt += sizeof(unsigned char);
lwstall = buffer[cnt] & 0x01;
cnt += sizeof(unsigned char);
rwstall = buffer[cnt] & 0x01;
cnt += sizeof(unsigned char);
control = (short)
rint(((short)(buffer[cnt] | (buffer[cnt+1] << 8))) *
RobotParams[param_idx]->AngleConvFactor);
cnt += sizeof(short);
return true;
}
// Spits out information that was previously parsed
void ErraticMotorPacket::Fill(player_erratic_data_t* data)
{
// Odometry data
{
// initialize position to current offset
data->position.pos.px = this->x_offset / 1e3;
data->position.pos.py = this->y_offset / 1e3;
// now transform current position by rotation if there is one
// and add to offset
if(this->angle_offset != 0)
{
double rot = DTOR(this->angle_offset); // convert
rotation to radians
data->position.pos.px += ((this->xpos/1e3) * cos(rot)
-
(this->ypos/1e3) * sin(rot));
data->position.pos.py += ((this->xpos/1e3) * sin(rot)
+
(this->ypos/1e3) * cos(rot));
data->position.pos.pa = DTOR(this->angle_offset +
angle);
}
else
{
data->position.pos.px += this->xpos / 1e3;
data->position.pos.py += this->ypos / 1e3;
data->position.pos.pa = DTOR(this->angle);
}
data->position.vel.px = (((this->lvel) + (this->rvel) ) / 2) /
1e3;
data->position.vel.py = 0.0;
data->position.vel.pa = (0.596*(double)(this->rvel -
this->lvel) /
(2.0/RobotParams[param_idx]->DiffConvFactor));
data->position.stall = (unsigned char)(this->lwstall ||
this->rwstall);
}
// Battery data
{
data->power.valid = PLAYER_POWER_MASK_VOLTS |
PLAYER_POWER_MASK_PERCENT;
data->power.volts = this->battery / 1e1;
data->power.percent = 1e2 * (data->power.volts /
VIDERE_NOMINAL_VOLTAGE);
}
}
Index: robot_params.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/robot_params.cc,v
retrieving revision 1.1.2.2
retrieving revision 1.1.2.3
diff -C2 -d -r1.1.2.2 -r1.1.2.3
*** robot_params.cc 7 Jun 2006 16:12:50 -0000 1.1.2.2
--- robot_params.cc 22 Sep 2006 20:46:43 -0000 1.1.2.3
***************
*** 27,85 ****
RobotParams_t erratic_params =
{
! 0.001534,
! "Videre",
! 0.011,
! 0.780, //This is the empirically correct value, but doesn't match wheel
size calculation
! 0,
! 1.626,
! 1,
! 1,
! 0,
! 0,
! 0,
! "",
! "",
! 0,
! 1,
! 0,
! 0,
! 0,
! 300,
! 1000,
! 0,
! 5,
! 5,
! 1,
! 0,
! 0,
! 0,
! 120,
! 330,
! 180,
! 279,
! 0,
! 0,
! 0,
! 1,
! 1,
! 8,
! "erratic",
! 38400,
! 0,
! 0,
! 0,
! 0,
! 20,
! 1.20482,
! {
! { 73, 105, 90 },
! { 130, 78, 41 },
! { 154, 30, 15 },
! { 154, -30, -15 },
! { 130, -78, -41 },
! { 73, -105, -90 },
! { -146, -60, -145 },
! { -146, 60, 145 },
! }
};
--- 27,91 ----
RobotParams_t erratic_params =
{
! 0.001534,
! "Videre",
! 0.011,
! 0.780, //This is the empirically correct value, but doesn't match wheel
size calculation
! 0,
! 1.626,
! 1,
! 1,
! 0,
! 0,
! 0,
! "",
! "",
! 0,
! 1,
! 0,
! 0,
! 0,
! 300,
! 1000,
! 0,
! 5,
! 5,
! 1,
! 0,
! 0,
! 0,
! 120,
! 392, // length
! 180,
! 415, // width
! 61, // axle distance to center of robot (positive forward)
! 0,
! 0,
! 0,
! 1,
! 1,
! 8,
! "erratic",
! 38400,
! 0,
! 0,
! 0,
! 0,
! 20,
! 1.20482,
! {
! { 73, 105, 90 },
! { 130, 78, 41 },
! { 154, 30, 15 },
! { 154, -30, -15 },
! { 130, -78, -41 },
! { 73, -105, -90 },
! { -146, -60, -145 },
! { -146, 60, 145 },
! },
! 2,
! {
! {0.1, 0.1, 0},
! {0.1, -0.1, 0},
! }
};
Index: robot_params.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/robot_params.h,v
retrieving revision 1.1.2.2
retrieving revision 1.1.2.3
diff -C2 -d -r1.1.2.2 -r1.1.2.3
*** robot_params.h 7 Jun 2006 16:12:51 -0000 1.1.2.2
--- robot_params.h 22 Sep 2006 20:46:43 -0000 1.1.2.3
***************
*** 1,27 ****
! /*
! * 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
! *
! */
#ifndef _ROBOT_PARAMS_H
#define _ROBOT_PARAMS_H
void initialize_robot_params(void);
--- 1,30 ----
! /**
! * Videre Erratic robot driver for Player
! *
! * Copyright (C) 2006
! * Videre Design
! * 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
! *
! **/
#ifndef _ROBOT_PARAMS_H
#define _ROBOT_PARAMS_H
+ #include "libplayercore/player.h"
void initialize_robot_params(void);
***************
*** 71,74 ****
--- 74,78 ----
int RobotRadius; //
int RobotWidth; //
+ int RobotAxleOffset; //
int RotAccel; //
int RotDecel; //
***************
*** 86,89 ****
--- 90,95 ----
double VelConvFactor; //
sonar_pose_t sonar_pose[32];
+ int NumIR;
+ player_pose_t IRPose[8];
} RobotParams_t;
Index: Makefile.am
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/Makefile.am,v
retrieving revision 1.1.2.3
retrieving revision 1.1.2.4
diff -C2 -d -r1.1.2.3 -r1.1.2.4
*** Makefile.am 7 Jun 2006 19:03:50 -0000 1.1.2.3
--- Makefile.am 22 Sep 2006 20:46:43 -0000 1.1.2.4
***************
*** 7,10 ****
--- 7,11 ----
liberratic_la_SOURCES = erratic.cc erratic.h \
+ motorpacket.h motorpacket.cc \
packet.h packet.cc \
robot_params.cc robot_params.h \
-------------------------------------------------------------------------
Take Surveys. Earn Cash. Influence the Future of IT
Join SourceForge.net's Techsay panel and you'll get the chance to share your
opinions on IT & business topics through brief surveys -- and earn cash
http://www.techsay.com/default.php?page=join.php&p=sourceforge&CID=DEVDEV
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
