rflex_lib.c

卡内基梅隆大学（CMU）开发的移动机器人控制开发软件包。可对多种机器人进行控制

 /*********************************************************
 *
 * 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 <netinet/in.h>
#include <carmen/drive_low_level.h>
#include "rflex.h"
#include "rflex-io.h"
#include "rflex_params.h"

typedef struct {
  int current_displacement_odometry;
  int current_bearing_odometry;

  int last_displacement_odometry;
  int last_bearing_odometry;

  int t_vel;
  int r_vel;
  int num_sonars;
  int ranges[MAX_NUM_SONARS];
  int num_bumpers;
  char *bumpers;
} rflex_status_t;

static rflex_status_t status;
static int dev_fd;
static int sonar_is_on = 0;

static double acceleration_state = 0;
static double deceleration_state = 0;

static int rflex_model = 0;

static double angle_conversion;
static double distance_conversion;

static inline int
sgn(long val)
{
  if (val < 0) 
    return(0);
  else 
    return(1);
}

/* COMPUTE CRC CODE */

static int
computeCRC( unsigned char *buf, int nChars )
{ 
  int i, crc;
  if (nChars==0) {
    crc = 0;
  } else {
    crc = buf[0];
    for (i=1; i<nChars; i++) {
      crc ^= buf[i];
    }
  }
  return(crc);
}

/* CONVERSION BYTES -> NUM */

static unsigned int
convertBytes2UInt16( unsigned char *bytes )
{
  unsigned int i;
  memcpy( &i, bytes, 2 );
  return(htons(i));
}


static unsigned long
convertBytes2UInt32( unsigned char *bytes )
{
  unsigned long i;
  memcpy( &i, bytes, 4 );
  return(htonl(i));
}

/* CONVERSION NUM -> BYTES */

static void
convertUInt8( unsigned int i, unsigned char *bytes )
{
  memcpy( bytes, &i, 1 );
}

static void
convertUInt32( unsigned long l, unsigned char *bytes )
{
  uint32_t conv;
  conv = htonl( l );
  memcpy( bytes, &conv, 4 );
}

static void
cmdSend( int port, int id, int opcode, int len, unsigned char *data )
{
  int i;
  static unsigned char cmd[MAX_COMMAND_LENGTH];
  /* START CODE */
  cmd[0] = 0x1b;
  cmd[1] = 0x02;
  /* PORT */
  cmd[2] = (unsigned char) port;
  /* ID */
  cmd[3] = (unsigned char) id;
  /* OPCODE */
  cmd[4] = (unsigned char) opcode;
  /* LENGTH */
  cmd[5] = (unsigned char) len;
  /* DATA */
  for (i=0; i<len; i++) {
    cmd[6+i] = data[i];
  }
  /* CRC */
  cmd[6+len] = computeCRC( &(cmd[2]), len+4 );    /* END CODE */
  cmd[6+len+1] = 0x1b;
  cmd[6+len+2] = 0x03;

  writeData( dev_fd, cmd, 9+len );
}

static void
parseMotReport( unsigned char *buffer )
{
  int rv, timeStamp, acc, trq;
  unsigned char axis, opcode;
    
  opcode = buffer[4];
  switch(opcode) {
  case MOT_SYSTEM_REPORT:
    rv        = convertBytes2UInt32(&(buffer[6]));
    timeStamp = convertBytes2UInt32(&(buffer[10]));
    axis      = buffer[14];
    if (axis == 0) {
      status.current_displacement_odometry=convertBytes2UInt32(&(buffer[15]));
      status.t_vel = convertBytes2UInt32(&(buffer[19]));
    } else if (axis == 1) {
      status.current_bearing_odometry = convertBytes2UInt32(&(buffer[15]));
      status.r_vel = convertBytes2UInt32(&(buffer[19]));
    }
    acc       = convertBytes2UInt32(&(buffer[23]));
    trq       = convertBytes2UInt32(&(buffer[27]));
    break;
  default:
    break;
  }
}

