ptu46.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
 *                      [e-mail]    [e-mail]
 *
 *  This program 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.
 *
 *  This program 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 this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/*
 * $Id: ptu46.cc,v 1.20.2.1 2006/09/22 23:58:35 gerkey Exp $
 *
 * methods for initializing, commanding, and getting data out of
 * the directed perceptions ptu-46 pan tilt unit camera
 * 
 * Author: Toby Collett (University of Auckland)
 * Date: 2003-02-10
 *
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_ptu46 ptu46
 * @brief Directed Perceptions PTU-46-17.5 pan-tilt unit

The ptu46 driver provides control of the PTU-46-17.5 pan-tilt unit from
directed perceptions through its text interface (This unit is standard on
the RWI b21r robot). This driver will probably work with other directed
perceptions pan tilt units, please let me know if you have tested it.

The ptu46 driver supports both position and velocity control, via the
PLAYER_PTZ_CONTROL_MODE_REQ request.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_ptz

@par Requires

- none

@par Configuration requests

- PLAYER_PTZ_CONTROL_MODE_REQ 

@par Configuration file options

- port (string)
  - Default: "/dev/ttyS0"
  - The serial port to which the unit is attached.

@author Toby Collett, Radu Bogdan Rusu

*/

/** @} */

/* This file is divided into two classes, the first simply deals with 
 * the control of the pan-tilt unit, providing simple interfaces such as
 * set pos and get pos.
 * The second class provides the player interface, hopefully this makes
 * the code easier to understand and a good base for a pretty much minimal
 * set up of a player driver
 */

// serial includes
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <strings.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <math.h>

//serial defines
#define PTU46_DEFAULT_BAUD B9600
#define PTU46_BUFFER_LEN 255

// command defines
#define PTU46_PAN 'p'
#define PTU46_TILT 't'
#define PTU46_MIN 'n'
#define PTU46_MAX 'x'
#define PTU46_MIN_SPEED 'l'
#define PTU46_MAX_SPEED 'u'
#define PTU46_VELOCITY 'v'
#define PTU46_POSITION 'i'

// Includes needed for player
#include <libplayercore/playercore.h>

#define DEFAULT_PTZ_PORT "/dev/ttyR1"
#define PTZ_SLEEP_TIME_USEC 100000

//
// Pan-Tilt Control Class
// 

class PTU46
{
    public:
	PTU46(char * port, int rate);
	~PTU46();

	// get count/degree resolution
	float GetRes (char type);
	// get position limit
	int GetLimit (char type, char LimType);

	// get/set position in degrees
	float GetPos (char type);
	bool SetPos  (char type, float pos, bool Block = false);

	// get/set speed in degrees/sec
	bool SetSpeed  (char type, float speed);
	float GetSpeed (char type);

	// get/set move mode
	bool SetMode (char type);
	char GetMode ();
	
	bool Open () {return fd >0;};

	// Position Limits
	int TMin, TMax, PMin, PMax;
	// Speed Limits
	int TSMin, TSMax, PSMin, PSMax;

    protected:
	// pan and tilt resolution
	float tr,pr;
	
	// serial port descriptor
	int fd;
	struct termios oldtio;
	
	// read buffer
	char buffer[PTU46_BUFFER_LEN+1];

	int Write(char * data, int length = 0);
};


