clodbuster.cc

机器人仿真软件

    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;
}

void 
ClodBuster::Main()
{
//  player_position_speed_pid_req_t pid;
//  unsigned char config[CLODBUSTER_CONFIG_BUFFER_SIZE];
  
  
//  int config_size;

  GetGraspBoardParams();

  // memory for PID
  int uVmax=max_limits[SET_SERVO_THROTTLE];//-center_limits[SET_SERVO_THROTTLE];
  int uVmin=min_limits[SET_SERVO_THROTTLE];//-center_limits[SET_SERVO_THROTTLE];
  int uV0=center_limits[SET_SERVO_THROTTLE];
  int uWmax=max_limits[SET_SERVO_FRONTSTEER];//-center_limits[SET_SERVO_FRONTSTEER];
  int uWmin=min_limits[SET_SERVO_FRONTSTEER];//-center_limits[SET_SERVO_FRONTSTEER];
  int uW0=center_limits[SET_SERVO_FRONTSTEER];
  float uV = uV0, uW=uW0, errV[3]={0}, errW[3]={0}, uVlast=uV0, uWlast=uW0,V;

  printf("V max min centre %d %d %d\n",uVmax,uVmin,uV0);
  printf("W max min centre %d %d %d\n",uWmax,uWmin,uW0);

  for(;;)
    {
      ProcessMessages();
      pthread_testcancel();
      
      // read encoders & update pose and velocity
      encoder_measurement = ReadEncoders();
      
      DifferenceEncoders();
      IntegrateEncoders();
      
      // remember old values
      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);


      // do control
      /* NEXT, write commands */
      if(!direct_command_control) // fix this it's reversed for playerv
	{
	  // do direct wheel velocity control here
	  //printf("speedDemand: %d\t turnRateDemand: %d\n",
	  //speedDemand, turnRateDemand);
	  unsigned char v,w;
	  v = SetServo(SET_SERVO_THROTTLE,speedDemand);
	  w = SetServo(SET_SERVO_FRONTSTEER,turnRateDemand);
	  printf("The vel/turn command numbers : v:%d (%u) w:%d (%u)\n",center_limits[SET_SERVO_THROTTLE]+speedDemand,v,center_limits[SET_SERVO_FRONTSTEER]+turnRateDemand,w);
    
	  
	}
      else
	{
	     //	  printf("Do PID !\n");
	     // find tracking errors
	     errV[0] = EncV - speedDemand*1e-3;
	     errW[0] = EncOmega - turnRateDemand*M_PI/180.0;
	     
	     // find actions
	     uV = uVlast + Kv->K1()*errV[0] + + Kv->K2()*errV[1] + Kv->K3()*errV[2];
	     if (uV > uVmax)
		  {
		       uV = uVmax;
		       printf("+V control saturated!\n");
		  }
	     if (uV < uVmin)
		  {
		       uV = uVmin;
		       printf("-V control saturated!\n");
		  }
	     printf("V loop err: %f, u = %f, r = %f, x = %f\n",errV[0], uV,speedDemand*1e-3,EncV);
	     //	     if (speedDemand*1e-3 > .0125) 
	     if (fabs(EncV) > .0125) 
		  {
		    if (fabs(EncV) <.1)
		      if (EncV <.1)
			V = -.1;
		      else
			V = .1;
		    else
		      V = EncV;
		    // NB "-" sign for wrong convention +ve -> left 
		    uW = uWlast - WheelBase/V*(Kw->K1()*errW[0] + + Kw->K2()*errW[1] + Kw->K3()*errW[2]);
		    //   uW = uWlast - 1e3*WheelBase/speedDemand*(Kw->K1()*errW[0] + + Kw->K2()*errW[1] + Kw->K3()*errW[2]);
		  }
	     else
		  {
		       // set it to zero (center)
		       uW = uW0;
		       uWlast = uW0;
		       for (int i=0;i<3;i++)
			    errW[i] = 0.;
		  } 
	     if (uW > uWmax)
		  {
		       uW = uWmax;
		       printf("+W control saturated!\n");
		  }
	     if (uW < uWmin)
		  {
		       uW = uWmin;
		       printf("-W control saturated!\n");
		  }
	     printf("W loop err: %f, u = %f, r = %f, x = %f\n",errW[0], uW,turnRateDemand*M_PI/180.0,EncOmega);
	     
	     // Write actions to control board
	     SetServo(SET_SERVO_THROTTLE,(unsigned char) uV);
	     SetServo(SET_SERVO_FRONTSTEER,(unsigned char) uW);
	     
	     for (int i=1;i<3;i++)
	       {
		 errW[i] = errW[i-1];
		 errV[i] = errV[i-1];
	       }
	     uVlast = uV;
	     uWlast = uW;

	}
      usleep(200000);
    }
  pthread_exit(NULL);
}

void
ClodBuster::ResetRawPositions()
{
  encoder_offset=ReadEncoders();
}

