/********************************************************* * * 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 <carmen/drive_low_level.h>#include <limits.h>#include "pioneer_params.h"#define        PIONEER_SERIAL_TIMEOUT            5static int dev_fd;static int sonar_is_on = 0;static int pioneer_version = 0;static double angle_conv_factor, dist_conv_factor, vel_conv_factor;static double range_conv_factor, diff_conv_factor, vel2_divisor;/*** Some pioneer internal constants.  I have them defined here since the ***//*** outside world doesn't need to know about them.                       ***/#define PIONEER_PACKET_HEADER  "\xFA\xFB"#define PIONEER_SYNC0           0#define PIONEER_SYNC1           1#define PIONEER_SYNC2           2#define PIONEER_PULSE           0#define PIONEER_OPEN            1#define PIONEER_CLOSE           2#define PIONEER_POLLING         3#define PIONEER_ENABLE          4#define PIONEER_SETA            5#define PIONEER_VEL             11#define PIONEER_RVEL            21#define PIONEER_SETRA           23#define PIONEER_SONAR           28#define PIONEER_STOP            29#define PIONEER_VEL2            32#define PIONEER_NEGATIVE_INT    0x1B#define PIONEER_STRING          0x2B#define PIONEER_POSITIVE_INT    0x3B#define PIONEER_ON              1#define PIONEER_OFF             0#define PIONEER_MOTOR_NO_POWER  0x31#define PIONEER_MOTOR_STOPPED   0x32#define PIONEER_MOTOR_MOVING    0x33#define PIONEER_BATTERY_CONVERSION_FACTOR           0.1#define PIONEER_COMPASS_CONVERSION_FACTOR           2.0#define PIONEER_MAX_NUM_SONARS   32#define PIONEER_MAX_NUM_BUMPERS  14/* Cyrill, 14.01.2004   according to the ActivMedia manual the discretization   is 4mm/sec, not 5mm/sec. So the factor should be 250.0   and not 200.0 *///#define PIONEER1_VEL2_CONVERSION_FACTOR             200.0             // m/s => 5mm/sec#define PIONEER1_VEL2_CONVERSION_FACTOR             250.0             // m/s => 4mm/sec#define PIONEER2_VEL2_CONVERSION_FACTOR             50.0              // m/s => 2cm/sec/*** When we receive an information packet back from the robot, it is     ***//*** initially read into a raw information packet as if to a big char     ***//*** buffer.  It is then converted to a regular information packet, which ***//*** contains more useful data types.                                     ***/struct pioneer_raw_information_packet {  unsigned char motor_status;  unsigned char x[2];  unsigned char y[2];  unsigned char orientation[2];  unsigned char vl[2];  unsigned char vr[2];  unsigned char battery;  unsigned char bumper[2];  unsigned char servo_position[2];  unsigned char pulse_width[2];  unsigned char compass_heading;  unsigned char num_sonar_readings;  struct   {    unsigned char n;    unsigned char range[2];  } sonar[3*PIONEER_MAX_NUM_SONARS];};typedef enum {PIONEER_NO_POWER, PIONEER_STOPPED, PIONEER_MOVING} motor_status_t;static struct pioneer_raw_information_packet raw_state;int pioneer_vel2(int vl, int vr) ;/*** TABLE OF CONTENTS ***//*** Higher-level robot functions ***/static int pioneer_buf_to_checksum(unsigned char *buf) {  return 256*buf[0] + buf[1];}static int pioneer_buf_to_int(unsigned char *buf) {  int val = buf[0] + 256*buf[1];  if (val > SHRT_MAX)    val -= USHRT_MAX;  return val;}static int pioneer_buf_to_unsigned_int(unsigned char *buf) {  int val = buf[0] + 256 * (0x7F & buf[1]);    return val;}static void pioneer_int_to_buf(unsigned char *buf, int n) {  if (n >= 0)    buf[0] = PIONEER_POSITIVE_INT;  else    buf[0] = PIONEER_NEGATIVE_INT;  n = abs(n);  buf[1] = n & 0x00FF;  buf[2] = (n & 0xFF00) >> 8;} static int pioneer_calculate_checksum(unsigned char *ptr, int length) {  unsigned int c = 0;    while(length > 1)     {      c += (*(ptr)<<8) | *(ptr+1);      c = c & 0xffff;      length -= 2;      ptr += 2;    }  if(length > 0)     c = c ^ (int)*(ptr++);  return(c);}/*** Return value:  the length of the string read                       ***/static int pioneer_read_string(unsigned char *buf, double read_timeout, double * packet_timestamp) {  int c = 0;  int header_length = strlen(PIONEER_PACKET_HEADER);  unsigned char header[header_length+1];  unsigned char length_char[1];  int i;  double start_time, current_time;  double header_time;  for (i = 0; i <= header_length; ++i)    header[i] = '\0';  start_time = carmen_get_time();  header_time = start_time;  do     {                                  /* busy waits until we read the   */      for (i = 1; i < header_length; ++i) /* command header                 */	header[i-1] = header[i];      carmen_serial_readn(dev_fd, header + header_length - 1, 1);      header_time=carmen_get_time();      if (read_timeout > 0) {	current_time = carmen_get_time();	if (current_time - start_time > read_timeout)	  // timeout	  return -1;      }    }   while (strcmp((char *)header, PIONEER_PACKET_HEADER) != 0);   carmen_serial_readn(dev_fd, length_char, 1);  c = (int) length_char[0];             /* the length, including chksum   */  carmen_serial_readn(dev_fd, buf, c);  c -= 2;                               /* removes checksum from length,  */                                        /* checksum is now at buf+c       */  if (pioneer_calculate_checksum(buf, c) != pioneer_buf_to_checksum(buf+c))     {      fprintf(stderr, "Checksum error on packet ");      for (i = 0; i < c; ++i)	fprintf(stderr, "%c", buf[i]);      fprintf(stderr, "\n");      return pioneer_read_string(buf, read_timeout, NULL);    }  if (packet_timestamp)	*packet_timestamp=header_time;  return c;}static int pioneer_send_string(unsigned char *ptr, int length) {  int send_length = length + 5;         /* adds a 2-byte packet header,   */                                        /* a 1-byte length (inc. chksum), */                                        /* and a 2-byte checksum          */  unsigned char cmd[256] = PIONEER_PACKET_HEADER;  int checksum;  int i;  if (length <= 0)    return 0;  cmd[2] = (unsigned char) length+2;  for (i = 0; i < length; ++i)    cmd[i+3] = ptr[i];  checksum = pioneer_calculate_checksum(ptr, length);  cmd[send_length-2] = (unsigned char) ((checksum & 0xFF00) >> 8);  cmd[send_length-1] = (unsigned char) (checksum & 0x00FF);  return carmen_serial_writen(dev_fd, cmd, send_length);}static int pioneer_send_command0(char cmd) {  char buf[256];  buf[0] = cmd;  return pioneer_send_string((unsigned char *)buf, 1);}static int pioneer_send_command1(char cmd, int arg1) {  char buf[256];    buf[0] = cmd;  pioneer_int_to_buf((unsigned char *)(buf+1), arg1);  return pioneer_send_string((unsigned char *)buf, 4);}static intpioneer_sonar(int b) {  if (pioneer_version == 1 && b == 0) {    int result = pioneer_send_command1(PIONEER_POLLING, 0);    if (result >=0)      sonar_is_on=0;    return result;  }  else {    int result = pioneer_send_command1(PIONEER_SONAR, b);    if (result >=0) {      if (b == 0)	sonar_is_on=0;      else	sonar_is_on=1;    }    return result;  }}static int pioneer_sync0(void) {  unsigned char buf[256];  buf[0] = !PIONEER_SYNC0;  pioneer_send_command0(PIONEER_SYNC0);  if ((pioneer_read_string(buf, 0.2, NULL) == -1) && (pioneer_version == 1))     {      /* sometimes the Pioneer I's want two syncs */      pioneer_send_command0(PIONEER_SYNC0);      if (pioneer_read_string(buf, 0.2, NULL) == -1) 	{	  carmen_warn("Could not SYNC0\n");	  return -1;	}    }  if (buf[0] != PIONEER_SYNC0)     {      carmen_warn("Could not SYNC0\n");      return -1;    }    return 0;}static int pioneer_sync1(void) {  unsigned char buf[256];  pioneer_send_command0(PIONEER_SYNC1);  pioneer_read_string(buf, PIONEER_SERIAL_TIMEOUT, NULL);  if (buf[0] != PIONEER_SYNC1)     {      carmen_warn("Could not SYNC1\n");      return -1;    }  return 0;}static int pioneer_sync2(void) {  unsigned char buf[256];  pioneer_send_command0(PIONEER_SYNC2);  pioneer_read_string(buf, PIONEER_SERIAL_TIMEOUT, NULL);  return 0;}int abs_max(int a, int b) {  a = abs(a);  b = abs(b);  return (a>b)?a:b;}intpioneer_vel2(int vl, int vr) {  int arg;  int absmax;  absmax = abs_max(vl, vr);  if (absmax > 127) {    float scale_factor;    scale_factor = 127.0 / ((float)absmax);    vl = (int) ( scale_factor * ((float)vl));    vr = (int) ( scale_factor * ((float)vr));  }  if (1)    fprintf(stderr,"(%d %d)\n", vl, vr);  arg = 256*vl + vr;  return pioneer_send_command1(PIONEER_VEL2, arg);}intcarmen_base_direct_sonar_on(void){  int err = 0;  fprintf(stderr, "Switching sonars on\n");    //  if (!sonar_is_on){     err = pioneer_sonar(1);    //  }  if (err < 0)    return -1;  return PIONEER_MAX_NUM_SONARS;}intcarmen_base_direct_sonar_off(void){  fprintf(stderr, "Switching sonars off\n");  //  if (sonar_is_on)   return pioneer_sonar(0);  //  return 0;}int carmen_base_direct_reset(void){  return 0;}int carmen_base_direct_initialize_robot(char *model, char *dev)