er.cc

机器人仿真软件

    send_command_2_arg( MOTOR_0, SETMOTORCMD, 0x0000, 2, ret );
    send_command_2_arg( MOTOR_0, SETPROFILEMODE, 0x0001, 2, ret );
    send_command_2_arg( MOTOR_0, SETSTOPMODE, 0x0001, 2, ret );
    send_command_4_arg( MOTOR_0, SETVEL, 0, 2, ret );
    send_command_4_arg( MOTOR_0, SETACCEL, 0, 2, ret );
    send_command_4_arg( MOTOR_0, SETDECEL, 0, 2, ret );
    send_command( MOTOR_0, UPDATE, 2, ret );
    send_command( MOTOR_0, RESET, 2, ret );

    /* motor 1 */
    send_command_2_arg( MOTOR_1, RESETEVENTSTATUS, 0x0000, 2, ret );
    send_command_2_arg( MOTOR_1, SETMOTORCMD, 0x0000, 2, ret );
    send_command_2_arg( MOTOR_1, SETPROFILEMODE, 0x0001, 2, ret );
    send_command_2_arg( MOTOR_1, SETSTOPMODE, 0x0001, 2, ret );
    send_command_4_arg( MOTOR_1, SETVEL, 0, 2, ret );
    send_command_4_arg( MOTOR_1, SETACCEL, 0, 2, ret );
    send_command_4_arg( MOTOR_1, SETDECEL, 0, 2, ret );
    send_command( MOTOR_1, UPDATE, 2, ret );
    send_command( MOTOR_1, RESET, 2, ret );
  }
  else
  {
    /* motor 0 */
    send_command_2_arg( MOTOR_0, RESETEVENTSTATUS, 0x0700, 2, ret );
    send_command_4_arg( MOTOR_0, SETVEL, 0, 2, ret );
    send_command( MOTOR_0, UPDATE, 2, ret );
    send_command( MOTOR_0, RESET, 2, ret );

    /* motor 1 */
    send_command_2_arg( MOTOR_1, RESETEVENTSTATUS, 0x0700, 2, ret );
    send_command_4_arg( MOTOR_1, SETVEL, 0, 2, ret );
    send_command( MOTOR_1, UPDATE, 2, ret );
    send_command( MOTOR_1, RESET, 2, ret );
  
  }
	
}

////////////////////
// periodic fcns
////////////////////


int
ER::GetOdom(int *ltics, int *rtics, int *lvel, int *rvel)
{

	unsigned char ret[6];

	/* motor 0 */
  send_command( MOTOR_0, GETCMDPOS, 6, ret );
	*ltics = _motor_0_dir * BytesToInt32(&(ret[2]));

	/* motor 1 */
  send_command( MOTOR_1, GETCMDPOS, 6, ret );
	*rtics = _motor_1_dir * BytesToInt32(&(ret[2]));

	/* hmmm, what to do here ??? */
	/*
	index += 4;
	*rvel = BytesToInt32(buf+index);
	index += 4;
	*lvel = BytesToInt32(buf+index);
	*/

  if( _debug )
  {
    printf("ltics: %d rtics: %d\n", *ltics, *rtics);
  } 

	return(0);
}

int
ER::GetBatteryVoltage(int* voltage)
{
	
	unsigned char ret[4];
	
  send_command_2_arg( MOTOR_1, READANALOG, 0x0001, 6, ret );

  if( _debug )
   	printf( "voltage?: %f\n", (float) BytesToFloat(&(ret[2])) );
  
  //yeah and do something with this voltage???
  
	return(0);
}

int
ER::GetRangeSensor(int s, float * val )
{
	
	unsigned char ret[6];

  send_command_2_arg( s / 8, READANALOG, s % 8, 6, ret );

	/* this is definately wrong, need to fix this */
	float range = (float) BytesToFloat( &(ret[2]) );

  if( _debug )
  	printf( "sensor value?: %d\n", s );

	val = ⦥

	return 0;
}


