/********************************************************* * * 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.5static 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);