orc_lib.c

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

 /*********************************************************
 *
 * This source code is part of the Carnegie Mellon Robot
 * Navigation Toolkit (CARMEN)
 *
 * CARMEN Copyright (c) 2002 Michael Montemerlo, Nicholas
 * Roy, Sebastian Thrun, Dirk Haehnel, Cyrill Stachniss,
 * and Jared Glover
 *
 * CARMEN is free software; you can redistribute it and/or 
 * modify it under the terms of the GNU General Public 
 * License as published by the Free Software Foundation; 
 * either version 2 of the License, or (at your option)
 * any later version.
 *
 * CARMEN is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied 
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  See the GNU General Public License for more 
 * details.
 *
 * You should have received a copy of the GNU General 
 * Public License along with CARMEN; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place, 
 * Suite 330, Boston, MA  02111-1307 USA
 *
 ********************************************************/

#include <carmen/carmen.h>
#include "../base_low_level.h"
#include <carmen/carmenserial.h>
#include <sys/ioctl.h>
#include <limits.h>

#define ORC_MASTER 0
#define ORC_SLAVE 1
#define ORC_PAD 2

#define ORC_STATUS 0x2A

// Sonar defines

#define ORC_LEFT_SONAR_PING_PIN 4
#define ORC_LEFT_SONAR_ECHO_PIN 5
#define ORC_RIGHT_SONAR_PING_PIN 6
#define ORC_RIGHT_SONAR_ECHO_PIN 7
#define ORC_SONAR_PING_MODE 6
#define ORC_SONAR_ECHO_MODE 7

#define ORC_LEFT_SONAR_PING 0
#define ORC_RIGHT_SONAR_PING 1
#define ORC_LEFT_SONAR_RANGE 49
#define ORC_RIGHT_SONAR_RANGE 51

// Arm defines

#define ORC_SERVO_PIN_0 0
#define ORC_SERVO_PIN_1 1
#define ORC_SERVO_PIN_2 2
#define ORC_SERVO_PIN_3 3

#define ORC_GRIPPER_PIN 12

#define ORC_SERVO_CURRENT 35
#define ORC_SERVO_PWM_STATE 8
#define ORC_SERVO_MODE 5

// Bumper defines


#define ORC_BUMPER_PIN_0 8
#define ORC_BUMPER_PIN_1 9
#define ORC_BUMPER_PIN_2 10
#define ORC_BUMPER_PIN_3 11

#define ORC_DIGITAL_IN_PULL_UP 1
#define ORC_DIGITAL_IN 6

// Configuring IR sensor to use Sonar ports
#define ORC_LEFT_IR_PING 5
#define ORC_RIGHT_IR_PING 7

// Configure pins to digital
#define ORC_LEFT_IR_ECHO 4
#define ORC_RIGHT_IR_ECHO 6

// Sets modes for pins  
#define ORC_IR_PING 3 // Digital Out; 
#define ORC_IR_ECHO 1 // Digital In (Pull-Up)

#define ORC_LEFT_MOTOR 0
#define ORC_RIGHT_MOTOR 2

#define ORC_LEFT_MOTOR_ACTUAL_PWM 14
#define ORC_RIGHT_MOTOR_ACTUAL_PWM 19

#define ORC_LEFT_MOTOR_SLEW 15
#define ORC_RIGHT_MOTOR_SLEW 21

#define ORC_LEFT_PINMODE 4
#define ORC_RIGHT_PINMODE 5

#define ORC_LEFT_MOTOR_QUAD_PORT 16
#define ORC_RIGHT_MOTOR_QUAD_PORT 18

#define ORC_LEFT_ENCODER_STATE 4
#define ORC_RIGHT_ENCODER_STATE 5

#define ORC_QUAD_PHASE_FAST 14

#define ORC_LEFT_MOTOR_DIR 25
#define ORC_RIGHT_MOTOR_DIR 26
#define ORC_FORWARD 1
#define ORC_BACKWARD 2

#define ORC_MAX_ANGULAR_VEL 8.0 // Radians / seconds