int
ER::SetVelocity(double lvel, double rvel)
{

	unsigned char ret[8];
  if( _debug )
	printf( "lvel: %f rvel: %f\n", lvel, rvel );

  send_command( MOTOR_0, GETEVENTSTATUS, 4, ret );
  
  if( _stopped )
  {
    send_command_2_arg( MOTOR_0, RESETEVENTSTATUS, 0x0300, 2, ret );
    send_command_2_arg( MOTOR_0, SETMOTORCMD, 0x6590, 2, ret );
    send_command_2_arg( MOTOR_0, SETPROFILEMODE, 0x0001, 2, ret );
  }
  else
  {
    send_command_2_arg( MOTOR_0, RESETEVENTSTATUS, 0x0700, 2, ret );
  
  }
  
  SpeedCommand( MOTOR_0, lvel, _motor_0_dir );
  
  send_command_4_arg( MOTOR_0, SETACCEL, 0x0000007E, 2, ret );


  send_command( MOTOR_1, GETEVENTSTATUS, 4, ret );
  if( _stopped )
  {
    send_command_2_arg( MOTOR_1, RESETEVENTSTATUS, 0x0300, 2, ret );
    send_command_2_arg( MOTOR_1, SETMOTORCMD, 0x6590, 2, ret );
    send_command_2_arg( MOTOR_1, SETPROFILEMODE, 0x0001, 2, ret );
  }
  else
  {
    send_command_2_arg( MOTOR_1, RESETEVENTSTATUS, 0x0700, 2, ret );
  }
  
  SpeedCommand( MOTOR_1, rvel, _motor_1_dir );
  send_command_4_arg( MOTOR_1, SETACCEL, 0x0000007E, 2, ret );


  send_command( MOTOR_0, UPDATE, 2, ret );
  send_command( MOTOR_1, UPDATE, 2, ret );

	_stopped = false;
  return 0;
}


void 
ER::Main()
{

	player_position2d_data_t data;
	int rtics, ltics, lvel, rvel;
	double lvel_mps, rvel_mps;
	pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);

	// push a pthread cleanup function that stops the robot
	pthread_cleanup_push(StopRobot,this);

	for(;;)
	{
		pthread_testcancel();
		ProcessMessages();
    
    	// handle pending config requests
	    //this->HandleConfig();

		//PLAYER_WARN("Done with handle config" );

		/* position command */

		//this->GetCommand();

		//PLAYER_WARN("Done with get Command config" );
		/* get battery voltage */

/*

		TODO:
			get hex to voltage to percentage conversion function

    	int volt;
	    if(GetBatteryVoltage(&volt) < 0) {
			PLAYER_WARN("failed to get voltage");
		}
		else printf("volt: %d\n", volt);

*/
	
		/* Get the 13 range sensor values */

/*

		TODO: 	read from config file to judge where the IRS are and what
			type they are
			
				hex => raw => distance for each type

		float val;
		for( int i = 0; i < 13; i++ ) {
			if( GetRangeSensor( i, &val ) < 0) {
				PLAYER_WARN("failed to get range sensor");
			}
		} 

*/

		/* get the odometry values */
		GetOdom( &ltics, &rtics, &lvel, &rvel );
		UpdateOdom( ltics, rtics );


		double tmp_angle;
		data.pos.px = this->_px;
		data.pos.py = this->_py;
		if(this->_pa < 0)
			tmp_angle = this->_pa + 2*M_PI;
		else
			tmp_angle = this->_pa;

		data.pos.pa = tmp_angle;

		data.vel.py = 0;
		lvel_mps = lvel * ER_MPS_PER_TICK;
		rvel_mps = rvel * ER_MPS_PER_TICK;
		data.vel.px = (lvel_mps + rvel_mps) / 2.0;
		data.vel.pa = (rvel_mps-lvel_mps) / 
                                             _axle_length;

                Publish(position_id, NULL, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, &data, sizeof(data));
//		PutData((unsigned char*)&data,sizeof(data),0,0);



	    usleep(ER_DELAY_US);

	} /* for */
	pthread_cleanup_pop(1);
  
}

