orc_drive_lib.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

       ORC_LEFT_MOTOR);
  carmen_base_command_velocity(right_desired_velocity, s_right_velocity,
       ORC_RIGHT_MOTOR);

  return 0;
}

// ----- GETTING STATE ----- //

// updates the internal state variables
int carmen_base_direct_update_status(double* packet_timestamp) //
{
  // fill in the time stamp and update internal time
  double curr_time = carmen_get_time();

  //s_delta_time = curr_time - s_time;
  s_time = curr_time;
  *packet_timestamp = curr_time;
  
  // sonar is not implemented in orc 5 so we don't deal with this
  s_left_range = 0.0;
  s_right_range = 0.0;

  // ir deleted since not being used

  // bumpers
  s_bumpers[0] = orc_digital_read( s_orc, ORC_BUMPER_PORT0 );
  s_bumpers[1] = orc_digital_read( s_orc, ORC_BUMPER_PORT1 );
  s_bumpers[2] = orc_digital_read( s_orc, ORC_BUMPER_PORT2 );
  s_bumpers[3] = orc_digital_read( s_orc, ORC_BUMPER_PORT3 );

  // read encoders
  int curr_left_tick = orc_quadphase_read( s_orc, ORC_LEFT_ENCODER_PORT );
  int curr_right_tick = orc_quadphase_read( s_orc, ORC_RIGHT_ENCODER_PORT );

  int left_delta = compute_delta_ticks_mod( curr_left_tick, s_left_tick ) * ORC_LEFT_REVERSE_ENCODER ;
  
  int right_delta = compute_delta_ticks_mod( curr_right_tick, s_right_tick ) * ORC_RIGHT_REVERSE_ENCODER ;
 
  s_left_displacement = delta_tick_to_metres( left_delta );
  s_right_displacement = delta_tick_to_metres( right_delta ); 
  s_left_tick = curr_left_tick;
  s_right_tick = curr_right_tick;

  // update velocity information
 
  int left_delta_velocity = orc_quadphase_read_velocity_signed( s_orc, ORC_LEFT_ENCODER_PORT );
  int right_delta_velocity = orc_quadphase_read_velocity_signed( s_orc, ORC_RIGHT_ENCODER_PORT );

  // looking for underflow or overflow
  /*

  if( left_delta_velocity > SHORT_MAX/2 )
    left_delta_velocity = SHORT_MAX - left_delta_velocity;
  if( right_delta_velocity  > SHORT_MAX/2 )
    right_delta_velocity = SHORT_MAX - right_delta_velocity;

  if( left_delta_velocity < -1.0 * SHORT_MAX/2 )
    left_delta_velocity = SHORT_MAX + left_delta_velocity;
  if( right_delta_velocity  < -1.0 * SHORT_MAX/2 )
    right_delta_velocity = SHORT_MAX + right_delta_velocity;

  */

  s_left_velocity = -1.0 * delta_tick_to_metres( left_delta_velocity ) * 60.0 ;  //s_left_displacement / s_delta_time;
  s_right_velocity = delta_tick_to_metres( right_delta_velocity ) * 60.0 ;  //s_right_displacement / s_delta_time;



  //printf( "  *** ticks: left %d, right %d \n", left_delta, right_delta );
  //printf( "  *** ticks: left %d, right %d \n", left_delta_velocity , right_delta_velocity );
  //printf( "  *** vels: left %f, right %f \n", s_left_velocity, s_right_velocity );

  // update position information
  double displacement = ( s_left_displacement + s_right_displacement )/2;
  double rotation = atan2( s_right_displacement - s_left_displacement, 
			   ORC_WHEEL_BASE );
  s_x = s_x + displacement*cos( s_theta );
  s_y = s_y + displacement*sin( s_theta );
  s_theta = carmen_normalize_theta( s_theta + rotation );

  return 0;
}

int carmen_base_direct_get_integrated_state(double *x_p, double *y_p,
					    double *theta_p, double *tv_p,
					    double *rv_p) //
{
  *x_p = s_x;
  *y_p = s_y;
  *theta_p = s_theta;
  *tv_p = (s_left_velocity+s_right_velocity)/2;
  *rv_p = atan2(s_right_velocity-s_left_velocity, ORC_WHEEL_BASE);  
  return 0;
}

