Update of /cvsroot/playerstage/code/player/server/drivers/mixed/botrics
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv14234/server/drivers/mixed/botrics
Modified Files:
Tag: release-2-0-patches
obot.cc
Log Message:
merged car-like command and set/reset odom support for obot driver from HEAD;
added -dev jsdev arg to playerjoy
Index: obot.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/botrics/obot.cc,v
retrieving revision 1.22
retrieving revision 1.22.2.1
diff -C2 -d -r1.22 -r1.22.2.1
*** obot.cc 6 Mar 2006 23:05:38 -0000 1.22
--- obot.cc 21 Sep 2006 03:51:36 -0000 1.22.2.1
***************
*** 51,58 ****
- none
@par Supported configuration requests
! - PLAYER_POSITION_GET_GEOM_REQ
! - PLAYER_POSITION_MOTOR_POWER_REQ
@par Configuration file options
--- 51,64 ----
- none
+ @par Supported commands
+
+ - PLAYER_POSITION2D_CMD_VEL
+ - PLAYER_POSITION2D_CMD_CAR
+
@par Supported configuration requests
! - PLAYER_POSITION2D_REQ_GET_GEOM
! - PLAYER_POSITION2D_REQ_SET_ODOM
! - PLAYER_POSITION2D_REQ_RESET_ODOM
@par Configuration file options
***************
*** 82,85 ****
--- 88,101 ----
- If non-zero, then assume that the motors and encoders connections
are swapped.
+
+ - car_angle_deadzone (angle)
+ - Default: 5.0 degrees
+ - Minimum angular error required to induce servoing when in car-like
+ command mode.
+
+ - car_angle_p (float)
+ - Default: 1.0
+ - Value to be multiplied by angular error (in rad) to produce angular
+ velocity command (in rad/sec) when in car-like command mode
@par Example
***************
*** 136,139 ****
--- 152,162 ----
int max_accel;
+ // Minimum angular error required to induce servoing when in car-like
+ // command mode.
+ double car_angle_deadzone;
+ // Value to be multiplied by angular error (in rad) to produce angular
+ // velocity command (in rad/sec) when in car-like command mode
+ double car_angle_p;
+
// Robot geometry (size and rotational offset)
player_bbox_t robot_size;
***************
*** 155,161 ****
--- 178,187 ----
int InitRobot();
int GetBatteryVoltage(int* voltage);
+ double angle_diff(double a, double b);
+ player_position2d_cmd_car_t last_car_cmd;
int last_final_lvel, last_final_rvel;
bool sent_new_command;
+ bool car_command_mode;
public:
***************
*** 169,172 ****
--- 195,199 ----
void ProcessCommand(player_position2d_cmd_vel_t * cmd);
+ void ProcessCarCommand(player_position2d_cmd_car_t * cmd);
// Process incoming messages from clients
***************
*** 226,229 ****
--- 253,260 ----
this->motors_swapped = cf->ReadInt(section, "motors_swapped", 0);
this->max_accel = cf->ReadInt(section, "max_accel", 5);
+
+ this->car_angle_deadzone = cf->ReadAngle(section, "car_angle_deadzone",
+ DTOR(5.0));
+ this->car_angle_p = cf->ReadFloat(section, "car_angle_p", 1.0);
}
***************
*** 325,328 ****
--- 356,360 ----
this->last_final_rvel = this->last_final_lvel = 0;
this->sent_new_command = false;
+ this->car_command_mode = false;
printf("Botrics Obot connection initializing (%s)...", serial_port);
***************
*** 434,439 ****
if(!this->sent_new_command)
{
! if(this->SetVelocity(this->last_final_lvel, this->last_final_rvel) < 0)
! PLAYER_ERROR("failed to set velocity");
}
--- 466,482 ----
if(!this->sent_new_command)
{
! // Which mode are we in?
! if(this->car_command_mode)
! {
! // Car-like command mode. Re-compute angular vel based on target
! // heading
! this->ProcessCarCommand(&this->last_car_cmd);
! }
! else
! {
! // Direct velocity command mode. Re-send last set of velocities.
! if(this->SetVelocity(this->last_final_lvel, this->last_final_rvel) <
0)
! PLAYER_ERROR("failed to set velocity");
! }
}
***************
*** 491,494 ****
--- 534,564 ----
}
+ // Process car-like command, which sets an angular position target and
+ // translational velocity target. The basic idea is to compute angular
+ // velocity so as to servo (with P-control) to target angle. Then pass the
+ // two velocities to ProcessCommand() for thresholding and unit conversion.
+ void
+ Obot::ProcessCarCommand(player_position2d_cmd_car_t * cmd)
+ {
+ // Cache this command for later reuse
+ this->last_car_cmd = *cmd;
+
+ // Build up a cmd_vel structure to pass to ProcessCommand()
+ player_position2d_cmd_vel_t vel_cmd;
+ memset(&vel_cmd,0,sizeof(vel_cmd));
+
+ // Pass through trans vel unmodified
+ vel_cmd.vel.px = cmd->velocity;
+
+ // Compute rot vel
+ double da = this->angle_diff(cmd->angle, this->pa);
+ if(fabs(da) < DTOR(this->car_angle_deadzone))
+ vel_cmd.vel.pa = 0.0;
+ else
+ vel_cmd.vel.pa = this->car_angle_p * da;
+
+ this->ProcessCommand(&vel_cmd);
+ }
+
void
Obot::ProcessCommand(player_position2d_cmd_vel_t * cmd)
***************
*** 604,607 ****
--- 674,693 ----
this->ProcessCommand((player_position2d_cmd_vel_t*)data);
this->sent_new_command = true;
+ this->car_command_mode = false;
+ }
+ return(0);
+ }
+ else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
+ PLAYER_POSITION2D_CMD_CAR,
+ this->position_addr))
+ {
+ // Only take the first new command (should probably take the last,
+ // but...)
+ if(!this->sent_new_command)
+ {
+ assert(hdr->size == sizeof(player_position2d_cmd_vel_t));
+ this->ProcessCarCommand((player_position2d_cmd_car_t*)data);
+ this->sent_new_command = true;
+ this->car_command_mode = true;
}
return(0);
***************
*** 644,647 ****
--- 730,773 ----
return(0);
}
+ else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
+ PLAYER_POSITION2D_REQ_SET_ODOM,
+ this->position_addr))
+ {
+ if(hdr->size != sizeof(player_position2d_set_odom_req_t))
+ {
+ PLAYER_WARN("Arg to odometry set requests wrong size; ignoring");
+ return(-1);
+ }
+ player_position2d_set_odom_req_t* set_odom_req =
+ (player_position2d_set_odom_req_t*)data;
+
+ // Just overwrite our current odometric pose.
+ this->px = set_odom_req->pose.px;
+ this->py = set_odom_req->pose.py;
+ this->pa = set_odom_req->pose.pa;
+
+ this->Publish(this->position_addr, resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_SET_ODOM);
+ return(0);
+ }
+ else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
+ PLAYER_POSITION2D_REQ_RESET_ODOM,
+ this->position_addr))
+ {
+ if(hdr->size != sizeof(player_position2d_reset_odom_config_t))
+ {
+ PLAYER_WARN("Arg to odometry reset requests wrong size; ignoring");
+ return(-1);
+ }
+
+ // Just overwrite our current odometric pose.
+ this->px = 0.0;
+ this->py = 0.0;
+ this->pa = 0.0;
+
+ this->Publish(this->position_addr, resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_RESET_ODOM);
+ return(0);
+ }
else
return -1;
***************
*** 1074,1075 ****
--- 1200,1217 ----
}
+ // computes the signed minimum difference between the two angles.
+ double
+ Obot::angle_diff(double a, double b)
+ {
+ double d1, d2;
+ a = NORMALIZE(a);
+ b = NORMALIZE(b);
+ d1 = a-b;
+ d2 = 2*M_PI - fabs(d1);
+ if(d1 > 0)
+ d2 *= -1.0;
+ if(fabs(d1) < fabs(d2))
+ return(d1);
+ else
+ return(d2);
+ }
-------------------------------------------------------------------------
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