static void
parseSonarReport( unsigned char *buffer )
{
  unsigned int sid;
 
  int count, retval, timeStamp;
  unsigned char opcode, dlen;
    
  opcode = buffer[4];
  dlen   = buffer[5];

  status.num_sonars=MAX_NUM_SONARS;
  switch(opcode) {
  case SONAR_REPORT:
    retval    = convertBytes2UInt32(&(buffer[6]));
    timeStamp = convertBytes2UInt32(&(buffer[10]));
    count = 0;
    while ((8+count*3<dlen) && (count<256)) {
      sid   = buffer[14+count*3];
      status.ranges[sid] = convertBytes2UInt16( &(buffer[14+count*3]) );
      count++;
    }
    //	    fprintf( stderr, "(s:%d)", count );
    break;
  default:
    break;
  }
}

static int
parseBuffer( unsigned char *buffer, unsigned int len )
{
  unsigned int port, dlen, crc;

  port   = buffer[2];
  dlen   = buffer[5];

  if (dlen+8>len) {
    return(0);
  } else {
    crc    = computeCRC( &(buffer[2]), dlen+4 );
    if (crc != buffer[len-3])
      return(0);
    switch(port) {
    case SYS_PORT:
      fprintf( stderr, "(sys)" );
      break;
    case MOT_PORT:
      parseMotReport( buffer );
      break;
    case JSTK_PORT:
      break;
    case SONAR_PORT:
      parseSonarReport( buffer );
      break;
    case DIO_PORT:
      fprintf( stderr, "(dio)" );
      break;
    case IR_PORT:
      fprintf( stderr, "(ir)" );
      break;
    default:
      break;
    }
  }
  return(1);
}

static void
clear_incoming_data(void)
{
  unsigned char buffer[4096];
  int len;
  int bytes;

  // 32 bytes here because the motion packet is 34. No sense in waiting for a
  // complete packet -- we can starve because the last 2 bytes might always
  // arrive with the next packet also, and we never leave the loop. 
  
  bytes = bytesWaiting(dev_fd);
  while (bytes > 32) {
    waitForAnswer(dev_fd, buffer, &len);
    parseBuffer(buffer, len);
    bytes = bytesWaiting(dev_fd); 
  }
}

void
carmen_rflex_digital_io_on(int period)
{
  unsigned char data[MAX_COMMAND_LENGTH];
  convertUInt32( (long) period, &(data[0]) );
  cmdSend(DIO_PORT, 0, DIO_REPORTS_REQ, 4, data);
}

void
carmen_rflex_digital_io_off(void)
{
  unsigned char data[MAX_COMMAND_LENGTH];
  convertUInt32( (long) 0, &(data[0]) );
  cmdSend(SONAR_PORT, 4, DIO_REPORTS_REQ, 4, data );
}

void
carmen_rflex_brake_on(void)
{
  cmdSend(MOT_PORT, 0, MOT_BRAKE_SET, 0, NULL);
}

void
carmen_rflex_brake_off(void)
{
  cmdSend(MOT_PORT, 0, MOT_BRAKE_RELEASE, 0, NULL);
}

void
carmen_rflex_motion_set_defaults(void)
{
  cmdSend(MOT_PORT, 0, MOT_SET_DEFAULTS, 0, NULL);
}

void
carmen_rflex_odometry_on(long period)
{ 
  unsigned char data[MAX_COMMAND_LENGTH];
  int num_read;
  
  convertUInt32( period, &(data[0]) );         /* period in ms */
  convertUInt32( (long) 3, &(data[4]) );       /* mask */
  cmdSend(MOT_PORT, 0, MOT_SYSTEM_REPORT_REQ, 8, data);

  num_read = read(dev_fd, data, MAX_COMMAND_LENGTH);   
}

void
carmen_rflex_odometry_off(void)
{ 
  unsigned char data[MAX_COMMAND_LENGTH];
  convertUInt32( (long) 0, &(data[0]) );       /* period in ms */
  convertUInt32( (long) 0, &(data[4]) );       /* mask */
  cmdSend(MOT_PORT, 0, MOT_SYSTEM_REPORT_REQ, 8, data);
}

void
carmen_rflex_stop_robot(void)
{
  carmen_base_direct_set_velocity(0, 0);
}

void