rflex_commands.cc

机器人仿真软件

#include <sys/types.h>
#include <sys/time.h>
#include <netinet/in.h>

#include "rflex-info.h"
#include "rflex_commands.h"
#include "rflex-io.h"
#include "rflex_configs.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
//#include <pthread.h>
#include <unistd.h>
#include <rflex.h>


//holds data until someone wants to read it
typedef struct {
  int distance;
  int bearing;
  int t_vel;
  int r_vel;
  int num_sonars;
  int *ranges;
  int *oldranges;
  int num_bumpers;
  char *bumpers;
  int voltage;
  bool brake;
  int lcd_x;
  int lcd_y;
  unsigned char * lcd_data;
  int num_ir;
  unsigned char * ir_ranges;
  int home_bearing;
  int home_bearing_found;
  
} rflex_status_t;


static int clear_incoming_data(int fd);


static rflex_status_t status;

//finds the sign of a value
static 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 */

#if 0 

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

#endif

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 );
}

#if 0 

static void convertUInt16( unsigned int i, unsigned char *bytes )
{
  uint16_t conv;
  conv = htonl( i );
  memcpy( bytes, &conv, 2 );
}

#endif

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

//sends a command to the rflex
static void cmdSend( int fd, 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;

  //pthread_testcancel();
  writeData( fd, cmd, 9+len );

	// Some issues with commands not being recognised if sent too rapidly
	// (too small a buffer on recieving end?
	// So we delay for a bit, specifically we wait until specified amount of
	// time has passed without recieving a packet. This roughtly approximates
	// to waiting till the command has finshed being executed on the robot
	int count;
	timeval now;
	timeval start = {0,0};
	do  
	{
		count = clear_incoming_data(fd);
  		gettimeofday(&now,NULL);
		if (count > 0 ) 
			start = now;
		count = (now.tv_sec - start.tv_sec) * 1000000 + (now.tv_usec - start.tv_usec);

               // release somewhat so other threads can run.
               usleep(500);
	} while (count < 10000);

  
  //pthread_testcancel();
}

void rflex_sonars_on( int fd )
{
  unsigned char data[MAX_COMMAND_LENGTH];
  convertUInt32( (unsigned long) rflex_configs.sonar_echo_delay/*30000*/, &(data[0]) );
  convertUInt32( (unsigned long) rflex_configs.sonar_ping_delay/*0*/, &(data[4]) );
  convertUInt32( (unsigned long) rflex_configs.sonar_set_delay/*0*/, &(data[8]) );
  convertUInt8(  (unsigned int) 2, &(data[12]) );
  cmdSend( fd, SONAR_PORT, 4, SONAR_RUN, 13, data );
}

void rflex_sonars_off( int fd )
{
  unsigned char data[MAX_COMMAND_LENGTH];
  convertUInt32( (long) 0, &(data[0]) );
  convertUInt32( (long) 0, &(data[4]) );
  convertUInt32( (long) 0, &(data[8]) );
  convertUInt8(  (int) 0, &(data[12]) );
  cmdSend( fd, SONAR_PORT, 4, SONAR_RUN, 13, data );
}

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

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

void rflex_ir_on( int fd )
{
  unsigned char data[MAX_COMMAND_LENGTH];
  convertUInt32( (long) 0, &(data[0]) );
  convertUInt32( (long) 70, &(data[4]) );
  convertUInt32( (long) 10, &(data[8]) );
  convertUInt32( (long) 20, &(data[12]) );
  convertUInt32( (long) 150, &(data[16]) );
  convertUInt8( (unsigned char) 2, &(data[20]) );
  cmdSend( fd, IR_PORT, 0, IR_RUN, 21, data );
}

void rflex_ir_off( int fd )
{
  unsigned char data[MAX_COMMAND_LENGTH];
  convertUInt32( (long) 0, &(data[0]) );
  convertUInt32( (long) 0, &(data[4]) );
  convertUInt32( (long) 0, &(data[8]) );
  convertUInt32( (long) 0, &(data[12]) );
  convertUInt32( (long) 0, &(data[16]) );
  convertUInt8( (unsigned char) 0, &(data[20]) );
  cmdSend( fd, IR_PORT, 0, IR_RUN, 21, data );
}
void rflex_brake_on( int fd )
{
  cmdSend( fd, MOT_PORT, 0, MOT_BRAKE_SET, 0, NULL );
}