////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int ER::ProcessMessage(MessageQueue * resp_queue,
                               player_msghdr * hdr,
                               void * data)
{
  assert(hdr);
  assert(data);
	
  if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, position_id))
  {
  	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.25;//htons((short) (250));
	geom.size.sl = 0.425;//htons((short) (425));    
    Publish(position_id, 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, position_id))
  {
  	player_position2d_power_config_t * powercfg = reinterpret_cast<player_position2d_power_config_t *> (data);

    printf("got motor power req: %d\n", powercfg->state);
	if(ChangeMotorState(powercfg->state) < 0)
	  Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_NACK, PLAYER_POSITION2D_REQ_MOTOR_POWER);
	else
	  Publish(position_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_MOTOR_POWER);
    return 0;
  }

  if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, position_id))
  {
  	player_position2d_cmd_vel_t & position_cmd = *reinterpret_cast<player_position2d_cmd_vel_t *> (data);

	double rotational_term;
	double command_rvel, command_lvel;
	double final_lvel = 0, final_rvel = 0;
	double last_final_lvel = 0, last_final_rvel = 0;

	// convert (tv,rv) to (lv,rv) and send to robot
	rotational_term = (position_cmd.vel.pa * M_PI / 180.0) * (_axle_length*1000.0) / 2.0;
	command_rvel = (position_cmd.vel.px) + rotational_term;
	command_lvel = (position_cmd.vel.px) - rotational_term;
    
//	printf( "position_cmd.xspeed: %d position_cmd.yawspeed: %d\n", position_cmd.xspeed, position_cmd.yawspeed );
	
	// sanity check on per-wheel speeds
	if(fabs(command_lvel) > ER_MAX_WHEELSPEED) {
		if(command_lvel > 0) {
			command_lvel = ER_MAX_WHEELSPEED;
			command_rvel *= ER_MAX_WHEELSPEED/command_lvel;
		}
		else {
			command_lvel = - ER_MAX_WHEELSPEED;
			command_rvel *= -ER_MAX_WHEELSPEED/command_lvel;
		}
	}
	if(fabs(command_rvel) > ER_MAX_WHEELSPEED) {
		if(command_rvel > 0) {
			command_rvel = ER_MAX_WHEELSPEED;
			command_lvel *= ER_MAX_WHEELSPEED/command_rvel;
		}
		else {
			command_rvel = - ER_MAX_WHEELSPEED;
			command_lvel *= -ER_MAX_WHEELSPEED/command_rvel;
		}
	}

	final_lvel = command_lvel;
	final_rvel = command_rvel;

  if( _debug )
    printf( "final_lvel: %f, final_rvel: %f\n", final_lvel, final_rvel );


	

	// TODO: do this min threshold smarter, to preserve desired travel 
	// direction

	if((final_lvel != last_final_lvel) ||
		(final_rvel != last_final_rvel)) {
		if( final_lvel * last_final_lvel < 0 || final_rvel * last_final_rvel < 0 ) {
//				PLAYER_WARN( "Changing motor direction, soft stop\n" );
			SetVelocity(0,0);
		}
		if(SetVelocity(final_lvel/10.0,final_rvel/10.0) < 0) {
			printf("failed to set velocity\n");
			pthread_exit(NULL);
		}
		if( (int) position_cmd.vel.px == 0 && (int) position_cmd.vel.pa == 0 )
		{
			printf( "STOP\n" );
			Stop( FULL_STOP+1 );			
		}

		last_final_lvel = final_lvel;
		last_final_rvel = final_rvel;
		MotorSpeed();
	}


    return 0;
  }
  
  return -1;
}



////////////////////
// util fcns
////////////////////

void
ER::MotorSpeed()
{
	unsigned char ret[8];

  send_command( MOTOR_0, GETEVENTSTATUS, 4, ret );
  send_command_2_arg( MOTOR_0, RESETEVENTSTATUS, 0x0700, 2, ret );
  send_command_4_arg( MOTOR_0, SETACCEL, 0x0000007A, 2, ret );
  send_command( MOTOR_1, GETEVENTSTATUS, 4, ret );
  send_command_2_arg( MOTOR_1, RESETEVENTSTATUS, 0x0700, 2, ret );
  send_command_4_arg( MOTOR_1, SETACCEL, 0x0000007A, 2, ret );

  send_command( MOTOR_0, UPDATE, 2, ret );
  send_command( MOTOR_1, UPDATE, 2, ret );
}

void
ER::SpeedCommand( unsigned char address, double speed, int dir ) {
	
  unsigned char ret[2];
  
	int whole = dir * (int) (speed * 16819.8);

  send_command_4_arg( address, SETVEL, whole, 2, ret );

//	printf( "speed: %f checksum: 0x%02x value: 0x%08x\n", speed, cmd[1], whole );
	
}

int 
ER::BytesToInt32(unsigned char *ptr)
{
  unsigned char char0,char1,char2,char3;
  int data = 0;

  char0 = ptr[3];
  char1 = ptr[2];
  char2 = ptr[1];
  char3 = ptr[0];

  data |=  ((int)char0)        & 0x000000FF;
  data |= (((int)char1) << 8)  & 0x0000FF00;
  data |= (((int)char2) << 16) & 0x00FF0000;
  data |= (((int)char3) << 24) & 0xFF000000;

  //this could just be ntohl

  return data;
}

