Update of /cvsroot/playerstage/code/player/server/drivers/mixed/erratic
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv26456
Modified Files:
erratic.cc erratic.h
Log Message:
Index: erratic.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.cc,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** erratic.cc 22 Sep 2006 07:08:15 -0000 1.2
--- erratic.cc 24 Sep 2006 06:06:25 -0000 1.3
***************
*** 208,211 ****
--- 208,212 ----
memset(&this->last_position_cmd, 0,
sizeof(player_position2d_cmd_vel_t));
+ memset(&this->last_car_cmd, 0, sizeof(player_position2d_cmd_car_t));
this->position_subscriptions = 0;
***************
*** 1180,1183 ****
--- 1181,1241 ----
}
+ // Process car-like command, which sets an angular position target and
+ // translational velocity target.
+ // Erratic handles this natively
+ //
+ void
+ Erratic::HandleCarCommand(player_position2d_cmd_car_t cmd)
+ {
+ int speedDemand, angleDemand;
+ unsigned short absspeedDemand, absangleDemand;
+ unsigned char motorcommand[4];
+ ErraticPacket *motorpacket;
+
+ speedDemand = (int)rint(cmd.velocity * 1e3); // convert to mm/s
+ angleDemand = (int)rint(RTOD(cmd.angle)); // convert to deg heading
+
+ // do separate trans and rot vels
+ motorcommand[0] = (command_e)trans_vel;
+ if(speedDemand >= 0)
+ motorcommand[1] = (argtype_e)argint;
+ else
+ motorcommand[1] = (argtype_e)argnint;
+
+ absspeedDemand = (unsigned short)abs(speedDemand);
+ if(absspeedDemand < this->motor_max_speed)
+ {
+ motorcommand[2] = absspeedDemand & 0x00FF;
+ motorcommand[3] = (absspeedDemand & 0xFF00) >> 8;
+ }
+ else
+ {
+ motorcommand[2] = this->motor_max_speed & 0x00FF;
+ motorcommand[3] = (this->motor_max_speed & 0xFF00) >> 8;
+ }
+
+ motorpacket = new ErraticPacket();
+ motorpacket->Build(motorcommand, 4);
+ Send(motorpacket);
+
+
+ // do separate trans and rot vels
+ motorcommand[0] = (command_e)rot_pos;
+ if(angleDemand >= 0)
+ motorcommand[1] = (argtype_e)argint;
+ else
+ motorcommand[1] = (argtype_e)argnint;
+
+ absangleDemand = (unsigned short)abs(angleDemand);
+ motorcommand[2] = absangleDemand & 0x00FF;
+ motorcommand[3] = (absangleDemand & 0xFF00) >> 8;
+
+ motorpacket = new ErraticPacket();
+ motorpacket->Build(motorcommand, 4);
+ Send(motorpacket);
+ }
+
+
+
// Handles one Player command detailing a velocity
void Erratic::HandlePositionCommand(player_position2d_cmd_vel_t position_cmd)
{
***************
*** 1290,1296 ****
{
// do separate trans and rot vels
-
-
-
motorcommand[0] = (command_e)trans_vel;
if(speedDemand >= 0)
--- 1348,1351 ----
***************
*** 1342,1350 ****
// Switchboard for robot commands, called from ProcessMessage
int Erratic::HandleCommand(player_msghdr * hdr, void* data) {
! if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL, this->position_id))
{
player_position2d_cmd_vel_t position_cmd =
*(player_position2d_cmd_vel_t*)data;
this->HandlePositionCommand(position_cmd);
} else
return(-1);
--- 1397,1410 ----
// Switchboard for robot commands, called from ProcessMessage
int Erratic::HandleCommand(player_msghdr * hdr, void* data) {
! if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL, this->position_id))
{
player_position2d_cmd_vel_t position_cmd =
*(player_position2d_cmd_vel_t*)data;
this->HandlePositionCommand(position_cmd);
+ } else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_CAR, this->position_id))
+ {
+ player_position2d_cmd_car_t position_cmd =
*(player_position2d_cmd_car_t*)data;
+ this->HandleCarCommand(position_cmd);
} else
+
return(-1);
Index: erratic.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/erratic/erratic.h,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** erratic.h 22 Sep 2006 19:24:30 -0000 1.3
--- erratic.h 24 Sep 2006 06:06:25 -0000 1.4
***************
*** 70,79 ****
set_max_position_velocity = 6,
reset_origo = 7,
! trans_vel = 11,
configuration = 18,
! rot_vel = 21,
set_max_rot_acc = 23,
stop = 29,
! wheel_vel = 32,
set_analog = 71,
save_config = 72,
--- 70,81 ----
set_max_position_velocity = 6,
reset_origo = 7,
! trans_vel = 11, // mm/s
! rot_pos = 12, // deg
! rot_dpos = 13, // deg
configuration = 18,
! rot_vel = 21, // deg/s
set_max_rot_acc = 23,
stop = 29,
! wheel_vel = 32, // mm/s
set_analog = 71,
save_config = 72,
***************
*** 153,156 ****
--- 155,159 ----
int HandleCommand(player_msghdr * hdr, void * data);
void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
+ void HandleCarCommand(player_position2d_cmd_car_t position_cmd);
void PublishAllData();
***************
*** 175,178 ****
--- 178,182 ----
player_position2d_cmd_vel_t last_position_cmd;
+ player_position2d_cmd_car_t last_car_cmd;
std::queue<ErraticPacket *> send_queue;
-------------------------------------------------------------------------
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