void rflex_brake_off( int fd )
{
  cmdSend( fd, MOT_PORT, 0, MOT_BRAKE_RELEASE, 0, NULL );
}

void rflex_motion_set_defaults( int fd )
{
  cmdSend( fd, MOT_PORT, 0, MOT_SET_DEFAULTS, 0, NULL );
}

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

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


void rflex_set_velocity( int fd, long tvel, long rvel,
			   long acceleration )
{
  unsigned char data[MAX_COMMAND_LENGTH];

  long utvel;long urvel;

  utvel=labs(tvel);
  urvel=labs(rvel);

  // ** workaround for stupid hardware bug, cause unknown, but this works
  // ** with minimal detriment to control
  // ** avoids all values with 1b in highest or 3'rd highest order byte
  
  // 0x1b is part of the packet terminating string
  // which is most likely what causes the bug

  // ** if 2'nd order byte is 1b, round to nearest 1c, or 1a
  if((urvel&0xff00)==0x1b00){
    // ** if lowest order byte is>127 round up, otherwise round down 
    urvel=urvel&0xffff0000|(urvel&0xff>127?0x1c00:0x1aff);
  }

  // ** if highest order byte is 1b, round to 1c, otherwise round to 1a
  if((urvel&0xff000000)==0x1b000000){
    // ** if 3'rd order byte is>127 round to 1c, otherwise round to 1a
    urvel=urvel&0x00ffffff|(urvel&0xff0000>127?0x1c000000:0x1aff0000);
  }

  convertUInt8( (long) 0,                 &(data[0]) );       /* forward motion */
  convertUInt32( (long) labs(utvel),        &(data[1]) );       /* abs trans velocity*/
  convertUInt32( (long) acceleration,    &(data[5]) );       /* trans acc */
  convertUInt32( (long) STD_TRANS_TORQUE, &(data[9]) );       /* trans torque */
  convertUInt8( (long) sgn(tvel),         &(data[13]) );      /* trans direction */

  cmdSend( fd, MOT_PORT, 0, MOT_AXIS_SET_DIR, 14, data );

  convertUInt8( (long) 1,                 &(data[0]) );       /* rotational motion */
  convertUInt32( (long) labs(urvel),        &(data[1]) );       /* abs rot velocity  */
  /* 0.275 rad/sec * 10000 */ 
  convertUInt32( (long) STD_ROT_ACC,      &(data[5]) );       /* rot acc */
  convertUInt32( (long) STD_ROT_TORQUE,   &(data[9]) );       /* rot torque */
  convertUInt8( (long) sgn(rvel),         &(data[13]) );      /* rot direction */

  cmdSend( fd, MOT_PORT, 0, MOT_AXIS_SET_DIR, 14, data );
}

void rflex_stop_robot( int fd, int deceleration)
{
  rflex_set_velocity( fd, 0, 0, deceleration);
}

int rflex_open_connection(char *device_name, int *fd)
{
  RFLEX_Device   rdev;

  strncpy( rdev.ttyport, device_name, MAX_NAME_LENGTH);
  rdev.baud           = 115200;
  rdev.databits       = 8;
  rdev.parity         = N;
  rdev.stopbits       = 1;
  rdev.hwf            = 0;
  rdev.swf            = 0;
  
  printf("trying port %s\n",rdev.ttyport); 
  if (DEVICE_connect_port( &rdev )<0) {
    fprintf(stderr,"Can't open device %s\n",rdev.ttyport);
    return -1;
  } 

  *fd = rdev.fd;
  rflex_odometry_on(*fd, 100000);
  rflex_digital_io_on(*fd, 100000);
  rflex_motion_set_defaults(*fd);
  return 0;
}

int rflex_close_connection(int *fd)
{
  assert(fd);
  if (*fd < 0)
  	return -1;
  rflex_motion_set_defaults(*fd);
  rflex_odometry_off(*fd);
  rflex_digital_io_off(*fd);
  rflex_sonars_off(*fd);
  rflex_ir_off(*fd);
  
  printf("Closing rflex serial port\n");
  close(*fd);
  *fd=-1;

  return 0;
}