// Constructor opens the serial port, and read the config info from it
PTU46::PTU46(char * port, int rate)
{
	tr = pr = 1;
	fd = -1;
		
	// open the serial port
	
	fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
	if ( fd<0 )
	{
		fprintf(stderr, "Could not open serial device %s\n",port);
		return;
	}
	fcntl(fd,F_SETFL, 0);
	
	// save the current io settings
	tcgetattr(fd, &oldtio);

	// rtv - CBAUD is pre-POSIX and doesn't exist on OS X
	// should replace this with ispeed and ospeed instead.
	
	// set up new settings
	struct termios newtio;
	memset(&newtio, 0,sizeof(newtio));
	newtio.c_cflag = /*(rate & CBAUD) |*/ CS8 | CLOCAL | CREAD;
	newtio.c_iflag = IGNPAR;
	newtio.c_oflag = 0;
	newtio.c_lflag = ICANON;
	if (cfsetispeed(&newtio, rate) < 0 || 	cfsetospeed(&newtio, rate) < 0)
	{
		fprintf(stderr,"Failed to set serial baud rate: %d\n", rate);
		tcsetattr(fd, TCSANOW, &oldtio);	
		close(fd);
		fd = -1;				
		return;
	}
	// activate new settings
	tcflush(fd, TCIFLUSH);
	tcsetattr(fd, TCSANOW, &newtio);

	// now set up the pan tilt camera
	Write(" "); // terse feedback
	usleep(100000);
	tcflush(fd, TCIFLUSH);

	Write("ft "); // terse feedback
	Write("ed "); // disable echo
	Write("ci "); // position mode

	// delay here so data has arrived at serial port so we can flush it
	usleep(200000);
	tcflush(fd, TCIFLUSH);

	// get pan tilt encoder res
	tr = GetRes(PTU46_TILT);
	pr = GetRes(PTU46_PAN);
	
	PMin = GetLimit(PTU46_PAN, PTU46_MIN);
	PMax = GetLimit(PTU46_PAN, PTU46_MAX);
	TMin = GetLimit(PTU46_TILT, PTU46_MIN);
	TMax = GetLimit(PTU46_TILT, PTU46_MAX);
	PSMin = GetLimit(PTU46_PAN, PTU46_MIN_SPEED);
	PSMax = GetLimit(PTU46_PAN, PTU46_MAX_SPEED);
	TSMin = GetLimit(PTU46_TILT, PTU46_MIN_SPEED);
	TSMax = GetLimit(PTU46_TILT, PTU46_MAX_SPEED);

	if (tr <= 0 || pr <= 0 || PMin == 0 || PMax == 0 || TMin == 0 || TMax == 0)
	{
		// if limit request failed try resetting the unit and then getting limits..
		Write(" r "); // reset pan-tilt unit (also clears any bad input on serial port)

		// wait for reset to complete
		int len = 0;
		char temp;
		char response[10] = "!T!T!P!P*";

		for (int i = 0; i < 9; ++i)
		{
			while((len = read(fd, &temp, 1 )) == 0);
			if ((len != 1) || (temp != response[i]))
			{
				fprintf(stderr,"Error Resetting Pan Tilt unit\n");
				fprintf(stderr,"Stopping access to pan-tilt unit\n");		
				tcsetattr(fd, TCSANOW, &oldtio);	
				fd = -1;
			}	
		}	

		// delay here so data has arrived at serial port so we can flush it
		usleep(100000);
		tcflush(fd, TCIFLUSH);


		// get pan tilt encoder res
		tr = GetRes(PTU46_TILT);
		pr = GetRes(PTU46_PAN);

		PMin = GetLimit(PTU46_PAN, PTU46_MIN);
		PMax = GetLimit(PTU46_PAN, PTU46_MAX);
		TMin = GetLimit(PTU46_TILT, PTU46_MIN);
		TMax = GetLimit(PTU46_TILT, PTU46_MAX);
		PSMin = GetLimit(PTU46_PAN, PTU46_MIN_SPEED);
		PSMax = GetLimit(PTU46_PAN, PTU46_MAX_SPEED);
		TSMin = GetLimit(PTU46_TILT, PTU46_MIN_SPEED);
		TSMax = GetLimit(PTU46_TILT, PTU46_MAX_SPEED);

		if (tr <= 0 || pr <= 0 || PMin == 0 || PMax == 0 || TMin == 0 || TMax == 0)
		{
			// if it really failed give up and disable the driver
			fprintf(stderr,"Error getting pan-tilt resolution...is the serial port correct?\n");
			fprintf(stderr,"Stopping access to pan-tilt unit\n");		
			tcsetattr(fd, TCSANOW, &oldtio);	
			fd = -1;
		}
	}
}


PTU46::~PTU46()
{
	// restore old port settings
	if (fd > 0)
		tcsetattr(fd, TCSANOW, &oldtio);	
}


int PTU46::Write(char * data, int length)
{

	if (fd < 0)
		return -1;
		
	// autocalculate if using short form
	if (length == 0)
		length = strlen(data);
		
	// ugly error handling, if write fails then shut down unit
	if(write(fd, data, length) < length)
	{
		fprintf(stderr,"Error writing to Pan Tilt Unit, disabling\n");
		tcsetattr(fd, TCSANOW, &oldtio);	
		fd = -1;
		return -1;
	}
	return 0;
}


// get count/degree resolution
float PTU46::GetRes(char type)
{
	if (fd < 0)
		return -1;
	char cmd[4] = " r ";
	cmd[0] = type;
	
	// get pan res
	int len = 0;
	Write(cmd);
	len = read(fd, buffer, PTU46_BUFFER_LEN );
	
	if (len < 3 || buffer[0] != '*')
	{
		fprintf(stderr,"Error getting pan-tilt res\n");
		return -1;
	}
		
	buffer[len] = '\0';
	return strtod(&buffer[2],NULL)/3600;	
}

// get position limit
int PTU46::GetLimit(char type, char LimType)
{
	if (fd < 0)
		return -1;
	char cmd[4] = "   ";
	cmd[0] = type;
	cmd[1] = LimType;
	
	// get limit
	int len = 0;
	Write(cmd);
	len = read(fd, buffer, PTU46_BUFFER_LEN );
	
	if (len < 3 || buffer[0] != '*')
	{
		fprintf(stderr,"Error getting pan-tilt limit\n");
		return -1;
	}
		
	buffer[len] = '\0';
	return strtol(&buffer[2],NULL,0);	
}


// get position in degrees
float PTU46::GetPos (char type)
{
    if (fd < 0)
	return -1;

    char cmd[4] = " p ";
    cmd[0] = type;
	
    // get pan pos
    int len = 0;
    Write (cmd);
    len = read (fd, buffer, PTU46_BUFFER_LEN );

    if (len < 3 || buffer[0] != '*')
    {
	fprintf(stderr,"Error getting pan-tilt pos\n");
	return -1;
    }
		
    buffer[len] = '\0';