[Playerstage-commit] code/player/server/drivers/mixed/clodbuster Makefile.am,1.2,1.3 clodbuster.cc,1

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 454-5900

Update of /cvsroot/playerstage/code/player/server/drivers/mixed/clodbuster
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv15661/server/drivers/mixed/clodbuster

Modified Files:
	Makefile.am clodbuster.cc clodbuster.h packet.cc 
Log Message:
updated clodbuster to v2 api 

Index: clodbuster.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/clodbuster/clodbuster.cc,v
retrieving revision 1.10
retrieving revision 1.11
diff -C2 -d -r1.10 -r1.11
*** clodbuster.cc	23 Feb 2006 18:54:54 -0000	1.10
--- clodbuster.cc	6 May 2006 23:40:00 -0000	1.11
***************
*** 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.3
diff -C2 -d -r1.2 -r1.3
*** packet.cc	30 Jan 2004 19:21:15 -0000	1.2
--- packet.cc	6 May 2006 23:40:00 -0000	1.3
***************
*** 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.3
diff -C2 -d -r1.2 -r1.3
*** Makefile.am	17 Nov 2004 19:53:48 -0000	1.2
--- Makefile.am	6 May 2006 23:40:00 -0000	1.3
***************
*** 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.5
diff -C2 -d -r1.4 -r1.5
*** clodbuster.h	22 May 2005 06:02:05 -0000	1.4
--- clodbuster.h	6 May 2006 23:40:00 -0000	1.5
***************
*** 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] code/player/server/drivers/mixed/clodbuster Makefile.am,1.2,1.3 clodbuster.cc,1

[Playerstage-commit] code/player/server/drivers/mixed/clodbuster Makefile.am,1.2,1.3 clodbuster.cc,1.10,1.11 clodbuster.h,1.4,1.5 packet.cc,1.2,1.3