//processes a motor packet from the rflex - and saves the data in the
//struct for later use
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.distance = convertBytes2UInt32(&(buffer[15]));
      status.t_vel = convertBytes2UInt32(&(buffer[19]));
    } else if (axis == 1) {
      status.bearing = convertBytes2UInt32(&(buffer[15]));
      status.r_vel = convertBytes2UInt32(&(buffer[19]));
    }
    acc       = convertBytes2UInt32(&(buffer[23]));
    trq       = convertBytes2UInt32(&(buffer[27]));
    break;
  default:
    break;
  }
}

//processes a dio packet from the rflex - and saves the data in the
//struct for later use, dio report includes bump sensors...
static void parseDioReport( unsigned char *buffer )
{
	unsigned long timeStamp;
	unsigned char opcode, length, address;
	unsigned short data;

	opcode = buffer[4];
	length = buffer[5];


	switch(opcode) 
	{
		case DIO_REPORT:
			if (length < 6)
   			{
       			fprintf(stderr, "DIO Data Packet too small\n");
       			break;
   			}
   			timeStamp = convertBytes2UInt32(&(buffer[6]));
   			address = buffer[10];
   			data = convertBytes2UInt16(&(buffer[11]));

			// Check for the heading home event;
			if(rflex_configs.heading_home_address == address)
			{
				if(status.home_bearing_found)
					break;
				static bool found_first = false;
				static int first_home_bearing = 0;
				if (found_first)
				{
					if ((first_home_bearing - status.bearing) > 0.785* rflex_configs.odo_angle_conversion)
					{
						first_home_bearing=static_cast<int> (first_home_bearing-rflex_configs.odo_angle_conversion*2*M_PI);
					}
					else if ((first_home_bearing - status.bearing) < 0.785* rflex_configs.odo_angle_conversion)
					{
						first_home_bearing=static_cast<int> (first_home_bearing+rflex_configs.odo_angle_conversion*2*M_PI);
					}
					if (abs(first_home_bearing - status.bearing) > 0.01 * rflex_configs.odo_angle_conversion)
					{
						rflex_configs.home_on_start = false;
						status.home_bearing=status.bearing > first_home_bearing? status.bearing:first_home_bearing;
						status.home_bearing_found = true;
						printf("Home bearing found %d\n",status.home_bearing);
					}
				}
				else
				{
						first_home_bearing=status.bearing;
						found_first = true;		
				}
				break;
			}


			if(BUMPER_ADDR == rflex_configs.bumper_style)
			{
			   // on the b21r the bump packets are address 0x40 -> 0x4D, there are some other dio packets
			   // but dont know what they do so we throw them away
			   
			   // check if the dio packet came from a bumper packet
			   if ((address < rflex_configs.bumper_address) || (address >= (rflex_configs.bumper_address+status.num_bumpers)))
			   {
			      // not bumper
			      fprintf(stderr,"(dio) address = 0x%02x ",address);
			      break;
			   }
			   else
			   {
			      // is bumper
			      //fprintf(stderr,"(bump) address = 0x%02x ",address);
			      // assign low data byte to the bumpers (16 bit DIO data, low 4 bits give which corners or the panel are 'bumped')
			      status.bumpers[address - rflex_configs.bumper_address] = data & 0x0F;
			   }
			}
			else
			{
			   // on the magellan pro the bump packets are address 0x40 and 0x41. Each bits of these address
			   // match one bumper
			   
			   // Check if the dio paquet came from a bumper packet
			   if ((address < rflex_configs.bumper_address) || (address >= (rflex_configs.bumper_address+(status.num_bumpers/8))))
			   {
			      // not bumper
			      fprintf(stderr,"(dio) address = 0x%02x ",address);
			      break;
			   }
			   else
			   {
			      // is bumper
			      fprintf(stderr,"(bump) address = 0x%02x ",address);

			      // Loop for each bit
			      for (int i=0; i<8; i++)
			      {
				 // assign each bit of the data to a bumper. 
				 status.bumpers[((address - rflex_configs.bumper_address) * 8 )+ i] = data & (0x01 << i);
			      }
			   }
			}
    		break;
   		default:
     		break;
   	}
}

// Processes the IR sensor report