int carmen_base_query_low_level(double *left_disp, double *right_disp,
				double *delta_time)
{
  // update internal variables
  int err;
  err = carmen_base_direct_update_status(NULL);
  if (err < 0)
    return -1;

  // return displacement information
  *left_disp = s_left_displacement;
  *right_disp = s_right_displacement;
  *delta_time = s_delta_time;
  return 0;
}


// this contains hard coded values from RSS; not updated since
// current orc 5 robot does not use sonars
int carmen_base_direct_get_sonars(double *ranges, carmen_point_t *positions,
				  int num_sonars) //
{
  if (num_sonars >= 1) {
    ranges[0] = s_left_range;
    positions[0].x = .05;
    positions[0].y = -.10;
    positions[0].theta = -M_PI/2;
  }
  if (num_sonars >= 2) {
    ranges[1] = s_right_range;
    positions[1].x = .05;
    positions[1].y = .10;
    positions[1].theta = M_PI/2;
  }

  return 0;
}

int carmen_base_direct_get_bumpers(unsigned char *bumpers_p, int num_bumpers) //
{
  if (num_bumpers >= 1)
    bumpers_p[0] = s_bumpers[0];
  if (num_bumpers >= 2)
    bumpers_p[1] = s_bumpers[1];
  if (num_bumpers >= 3)
    bumpers_p[2] = s_bumpers[2];
  if (num_bumpers >= 4)
    bumpers_p[3] = s_bumpers[3];

  return 4;
}

// ----- HELPER FUNCTIONS ----- //
static double delta_tick_to_metres(int delta_tick)
{
  double ticks2rev = 1.0 / ORC_TICKS_PER_REVOLUTION;
  double revolutions = (double)delta_tick * ticks2rev ;
  double radians = revolutions*2*M_PI;
  double metres = radians*(ORC_WHEEL_DIAMETER/2.0);

  return metres;
}

// we assume the direction is the smallest route around the circle
static int compute_delta_ticks_mod( int curr, int prev )
{

  // compute the number of ticks traversed short and long way around
  // and pick the path with the minimal distance
  int abs_reg = abs( curr - prev );
  int abs_opp = 65536 - abs_reg;
  int actual_delta = min( abs_reg, abs_opp );

  // give the angle the correct sign -- ccw is positive
  if( ( curr > prev && actual_delta == abs_reg ) || 
      ( curr < prev && actual_delta == abs_opp ) )
    {
      actual_delta = -actual_delta;
    }
  
  return actual_delta;
}

static int min( int a, int b )
{
  return ( a < b ) ? a : b ;
}

/* --------------------------------------------------------------

             FUNCTIONS BELOW ARE NOT YET IMPLEMENTED
	        DO NOT APPLY YET... OR EVER?? 
 	  -- some not even implemented in the RSS code

   --------------------------------------------------------------*/

// ---- SETTING CONTROLS ---- //
int carmen_base_direct_sonar_on(void) //
{
  printf("carmen_base_direct_sonar_on\n");
  s_sonar_on = 1;

  return 2;
}

int carmen_base_direct_sonar_off(void) //
{
  s_sonar_on = 0;
  return 2;
}


int carmen_base_direct_set_acceleration(double new_acceleration) //
{
  s_acceleration = new_acceleration;
  return 0;
}

int carmen_base_direct_set_deceleration(double new_deceleration) //
{
  s_deceleration = new_deceleration;
  return 0;
}

// ----- GETTING STATE INFORMATION ----- //
int carmen_base_direct_get_state(double *disp_p __attribute__ ((unused)), 
				 double *rot_p __attribute__ ((unused)),
				 double *tv_p __attribute__ ((unused)), 
				 double *rv_p __attribute__ ((unused)))//
{

  carmen_die("Turn base_hardware_integrator on\n");

  return 0;
}

int carmen_base_query_encoders(double *disp_p __attribute__ ((unused)),
			       double *rot_p __attribute__ ((unused)),
			       double *tv_p __attribute__ ((unused)),
			       double *rv_p __attribute__ ((unused))) //
{
  carmen_die("Turn base_hardware_integrator on\n");

  return 0;
}


// ----- LOW LEVEL COMMUNICATION ----- //
int carmen_base_direct_send_binary_data(unsigned char *data, int size)//
{
  return carmen_serial_writen(s_serial_fd, data, size);
  return 0;
}

int  carmen_base_direct_get_binary_data(unsigned char **data,
					int *size)
{
  *data = NULL;
  *size = 0;
  return 0;
}