orc_arm_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 "arm_low_level.h"
#include <carmen/serial.h>
#include <sys/ioctl.h>
#include <limits.h>

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

#define ORC_STATUS 0x2A

#define ORC_LEFT_SONAR_PING 4
#define ORC_RIGHT_SONAR_PING 6

#define ORC_LEFT_SONAR_ECHO 49
#define ORC_RIGHT_SONAR_ECHO 51

#define ORC_SONAR_PING 6
#define ORC_SONAR_ECHO 7

#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_PINMODE 4
#define ORC_RIGHT_PINMODE 5

#define ORC_LEFT_MOTOR_QUAD_PORT 16
#define ORC_RIGHT_MOTOR_QUAD_PORT 18

#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 255
#define ORC_FF_GAIN ((ORC_MAX_PWM / ORC_MAX_ANGULAR_VEL) * 0.9)
#define ORC_P_GAIN 8
#define ORC_I_GAIN 3
#define ORC_D_GAIN 15

#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

#define ORC_DIGITAL_IN_PULL_UP 1
#define ORC_DIGITAL_IN 6

#define ORC_SERVO_CURRENT 35
#define ORC_SERVO_PWM_STATE 8
#define ORC_SERVO_PIN 5

// velezj: For RssII Arm
#define ORC_SHOULDER_MOTOR 1
#define ORC_SHOULDER_MOTOR_ACTUAL_PWM 17
#define ORC_SHOULDER_MOTOR_DIR 25
#define ORC_SHOULDER_MOTOR_QUAD_PORT 16
#define ORC_SHOULDER_MOTOR_ENCODER 7
#define ORC_SHOULDER_MOTOR_DIRECTION_SIGN 1
#define ORC_ELBOW_MOTOR 3
#define ORC_ELBOW_MOTOR_ACTUAL_PWM 23
#define ORC_ELBOW_MOTOR_DIR 26
#define ORC_ELBOW_MOTOR_QUAD_PORT 18
#define ORC_ELBOW_MOTOR_ENCODER 10
#define ORC_ELBOW_MOTOR_DIRECTION_SIGN -1
#define ORC_ARM_GEAR_REDUCTION ( 65.5 * 5.0 )
#define ORC_ARM_TICKS_PER_RADIAN ( ORC_ENCODER_RESOLUTION * ORC_ARM_GEAR_REDUCTION / ( 2.0 * M_PI ) )
#define ORC_ELBOW_MAX_PWM 40 // 27
#define ORC_ELBOW_MIN_PWM 35 // 22
#define ORC_SHOULDER_MAX_PWM 50 // 37
#define ORC_SHOULDER_MIN_PWM 42 // 30
#define ORC_ARM_P_GAIN 8
#define ORC_ARM_D_GAIN 15
#define ORC_ARM_I_GAIN 0 // 3
#define ORC_ARM_MAX_ANGULAR_VEL 0.0035 // Radians / second
#define ORC_ARM_MIN_ANGULAR_VEL 0.00125 // Radians / second 
//#define ORC_ARM_FF_GAIN ((ORC_ARM_MAX_PWM / ORC_ARM_MAX_ANGULAR_VEL) * 0.9)
#define ORC_ARM_FF_GAIN ( (90 / ORC_ARM_MAX_ANGULAR_VEL ) * 0.9 );
#define ORC_ARM_THETA_P_GAIN 0.008
#define ORC_ARM_THETA_D_GAIN 0.028 // 0.018
#define ORC_ARM_THETA_I_GAIN 0.000 // 0.003
#define ORC_ARM_GRIPPER_SERVO 0 // 0
#define ORC_ARM_GRIPPER_MIN_PWM 8600
#define ORC_ARM_GRIPPER_PWM_PER_RADIAN 2214.2 

static double acceleration;
static double deceleration;

static double x, y, theta;
static double displacement, rotation;
static double left_velocity, right_velocity;
static double left_iTerm = 0.0, right_iTerm = 0.0;
static double left_vel_ramp = 0.0, right_vel_ramp = 0.0;
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;

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 char bumpers[4];
static int gripper_state = 0;
static int serial_fd = -1;