clodbuster_encoder_data_t  ClodBuster::ReadEncoders()
{ GRASPPacket packet,rpacket;
 clodbuster_encoder_data_t temp;

 packet.Build(ECHO_ENCODER_COUNTS_TS);
 packet.Send(clodbuster_fd);

 rpacket.Receive(clodbuster_fd,ECHO_ENCODER_COUNTS_TS);
 rpacket.size=rpacket.retsize;
 rpacket.PrintHex();

 // make 24bit signed int a 32bit signed int
 temp.left = (rpacket.packet[0]<<16)|(rpacket.packet[1]<<8)|(rpacket.packet[2]);
 if (rpacket.packet[0] & 0x80)
   temp.left |= 0xff<<24;
 temp.right = (rpacket.packet[3]<<16)|(rpacket.packet[4]<<8)|(rpacket.packet[5]);
 if (rpacket.packet[3] & 0x80)
   temp.right |= 0xff<<24;
 // temp.left = (((char) rpacket.packet[0])<<16)|(rpacket.packet[1]<<8)|(rpacket.packet[2]);
 // temp.right = (((char) rpacket.packet[3])<<16)|(rpacket.packet[4]<<8)|(rpacket.packet[5]);

 // get timer count 32bit unsigned int
 temp.time_count = (rpacket.packet[6] << 24)|(rpacket.packet[7] <<16)|(rpacket.packet[8]<<8)|(rpacket.packet[9]);

 return temp;

}

void ClodBuster::GetGraspBoardParams()
{
  GRASPPacket packet,rpacket;
  packet.Build(ECHO_MAX_SERVO_LIMITS);
  packet.Send(clodbuster_fd);
  printf("Servo Limit Enquiry: ");

  packet.PrintHex();

  rpacket.Receive(clodbuster_fd,ECHO_MAX_SERVO_LIMITS);
  
  memcpy(max_limits,rpacket.packet,8);
  rpacket.size = rpacket.retsize;
  
  rpacket.PrintHex();

  packet.Build(ECHO_MIN_SERVO_LIMITS);
  packet.Send(clodbuster_fd);
   printf("Servo Limit Enquiry: ");

  packet.PrintHex();
 
  rpacket.Receive(clodbuster_fd,ECHO_MIN_SERVO_LIMITS);
  rpacket.size = rpacket.retsize;
  
  rpacket.PrintHex();
  memcpy(min_limits,rpacket.packet,8);
  packet.Build(ECHO_CEN_SERVO_LIMITS);
  packet.Send(clodbuster_fd);
  printf("Servo Limit Enquiry: ");

  packet.PrintHex();

  rpacket.Receive(clodbuster_fd,ECHO_CEN_SERVO_LIMITS);
  
  memcpy(center_limits,rpacket.packet,8);
  rpacket.size = rpacket.retsize;
  
  rpacket.PrintHex();
}

unsigned char 
ClodBuster::SetServo(unsigned char chan, int value)
{
  GRASPPacket spacket;
  unsigned char cmd;
  int demanded = center_limits[chan]+value/10;

  if (demanded > max_limits[chan])
    cmd = (unsigned char) max_limits[chan];
  else if (demanded < min_limits[chan])
    cmd = (unsigned char) min_limits[chan];
  else
    cmd = (unsigned char)demanded;
  spacket.Build(chan,cmd);
  spacket.Send(clodbuster_fd);
  // spacket.PrintHex();

  return(cmd);
}
void 
ClodBuster::SetServo(unsigned char chan, unsigned char cmd)
{
  GRASPPacket spacket;
  spacket.Build(chan,cmd);
  spacket.Send(clodbuster_fd);
  // spacket.PrintHex();
}

void ClodBuster::IntegrateEncoders()
{
  // assign something to xpos,ypos, theta
  float dEr = encoder_measurement.right-old_encoder_measurement.right; 
  float dEl = encoder_measurement.left-old_encoder_measurement.left; 
  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;
}

void ClodBuster::DifferenceEncoders()
{
  float dEr = encoder_measurement.right-old_encoder_measurement.right; 
  float dEl = encoder_measurement.left-old_encoder_measurement.left; 
  int64_t dtc =  encoder_measurement.time_count-old_encoder_measurement.time_count;
  if (dtc< -2147483648LL) 
       {
	    dtc += 4294967296LL; //(1<<32);
	    printf("encoder timer rollover caught\n");
       }
  float dt = dtc*1.6e-6;
  if (dt < 20e-3)
       printf("dt way too short %f s\n",dt);
  else if  (dt > 2.0/LoopFreq)
       printf("dt way too long %f s\n",dt);
       
  EncV = Kenc*(dEr+dEl)*.5/dt;
  EncOmega = Kenc*(dEr-dEl)/WheelSeparation/dt;     
  EncVleft = dEl/dt;
  EncVright = dEr/dt;
  printf("EncV = %f, EncW = %f, dt = %f\n",EncV,EncOmega*180/M_PI,dt);

}