/********************************************************* * * 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 Armstatic 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;