#define ORC_MAX_PWM 250
#define ORC_FF_GAIN ((ORC_MAX_PWM / ORC_MAX_ANGULAR_VEL) * 0.9)
#define ORC_P_GAIN 20
#define ORC_D_GAIN 5

#define ORC_VEL_ACCEL_TEMP 0.9
#define ORC_VEL_DECEL_TEMP 0.4

#define ORC_LEFT_MOTOR_ENCODER 7
#define ORC_RIGHT_MOTOR_ENCODER 10

#define ORC_MASTER_TIME 4
#define ORC_SLAVE_TIME 48

#define ORC_WHEEL_DIAMETER .125
#define ORC_WHEEL_BASE .43
#define ORC_ENCODER_RESOLUTION 500
#define ORC_GEAR_RATIO 65.5

static double acceleration;
static double deceleration;

static double x, y, theta;
static double displacement, rotation;
static double left_velocity, right_velocity;
static double left_error_prev = 0.0, right_error_prev = 0.0;
static double left_desired_velocity = 0, right_desired_velocity = 0;
static double left_range, right_range;
static int initialized = 0;
static int sonar_on = 1;


// We ignore the d-term the first time we enter the control loop
// after receiving a new velocity command. This is because the
// d-term captures the derivative of the error, which is not 
// meaningful if the error is caused by the command that moved
// the desired velocity set point.
static int ignore_left_d_term = 1;
static int ignore_right_d_term = 1;

static int left_pwm = 0, right_pwm = 0;
static short last_master_ticks;
static short last_slave_ticks;
static int left_last_tick, right_last_tick;
static double time_since_last_command;
static double last_command_time;

static double servo_state[4];
static double servo_current[2];

static unsigned char irs[4];
static unsigned char bumpers[4];
static int gripper_state = 0;
static int serial_fd = -1;

static double left_displacement, right_displacement;
static double delta_slave_time;

static void recover_failure(void)
{
  printf("Recovering from Serial Failure\n");
  initialized = 0;
  if (serial_fd >= 0) {
    close(serial_fd);
    serial_fd = -1;
  }
  while (serial_fd < 0) {
    sleep(1);
    carmen_warn("Trying to reconnect to base...\n");
    carmen_base_direct_initialize_robot(NULL,  NULL);
  }
}

unsigned char create_checksum(unsigned char *buffer, int size)
{
  unsigned char checksum = 0;
  int i;

  for (i = 0; i < size; i++)
    checksum = (checksum << 1) + buffer[i] + (checksum & 0x80 ? 1 : 0);

  return checksum;
}

void send_packet(unsigned char *byte, int length,  unsigned char where)
{
  static unsigned char *buffer;
  static unsigned char size;
  static int buffer_size = 0;
  int i;
  int num_written;

  if (buffer_size == 0) {
    buffer = (unsigned char *)calloc(sizeof(unsigned char), length+4);
    carmen_test_alloc(buffer);
    buffer_size = length+4;
  } else if (buffer_size < length+4) {
    buffer = (unsigned char *)realloc
      (buffer, sizeof(unsigned char)*(length+4));
    carmen_test_alloc(buffer);
    buffer_size = length+4;
  }

  size = length+4;

  buffer[0] = 0xED;
  buffer[1] = size;
  buffer[2] = (where << 6) | 0xF;
  for (i = 0; i < length; i++)
    buffer[i+3] = byte[i];
  buffer[length+3] = create_checksum(buffer, length+3);

  num_written = carmen_serial_writen(serial_fd, buffer, length+4);
  if (num_written < 0)
    recover_failure();
}

static unsigned char *check_packet_length(int packet_length)
{
  unsigned char *buffer = (unsigned char *)calloc(sizeof(unsigned char), packet_length);
  carmen_test_alloc(buffer);

  return buffer;
}