float 
ER::BytesToFloat(unsigned char *ptr)
{
//  unsigned char char0,char1,char2,char3;
  float data = 0;

	sscanf( (const char *) ptr, "%f", &data );

  return data;
}

int 
ER::ComputeTickDiff(int from, int to) 
{
  int diff1, diff2;

  // find difference in two directions and pick shortest
  if(to > from) 
  {
    diff1 = to - from;
    diff2 = (-ER_MAX_TICKS - from) + (to - ER_MAX_TICKS);
  }
  else 
  {
    diff1 = to - from;
    diff2 = (from - ER_MAX_TICKS) + (-ER_MAX_TICKS - to);
  }

  if(abs(diff1) < abs(diff2)) 
    return(diff1);
  else
    return(diff2);

	return 0;
}

void
ER::UpdateOdom(int ltics, int rtics)
{
	
	int ltics_delta, rtics_delta;
	double l_delta, r_delta, a_delta, d_delta;
	int max_tics;
	static struct timeval lasttime;
	struct timeval currtime;
	double timediff;

	if(!this->_odom_initialized)
	{
		this->_last_ltics = ltics;
		this->_last_rtics = rtics;
		gettimeofday(&lasttime,NULL);
		this->_odom_initialized = true;
		return;
	}
  
//	ltics_delta = ComputeTickDiff(last_ltics,ltics);
//	rtics_delta = ComputeTickDiff(last_rtics,rtics);

	ltics_delta = ltics - this->_last_ltics;
	rtics_delta = rtics - this->_last_rtics;

  // mysterious rollover code borrowed from CARMEN
/* 
  if(ltics_delta > SHRT_MAX/2)
    ltics_delta += SHRT_MIN;
  if(ltics_delta < -SHRT_MIN/2)
    ltics_delta -= SHRT_MIN;
  if(rtics_delta > SHRT_MAX/2)
    rtics_delta += SHRT_MIN;
  if(rtics_delta < -SHRT_MIN/2)
    rtics_delta -= SHRT_MIN;
*/

  gettimeofday(&currtime,NULL);
  timediff = (currtime.tv_sec + currtime.tv_usec/1e6)-
             (lasttime.tv_sec + lasttime.tv_usec/1e6);
  max_tics = (int)rint(ER_MAX_WHEELSPEED / ER_M_PER_TICK / timediff);
  lasttime = currtime;
	
	if( _debug ) {
	  printf("ltics: %d\trtics: %d\n", ltics,rtics);
	  printf("ldelt: %d\trdelt: %d\n", ltics_delta, rtics_delta);
	}
  //printf("maxtics: %d\n", max_tics);

  if(abs(ltics_delta) > max_tics || abs(rtics_delta) > max_tics)
  {
    printf("Invalid odometry change (too big); ignoring\n");
    return;
  }

  l_delta = ltics_delta * ER_M_PER_TICK;
  r_delta = rtics_delta * ER_M_PER_TICK;


  a_delta = (r_delta - l_delta) / ( _axle_length );
  d_delta = (l_delta + r_delta) / 2.0;

  this->_px += d_delta * cos(this->_pa + (a_delta / 2));
  this->_py += d_delta * sin(this->_pa + (a_delta / 2));
  this->_pa += a_delta;
  this->_pa = NORMALIZE(this->_pa);
  
	if( _debug ) {
		printf("er: pose: %f,%f,%f\n", this->_px,this->_py,this->_pa * 180.0 / M_PI);
	}
  this->_last_ltics = ltics;
  this->_last_rtics = rtics;
}


int 
ER::ChangeMotorState(int state)
{
/*
  unsigned char buf[1];
  if(state)
    buf[0] = TROGDOR_ENABLE_VEL_CONTROL;
  else
    buf[0] = TROGDOR_DISABLE_VEL_CONTROL;
  return(WriteBuf(buf,sizeof(buf)));
*/
	return 0;
}

static void
StopRobot(void* erdev)
{
  ER* er = (ER*)erdev;

//  if(er->Stop( FULL_STOP ) < 0)
//    PLAYER_ERROR("failed to stop robot on thread exit");

	er->Stop(FULL_STOP );
}