Update of /cvsroot/playerstage/code/player/server/drivers/mixed/clodbuster
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv4552/server/drivers/mixed/clodbuster
Modified Files:
Tag: release-2-0-patches
Makefile.am clodbuster.cc clodbuster.h packet.cc
Log Message:
backported lots of stuff from HEAD
Index: clodbuster.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/clodbuster/clodbuster.cc,v
retrieving revision 1.10
retrieving revision 1.10.2.1
diff -C2 -d -r1.10 -r1.10.2.1
*** clodbuster.cc 23 Feb 2006 18:54:54 -0000 1.10
--- clodbuster.cc 7 Jun 2006 16:12:49 -0000 1.10.2.1
***************
*** 120,124 ****
ClodBuster::ClodBuster( ConfigFile* cf, int section)
! : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_POSITION_CODE, PLAYER_ALL_MODE)
{
clodbuster_fd = -1;
--- 120,124 ----
ClodBuster::ClodBuster( ConfigFile* cf, int section)
! : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_POSITION2D_CODE)
{
clodbuster_fd = -1;
***************
*** 230,242 ****
// reset odometry
ResetRawPositions();
!
! /* player_position_cmd_t zero_cmd;
! memset(&zero_cmd,0,sizeof(player_position_cmd_t));
! memset(&this->position_data,0,sizeof(player_position_data_t));
! this->PutCommand(this->device_id,(void*)&zero_cmd,
! sizeof(player_position_cmd_t),NULL);
! this->PutData((void*)&this->position_data,
! sizeof(player_position_data_t),NULL);*/
!
direct_command_control = true;
--- 230,234 ----
// reset odometry
ResetRawPositions();
!
direct_command_control = true;
***************
*** 270,385 ****
////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
! int ClodBuster::ProcessMessage(ClientData * client, player_msghdr * hdr,
uint8_t * data, uint8_t * resp_data, size_t * resp_len)
{
assert(hdr);
assert(data);
- assert(resp_data);
- assert(resp_len);
! if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_SET_ODOM,
device_id))
{
! assert(hdr->size == sizeof(player_position_set_odom_req_t));
! player_position_set_odom_req_t & set_odom_req =
*((player_position_set_odom_req_t*)data);
!
! this->position_data.xpos=(set_odom_req.x);
! this->position_data.ypos=(set_odom_req.y);
! this->position_data.yaw= set_odom_req.theta>>8;
!
! *resp_len = 0;
! return PLAYER_MSGTYPE_RESP_ACK;
}
! if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_GET_GEOM,
device_id))
{
! assert(*resp_len >= sizeof(player_position_geom_t));
! player_position_geom_t & geom = *((player_position_geom_t*)resp_data);
! *resp_len = sizeof(player_position_geom_t);
// TODO : get values from somewhere.
! geom.pose[0] = htons((short) (-100));
! geom.pose[1] = htons((short) (0));
! geom.pose[2] = htons((short) (0));
! geom.size[0] = htons((short) (2 * 250));
! geom.size[1] = htons((short) (2 * 225));
! return PLAYER_MSGTYPE_RESP_ACK;
}
! if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_MOTOR_POWER,
device_id))
{
! assert(hdr->size == sizeof(player_position_power_config_t));
! player_position_power_config_t & power_config =
*((player_position_power_config_t*)data);
GRASPPacket packet;
! if(power_config.value==1)
! packet.Build(SET_SLEEP_MODE,SLEEP_MODE_OFF);
! else
! packet.Build(SET_SLEEP_MODE,SLEEP_MODE_ON);
!
! packet.Send(clodbuster_fd);
! *resp_len = 0;
! return PLAYER_MSGTYPE_RESP_ACK;
}
! /* velocity control mode:
! * 0 = direct wheel velocity control (default)
! * 1 = separate translational and rotational control
! */
! if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_VELOCITY_MODE,
device_id))
{
! assert(hdr->size == sizeof(player_position_velocitymode_config_t));
! player_position_velocitymode_config_t & velmode_config =
*((player_position_velocitymode_config_t*)data);
if(velmode_config.value)
! direct_command_control = false;
! else
! direct_command_control = true;
! *resp_len = 0;
! return PLAYER_MSGTYPE_RESP_ACK;
}
! if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_RESET_ODOM,
device_id))
{
ResetRawPositions();
! *resp_len = 0;
! return PLAYER_MSGTYPE_RESP_ACK;
}
! if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION_SPEED_PID,
device_id))
{
! assert(hdr->size == sizeof(player_position_speed_pid_req_t));
! player_position_speed_pid_req_t & pid =
*((player_position_speed_pid_req_t*)data);
! kp = ntohl(pid.kp);
! ki = ntohl(pid.ki);
! kd = ntohl(pid.kd);
! *resp_len = 0;
! return PLAYER_MSGTYPE_RESP_ACK;
}
! if (MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 0, device_id))
{
! assert(hdr->size == sizeof(player_position_cmd_t));
! player_position_cmd_t & command = *((player_position_cmd_t*)data);
newmotorspeed = false;
! if( speedDemand != (int) ntohl(command.xspeed))
newmotorspeed = true;
! speedDemand = (int) ntohl(command.xspeed);
newmotorturn = false;
! if(turnRateDemand != (int) ntohl(command.yawspeed))
newmotorturn = true;
! turnRateDemand = (int) ntohl(command.yawspeed);
! *resp_len = 0;
! return 0;
}
- *resp_len = 0;
return -1;
}
--- 262,371 ----
////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
! int ClodBuster::ProcessMessage (MessageQueue * resp_queue, player_msghdr *
hdr, void * data)
{
+
assert(hdr);
assert(data);
! if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_SET_ODOM, device_addr))
{
! assert(hdr->size == sizeof(player_position2d_set_odom_req_t));
! player_position2d_set_odom_req_t & set_odom_req =
*((player_position2d_set_odom_req_t*)data);
!
! this->position_data.pos = set_odom_req.pose;
!
! Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_SET_ODOM);
! return 0;
}
! if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_GET_GEOM, device_addr))
{
! player_position2d_geom_t geom;
// TODO : get values from somewhere.
! geom.pose.px = -0.1;//htons((short) (-100));
! geom.pose.py = 0;//htons((short) (0));
! geom.pose.pa = 0;//htons((short) (0));
! geom.size.sw = 0.5;//htons((short) (2 * 250));
! geom.size.sl = 0.45;//htons((short) (2 * 225));
! Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_GET_GEOM, &geom, sizeof(geom));
! return 0;
}
! if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_MOTOR_POWER, device_addr))
{
! assert(hdr->size == sizeof(player_position2d_power_config_t));
! player_position2d_power_config_t & power_config =
*((player_position2d_power_config_t*)data);
GRASPPacket packet;
! if(power_config.state==1)
! packet.Build(SET_SLEEP_MODE,SLEEP_MODE_OFF);
! else
! packet.Build(SET_SLEEP_MODE,SLEEP_MODE_ON);
! packet.Send(clodbuster_fd);
!
! Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_MOTOR_POWER);
! return 0;
}
! /* velocity control mode:
! * 0 = direct wheel velocity control (default)
! * 1 = separate translational and rotational control
! */
! if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_VELOCITY_MODE, device_addr))
{
! assert(hdr->size == sizeof(player_position2d_velocity_mode_config_t));
! player_position2d_velocity_mode_config_t & velmode_config =
*((player_position2d_velocity_mode_config_t*)data);
if(velmode_config.value)
! direct_command_control = false;
! else
! direct_command_control = true;
! Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_VELOCITY_MODE);
! return 0;
}
! if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_RESET_ODOM, device_addr))
{
ResetRawPositions();
! Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_RESET_ODOM);
! return 0;
}
! if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_SPEED_PID, device_addr))
{
! assert(hdr->size == sizeof(player_position2d_speed_pid_req_t));
! player_position2d_speed_pid_req_t & pid =
*((player_position2d_speed_pid_req_t*)data);
! kp = pid.kp;
! ki = pid.ki;
! kd = pid.kd;
! Publish(device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_SPEED_PID);
! return 0;
}
! if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION2D_CMD_VEL, device_addr))
{
! assert(hdr->size == sizeof(player_position2d_cmd_vel_t));
! player_position2d_cmd_vel_t & command =
*((player_position2d_cmd_vel_t*)data);
newmotorspeed = false;
! if( speedDemand != (int) command.vel.px)
newmotorspeed = true;
! speedDemand = (int) command.vel.px;
newmotorturn = false;
! if(turnRateDemand != (int) command.vel.pa)
newmotorturn = true;
! turnRateDemand = (int) command.vel.pa;
! return 0;
}
return -1;
}
***************
*** 422,427 ****
old_encoder_measurement = encoder_measurement;
!
this->PutMsg(device_id,NULL,PLAYER_MSGTYPE_DATA,0,(void*)&this->position_data,
! sizeof(player_position_data_t),NULL);
// printf("left: %d , right %d count:
%u\n",encoder_measurement.left,encoder_measurement.right,encoder_measurement.time_count);
// printf("left: %d , right
%d\n",encoder_measurement.left-encoder_offset.left,encoder_measurement.right-encoder_offset.right);
--- 408,413 ----
old_encoder_measurement = encoder_measurement;
!
Publish(device_addr,NULL,PLAYER_MSGTYPE_DATA,PLAYER_POSITION2D_DATA_STATE,(void*)&this->position_data,
! sizeof(player_position2d_data_t),NULL);
// printf("left: %d , right %d count:
%u\n",encoder_measurement.left,encoder_measurement.right,encoder_measurement.time_count);
// printf("left: %d , right
%d\n",encoder_measurement.left-encoder_offset.left,encoder_measurement.right-encoder_offset.right);
***************
*** 624,632 ****
float L = Kenc*(dEr+dEl)*.5;
float D = Kenc*(dEr-dEl)/WheelSeparation;
! float Phi = M_PI/180.0*this->position_data.yaw+0.5*D;
! this->position_data.xpos += (int32_t) (L*cos(Phi)*1.0e3);
! this->position_data.ypos += (int32_t) (L*sin(Phi)*1.0e3);
! this->position_data.yaw += (int32_t) (D*180.0/M_PI);
}
--- 610,618 ----
float L = Kenc*(dEr+dEl)*.5;
float D = Kenc*(dEr-dEl)/WheelSeparation;
! float Phi = this->position_data.pos.pa+0.5*D;
! this->position_data.pos.px += L*cos(Phi);
! this->position_data.pos.py += L*sin(Phi);
! this->position_data.pos.pa += D;
}
Index: packet.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/clodbuster/packet.cc,v
retrieving revision 1.2
retrieving revision 1.2.12.1
diff -C2 -d -r1.2 -r1.2.12.1
*** packet.cc 30 Jan 2004 19:21:15 -0000 1.2
--- packet.cc 7 Jun 2006 16:12:49 -0000 1.2.12.1
***************
*** 39,43 ****
extern PlayerTime* GlobalTime;
! extern bool debug;
void GRASPPacket::Print() {
--- 39,43 ----
extern PlayerTime* GlobalTime;
! //extern bool debug;
void GRASPPacket::Print() {
***************
*** 124,133 ****
}
! if(debug)
{
struct timeval dummy;
GlobalTime->GetTime(&dummy);
printf("GRASPPacket::Send():%ld:%ld\n", dummy.tv_sec, dummy.tv_usec);
! }
return(0);
}
--- 124,133 ----
}
! /* if(debug)
{
struct timeval dummy;
GlobalTime->GetTime(&dummy);
printf("GRASPPacket::Send():%ld:%ld\n", dummy.tv_sec, dummy.tv_usec);
! }*/
return(0);
}
Index: Makefile.am
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/clodbuster/Makefile.am,v
retrieving revision 1.2
retrieving revision 1.2.8.1
diff -C2 -d -r1.2 -r1.2.8.1
*** Makefile.am 17 Nov 2004 19:53:48 -0000 1.2
--- Makefile.am 7 Jun 2006 16:12:49 -0000 1.2.8.1
***************
*** 1,7 ****
! AM_CPPFLAGS = -Wall -I$(top_srcdir)/server -I$(top_srcdir)/replace
! noinst_LIBRARIES = @CLODBUSTER_LIB@
! EXTRA_LIBRARIES = libclodbuster.a
- libclodbuster_a_SOURCES = clodbuster.cc packet.cc clodbuster.h packet.h
--- 1,9 ----
! AM_CPPFLAGS = -Wall -I$(top_srcdir) -I$(top_srcdir)/replace
! noinst_LTLIBRARIES =
! if INCLUDE_CLODBUSTER
! noinst_LTLIBRARIES += libclodbuster.la
! endif
! libclodbuster_la_SOURCES = clodbuster.cc packet.cc clodbuster.h packet.h
Index: clodbuster.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/clodbuster/clodbuster.h,v
retrieving revision 1.4
retrieving revision 1.4.4.1
diff -C2 -d -r1.4 -r1.4.4.1
*** clodbuster.h 22 May 2005 06:02:05 -0000 1.4
--- clodbuster.h 7 Jun 2006 16:12:49 -0000 1.4.4.1
***************
*** 35,49 ****
#include <sys/time.h>
! #include <driver.h>
! #include <drivertable.h>
! #include <packet.h>
! #include <player.h>
! //#include <robot_params.h>
! /*
! #define P2OS_MOTORS_REQUEST_ON 0
! #define P2OS_MOTORS_ON 1
! #define P2OS_MOTORS_REQUEST_OFF 2
! #define P2OS_MOTORS_OFF 3
! */
/* data for the clodbuster */
#define CLODBUSTER_CYCLETIME_USEC 50000
--- 35,40 ----
#include <sys/time.h>
! #include <libplayercore/playercore.h>
!
/* data for the clodbuster */
#define CLODBUSTER_CYCLETIME_USEC 50000
***************
*** 138,142 ****
{
private:
! player_position_data_t position_data;
void ResetRawPositions();
clodbuster_encoder_data_t ReadEncoders();
--- 129,133 ----
{
private:
! player_position2d_data_t position_data;
void ResetRawPositions();
clodbuster_encoder_data_t ReadEncoders();
***************
*** 194,199 ****
virtual void Main();
! // MessageHandler
! int ProcessMessage(ClientData * client, player_msghdr * hdr, uint8_t *
data, uint8_t * resp_data, size_t * resp_len);
virtual int Setup();
--- 185,190 ----
virtual void Main();
! // Process incoming messages from clients
! int ProcessMessage (MessageQueue * resp_queue, player_msghdr * hdr, void
* data);
virtual int Setup();
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
