Update of /cvsroot/playerstage/code/player/server/drivers/mixed/erratic
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv5290/server/drivers/mixed/erratic
Modified Files:
erratic.cc erratic.h motorpacket.cc motorpacket.h
robot_params.cc robot_params.h sip.h
Log Message:
updated .cfg files and docs
Index: erratic.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.cc,v
retrieving revision 1.19
retrieving revision 1.20
diff -C2 -d -r1.19 -r1.20
*** erratic.cc 3 Dec 2007 22:35:10 -0000 1.19
--- erratic.cc 13 Dec 2007 20:23:03 -0000 1.20
***************
*** 7,25 ****
* 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,
[...2997 lines suppressed...]
! else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL_HEAD, this->position_id))
! {
! player_position2d_cmd_car_t position_cmd =
*(player_position2d_cmd_car_t*)data;
! this->HandleCarCommand(position_cmd);
! }
! else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_PTZ_CMD_STATE, this->ptz_id))
! {
! player_ptz_cmd_t ptz_cmd = *(player_ptz_cmd_t*)data;
! this->HandlePtzCommand(ptz_cmd, this->ptz_id);
! }
! else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_PTZ_CMD_STATE, this->ptz2_id))
! {
! player_ptz_cmd_t ptz_cmd = *(player_ptz_cmd_t*)data;
! this->HandlePtzCommand(ptz_cmd, this->ptz2_id);
! }
! else
! return(-1);
! return(0);
}
Index: erratic.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.h,v
retrieving revision 1.10
retrieving revision 1.11
diff -C2 -d -r1.10 -r1.11
*** erratic.h 1 Nov 2007 22:16:20 -0000 1.10
--- erratic.h 13 Dec 2007 20:23:04 -0000 1.11
***************
*** 49,52 ****
--- 49,57 ----
#define CPU_VOLTAGE 3.5
+ // angular constants, angular units are 4096 / rev
+ #define ATOR(x) (M_PI * ((double)(x)) / 2048.0)
+ #define ATOD(x) (180.0 * ((double)(x)) / 2048.0)
+ #define RTOA(x) ((short)((x) * 2048.0) / M_PI)
+
// Default max speeds
#define MOTOR_DEF_MAX_SPEED 0.5
***************
*** 76,79 ****
--- 81,85 ----
rot_vel = 21, // deg/s
set_max_rot_acc = 23,
+ set_sonar = 28,
stop = 29,
wheel_vel = 32, // mm/s
***************
*** 82,85 ****
--- 88,92 ----
set_pwm_freq = 73,
set_pwm_max_on = 74,
+ servo_pos = 75,
set_pid_trans_p = 80,
set_pid_trans_v = 81,
***************
*** 106,110 ****
motor = 0x80,
encoder = 0x90,
! ain = 0x9a
} reply_e;
--- 113,118 ----
motor = 0x80,
encoder = 0x90,
! ain = 0x9a,
! sonar = 0x9b
} reply_e;
***************
*** 118,121 ****
--- 126,130 ----
player_aio_data_t aio;
player_ir_data ir;
+ player_sonar_data sonar;
} __attribute__ ((packed)) player_erratic_data_t;
***************
*** 137,143 ****
--- 146,157 ----
player_devaddr_t aio_id;
player_devaddr_t ir_id;
+ player_devaddr_t sonar_id;
+ player_devaddr_t ptz_id, ptz2_id;
int position_subscriptions;
int aio_ir_subscriptions;
+ int sonar_subscriptions;
+ int ptz_subscriptions;
+ int ptz2_subscriptions;
//ErraticMotorPacket* sippacket;
***************
*** 152,155 ****
--- 166,170 ----
void ToggleAIn(unsigned char val);
+ void ToggleSonar(unsigned char val);
int HandleConfig(QueuePointer &resp_queue, player_msghdr * hdr, void* data);
***************
*** 157,160 ****
--- 172,176 ----
void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
void HandleCarCommand(player_position2d_cmd_car_t position_cmd);
+ void HandlePtzCommand(player_ptz_cmd_t ptz_cmd, player_devaddr_t id);
void PublishAllData();
***************
*** 163,166 ****
--- 179,183 ----
void PublishAIn();
void PublishIR();
+ void PublishSonar();
float IRRangeFromVoltage(float voltage);
***************
*** 221,225 ****
Erratic(ConfigFile* cf, int section);
- virtual ~Erratic();
virtual int Subscribe(player_devaddr_t id);
--- 238,241 ----
Index: motorpacket.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/motorpacket.cc,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** motorpacket.cc 30 Sep 2006 22:38:30 -0000 1.2
--- motorpacket.cc 13 Dec 2007 20:23:04 -0000 1.3
***************
*** 66,77 ****
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);
--- 66,78 ----
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);
***************
*** 109,115 ****
// 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);
--- 110,118 ----
// 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);
! angle = (short)(buffer[cnt] | (buffer[cnt+1] << 8)); // keep as 4096 / full
turn
!
cnt += sizeof(short);
***************
*** 144,178 ****
void ErraticMotorPacket::Fill(player_erratic_data_t* data)
{
! // Odometry data
! {
! // initialize position to current offset
! data->position.pos.px = (this->x_offset + this->xpos) / 1e3;
! data->position.pos.py = (this->y_offset + this->ypos) / 1e3;
! data->position.pos.pa = DTOR(this->angle_offset + this->angle);
! // 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
! float tx = data->position.pos.px * cos(rot) -
! data->position.pos.py * sin(rot);
! data->position.pos.py = data->position.pos.px *
sin(rot) +
! data->position.pos.py * cos(rot);
! data->position.pos.px = tx;
! }
! 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);
! }
}
--- 147,182 ----
void ErraticMotorPacket::Fill(player_erratic_data_t* data)
{
! // Odometry data
! {
! // initialize position to current offset
! data->position.pos.px = (double)(this->xpos - this->x_offset) / 1e3;
! data->position.pos.py = (double)(this->ypos - 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 = ATOR(this->angle_offset); // convert rotation to radians
! double ax = data->position.pos.px;
! double ay = data->position.pos.py;
! data->position.pos.px = ax * cos(rot) + ay * sin(rot);
! data->position.pos.py = -ax * sin(rot) + ay * cos(rot);
! data->position.pos.pa = ATOR(angle - this->angle_offset);
! }
! else
! data->position.pos.pa = ATOR(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.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** robot_params.cc 18 Dec 2006 06:03:27 -0000 1.3
--- robot_params.cc 13 Dec 2007 20:23:04 -0000 1.4
***************
*** 27,34 ****
#define DTOR(x) (M_PI * x / 180.0)
! RobotParams_t erratic_params =
{
0.001534,
! "Videre",
0.011,
0.780, //This is the empirically correct value, but doesn't match wheel
size calculation
--- 27,34 ----
#define DTOR(x) (M_PI * x / 180.0)
! RobotParams_t erratic_params_rev_E =
{
0.001534,
! "Erratic",
0.011,
0.780, //This is the empirically correct value, but doesn't match wheel
size calculation
***************
*** 57,64 ****
0,
120,
! 392, // length
! 180,
! 415, // width
! 61, // axle distance to center of robot (positive forward)
0,
0,
--- 57,64 ----
0,
120,
! 392, // length, mm
! 180, // radius, mm
! 415, // width, mm
! 61, // axle distance to center of robot (positive
forward)
0,
0,
***************
*** 66,71 ****
1,
1,
8,
! "erratic",
38400,
0,
--- 66,141 ----
1,
1,
+ "Rev E",
+ 38400,
+ 0,
+ 0,
+ 0,
+ 0,
+ 20,
+ 1.20482,
8,
! { // sonar poses, in m and rads
! { 0.180, 0.110, 0.0, 0.0, 0.0, DTOR(90) },
! { 0.200, 0.085, 0.0, 0.0, 0.0, DTOR(0) },
! { 0.190, 0.065, 0.0, 0.0, 0.0, DTOR(-53) },
! { 0.170, 0.045, 0.0, 0.0, 0.0, DTOR(-24) },
! { 0.180, -0.110, 0.0, 0.0, 0.0, DTOR(-90) },
! { 0.200, -0.085, 0.0, 0.0, 0.0, DTOR( 0) },
! { 0.190, -0.065, 0.0, 0.0, 0.0, DTOR( 53) },
! { 0.170, -0.045, 0.0, 0.0, 0.0, DTOR( 24) },
! },
! 2,
! {
! {0.1, 0.1, 0.0, 0.0, 0.0, 0.0}, // ir poses in m and deg
! {0.1, -0.1, 0.0, 0.0, 0.0, 0.0},
! }
! };
!
!
! //
! // Rev G
! // Sonar position changes
! //
!
! RobotParams_t erratic_params_rev_G =
! {
! 0.001534,
! "Erratic",
! 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, mm
! 180, // radius, mm
! 415, // width, mm
! 61, // axle distance to center of robot (positive
forward)
! 0,
! 0,
! 0,
! 1,
! 1,
! "Rev G",
38400,
0,
***************
*** 77,96 ****
8,
{ // sonar poses, in m and rads
! { 0.180, 0.110, DTOR(90) },
! { 0.200, 0.85, DTOR(0) },
! { 0.190, 0.65, DTOR(-53) },
! { 0.170, 0.45, DTOR(-24) },
! { 0.170, -0.45, DTOR( 24) },
! { 0.190, -0.65, DTOR( 53) },
! { 0.200, -0.85, DTOR( 0) },
! { 0.180,-0.110, DTOR(-90) },
},
2,
{
! {0.1, 0.1, 0}, // ir poses in m and deg
! {0.1, -0.1, 0},
}
};
! RobotParams_t *RobotParams[1] = {&erratic_params};
--- 147,239 ----
8,
{ // sonar poses, in m and rads
! { 0.180, 0.120, 0.0, 0.0, 0.0, DTOR(90) },
! { 0.200, 0.100, 0.0, 0.0, 0.0, DTOR(53) },
! { 0.205, 0.065, 0.0, 0.0, 0.0, DTOR(24) },
! { 0.210, 0.045, 0.0, 0.0, 0.0, DTOR(0) },
! { 0.210, -0.045, 0.0, 0.0, 0.0, DTOR(0) },
! { 0.205, -0.065, 0.0, 0.0, 0.0, DTOR(-24) },
! { 0.200, -0.100, 0.0, 0.0, 0.0, DTOR(-53) },
! { 0.180, -0.120, 0.0, 0.0, 0.0, DTOR(-90) },
},
2,
{
! {0.1, 0.1, 0.0, 0.0, 0.0, 0.0}, // ir poses in m and deg
! {0.1, -0.1, 0.0, 0.0, 0.0, 0.0},
}
};
! //
! // Rev H
! // Shayang-Ye motors
! // DistConvFactor and DiffConvFactor changes
! //
!
! RobotParams_t erratic_params_rev_H =
! {
! 0.001534,
! "Erratic",
! 0.011,
! 0.930, //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, mm
! 180, // radius, mm
! 415, // width, mm
! 61, // axle distance to center of robot (positive
forward)
! 0,
! 0,
! 0,
! 1,
! 1,
! "Rev H",
! 38400,
! 0,
! 0,
! 0,
! 0,
! 20,
! 1.20482,
! 8,
! { // sonar poses, in m and rads
! { 0.180, 0.120, 0.0, 0.0, 0.0, DTOR(90) },
! { 0.200, 0.100, 0.0, 0.0, 0.0, DTOR(53) },
! { 0.205, 0.065, 0.0, 0.0, 0.0, DTOR(24) },
! { 0.210, 0.045, 0.0, 0.0, 0.0, DTOR(0) },
! { 0.210, -0.045, 0.0, 0.0, 0.0, DTOR(0) },
! { 0.205, -0.065, 0.0, 0.0, 0.0, DTOR(-24) },
! { 0.200, -0.100, 0.0, 0.0, 0.0, DTOR(-53) },
! { 0.180, -0.120, 0.0, 0.0, 0.0, DTOR(-90) },
! },
! 2,
! {
! {0.1, 0.1, 0.0, 0.0, 0.0, 0.0}, // ir poses in m and deg
! {0.1, -0.1, 0.0, 0.0, 0.0, 0.0},
! }
! };
!
!
! RobotParams_t *RobotParams[3] = {&erratic_params_rev_E,
&erratic_params_rev_G,
! &erratic_params_rev_H};
Index: robot_params.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/robot_params.h,v
retrieving revision 1.5
retrieving revision 1.6
diff -C2 -d -r1.5 -r1.6
*** robot_params.h 3 Dec 2007 22:35:10 -0000 1.5
--- robot_params.h 13 Dec 2007 20:23:04 -0000 1.6
***************
*** 28,40 ****
#include "libplayercore/player.h"
! #define PLAYER_NUM_ROBOT_TYPES 30
!
! typedef struct
! {
! double x;
! double y;
! double th;
! } sonar_pose_t;
--- 28,34 ----
#include "libplayercore/player.h"
! void initialize_robot_params(void);
! #define PLAYER_NUM_ROBOT_TYPES 30
***************
*** 78,82 ****
int SettableAccsDecs; //
int SettableVelMaxes; //
- int SonarNum; //
char* Subclass;
int SwitchToBaudRate; //
--- 72,75 ----
***************
*** 88,92 ****
double VelConvFactor; //
int NumSonars;
! sonar_pose_t sonar_pose[32];
int NumIR;
player_pose3d_t IRPose[8];
--- 81,85 ----
double VelConvFactor; //
int NumSonars;
! player_pose3d_t sonar_pose[32];
int NumIR;
player_pose3d_t IRPose[8];
-------------------------------------------------------------------------
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://ad.doubleclick.net/clk;164216239;13503038;w?http://sf.net/marketplace
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