static unsigned char *read_packet(void)
{
  unsigned char byte, routing;
  unsigned char *buffer = NULL;
  unsigned char *data_ptr;
  unsigned char checksum;
  //  fd_set rfds;

  int num_ready_chars;
  int packet_length;
  struct timeval timeout;
  int num_ready;

  int count = 0;
  int packet_count = 0;

  do {
    do {
      num_ready_chars = carmen_serial_numChars(serial_fd);
      
      if (num_ready_chars == -1) 
	return NULL;
      
      count = 0;
      while (count < 50 && num_ready_chars == 0) {
        timeout.tv_sec = 0;
        timeout.tv_usec = 1000;
	//	FD_ZERO(&rfds);
	//	FD_SET(0, &rfds);
	//        num_ready = select(1, &rfds, NULL, NULL, &timeout);
        num_ready = select(0, NULL, NULL, NULL, &timeout);
        num_ready_chars = carmen_serial_numChars(serial_fd);
        count++;
      }

      if (num_ready_chars < 1) {
	return NULL;
      } 
      //      carmen_warn("Ready: %d\n", num_ready_chars);
      
      if (carmen_serial_readn(serial_fd, &byte, 1) < 0)
	return NULL;
    } while (byte != 0xED);
    
    if (carmen_serial_readn(serial_fd, &byte, 1) < 0) 
      return NULL;
    
    packet_length = byte;
    
    buffer = check_packet_length(packet_length);
    buffer[0] = 0xED;
    buffer[1] = byte;
    if (carmen_serial_readn(serial_fd, &byte, 1) < 0) 
      return NULL;
    
    buffer[2] = byte; 
    data_ptr = buffer+3;
    routing = byte >> 6;

    if (carmen_serial_readn(serial_fd, data_ptr, packet_length-3) < 0) 
      return NULL;
  
    checksum = create_checksum(buffer, packet_length-1);
    
    if (checksum != buffer[packet_length-1]) {
      carmen_warn("Corrupted data from serial line. "
		  "Dropping packet.\n");
      free(buffer);
      buffer = NULL;
    }
    
    if (routing == ORC_PAD) {
      //      carmen_warn("Tossing orc_pad packet\n");
      free(buffer);
      buffer = NULL;
    } 
    //    else
    //      carmen_warn("Got packet %x from %d : %d\n", buffer[3],
    //		  routing, byte & 0x3f);    
    packet_count++;
    //  } while (!buffer && packet_count < 3);
  } while (!buffer);

  //  carmen_warn("Returning buffer of size %d\n", packet_length);
  
  return buffer;
}

static int wait_for_ack(void)
{
  unsigned char *buffer;
  unsigned char response;
  
  buffer = read_packet();
  if (buffer == NULL)
    return -1;

  response = buffer[3];
  free(buffer);

  if (response != 0)
    return -1;

  return 0;
}

static int send_packet_and_ack(unsigned char *byte, int length,  
			       unsigned char where)
{
  int count;
  int err;

  count = 0;
  do {
    send_packet(byte, length, where);
    err = wait_for_ack();
    if (err == 0)
      return 0;
    //    carmen_warn("Resending: %d\n", count);
    count++;
  } while (count < 3);

  return -1;
}
void carmen_base_command_velocity(double desired_velocity, 
				  double current_velocity,
				  unsigned char WHICH_MOTOR)
{
  double desired_angular_velocity, current_angular_velocity;
  double pTerm = 0.0, dTerm = 0.0, velError = 0.0;
  double *velErrorPrevPtr;
  double pGain = ORC_P_GAIN;

  int current_pwm;
  int new_pwm;

  unsigned char command_pwm;
  unsigned char dir;
  unsigned char buffer[4];
  int *ignore_d_term;

  if (WHICH_MOTOR == ORC_LEFT_MOTOR) {
    velErrorPrevPtr = &left_error_prev;
    current_pwm = left_pwm;
    ignore_d_term = &ignore_left_d_term;
    pGain = pGain * 1.2;
  } else {
    velErrorPrevPtr = &right_error_prev;
    current_pwm = right_pwm;
    ignore_d_term = &ignore_right_d_term;
  }

  desired_angular_velocity = desired_velocity / (ORC_WHEEL_DIAMETER/2.0);
  current_angular_velocity = current_velocity / (ORC_WHEEL_DIAMETER/2.0);