// velezj: rssII Arm
static double gripper_theta = 0.0, gripper_desired_theta = 0.0;
static double shoulder_theta = 0.0, elbow_theta = 0.0;
static double shoulder_desired_theta = 0.0, elbow_desired_theta = 0.0;
static double shoulder_theta_iTerm = 0.0, elbow_theta_iTerm = 0.0;
static double shoulder_theta_error_prev = 0.0, elbow_theta_error_prev = 0.0;
static double shoulder_iTerm = 0.0, elbow_iTerm = 0.0;
static double shoulder_error_prev = 0.0, elbow_error_prev = 0.0;
static double shoulder_desired_angular_velocity = 0.0, elbow_desired_angular_velocity = 0.0;
static double shoulder_angular_velocity, elbow_angular_velocity;
static int shoulder_pwm, elbow_pwm;
static int shoulder_last_tick = 0, elbow_last_tick = 0;

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 arm...\n");
    carmen_arm_direct_initialize(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;
  
  static unsigned char count = 0;

  //  carmen_warn("Sent packet %x to %d : %d\n", byte[0], where, count);
  count++;
  if (count == 0x40)
    count = 0;

  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) | count; // was | 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_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);

  //  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;
    count++;
  } while (count < 3);

  return -1;
}

static void command_velocity(double desired_velocity, double current_velocity,
           int current_pwm, unsigned char WHICH_MOTOR)
{
  double desired_angular_velocity, current_angular_velocity;
  double desired_velocity_ramp = 0;
  double ffTerm = 0.0, pTerm = 0.0, dTerm = 0.0, velError = 0.0;
  double * iTermPtr;
  double * velErrorPrevPtr;
  double * velRampPtr;
  unsigned char command_pwm;
  unsigned char dir;
  unsigned char buffer[4];
  char which;

  if (WHICH_MOTOR == ORC_LEFT_MOTOR) {
    which = 'L';
    iTermPtr = &left_iTerm;
    velRampPtr = &left_vel_ramp;
    velErrorPrevPtr = &left_error_prev;
  }
  else {
    which = 'R';
    iTermPtr = &right_iTerm;
    velRampPtr = &right_vel_ramp;
    velErrorPrevPtr = &right_error_prev;
  }

  if (desired_velocity == 0) {
    *velRampPtr = 0;
  }
  else if (desired_velocity > *velRampPtr) {
    (*velRampPtr) += acceleration * time_since_last_command;
    printf("accel %f %f %f\n", acceleration * time_since_last_command,
	   acceleration, time_since_last_command);
    //    (*velRampPtr) += ORC_VEL_ACCEL_TEMP * time_since_last_command;
    //    printf("accel %f\n", ORC_VEL_ACCEL_TEMP * time_since_last_command);
    if (*velRampPtr > desired_velocity) {
      *velRampPtr = desired_velocity;
    }
  }
  else if (desired_velocity < *velRampPtr) {
    (*velRampPtr) -= deceleration * time_since_last_command;
    printf("decel %f %f %f\n", deceleration * time_since_last_command,
    	   deceleration, time_since_last_command);
    //    (*velRampPtr) -= ORC_VEL_DECEL_TEMP * time_since_last_command;
    //    printf("decel %f\n", ORC_VEL_DECEL_TEMP * time_since_last_command);
    if (*velRampPtr < desired_velocity) {
      *velRampPtr = desired_velocity;
    }
  }

  desired_velocity_ramp = *velRampPtr;

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


  if (fabs(desired_angular_velocity) > .001) {
      /* Nick, what did you mean to do here?  I don't understand these
         comparisons.  Do you mean < 3 and &&? */
    if (desired_angular_velocity > ORC_MAX_ANGULAR_VEL)
      desired_angular_velocity = ORC_MAX_ANGULAR_VEL;
    if (desired_angular_velocity < -ORC_MAX_ANGULAR_VEL)
      desired_angular_velocity = -ORC_MAX_ANGULAR_VEL;

    velError = (desired_angular_velocity - current_angular_velocity);
    ffTerm = desired_angular_velocity * ORC_FF_GAIN;
    pTerm = velError * ORC_P_GAIN;
    *iTermPtr += velError * ORC_I_GAIN;