camera.c

机器人仿真软件

#include <replace/replace.h>
#include "camera.h"

color_config range;

/************************************************************************
			    *** WRITE CHECK ***
**************************************************************************/
/* Description: This function writes a command to the camera and checks if the
                write was done successfully by checking camera's response.
   Parameters:  fd: serial port file descriptor
                msg: the command to be send to the camera
   Returns:     1: if the write was successful
                0: otherwise
*/
int write_check(int fd, char *msg, int respond_size)
{
	char respond[5];

	// write the command to the camera
	if( write(fd, msg, strlen(msg)) != (int)strlen(msg) )
	{
		printf( "Cmucam2: writing to serial device failed.\n" );
		return 0;
	}

	if( get_bytes(fd, respond, respond_size) < 1 )
	{
		printf( "Cmucam2: get bytes failed\n" );
		return 0;
	}

	// If NCK is returned, there was an error in writing
	if(respond[0] == 'N')
	{
		printf("Cmucam2: received NCK!\n");
		return 0;
	}
	return 1;
} 

int power(int fd, int on)
{
	if(on)
		return write_check(fd, "CP 1\r", 5);
	return write_check(fd, "CP 0\r", 5);
}

/**************************************************************************
			                      *** SET IMAGER CONFIG ***
**************************************************************************/
/* Description: This function sets the camera's internal resiger values
                for controlling image qualities.
   Parameters:  fd: serial port file descriptor
                player_blobfinder_imager_config: a Player packet containing
                information for camera's internal register:
                contrast, brightness, color mode, Exposure
   Returns:     1: If the command was successfully sent to the camera
                0: Otherwise
*/

int set_imager_config(int fd, imager_config ic)
{
   int value[8], size = 0;                 // The numbers used in the command:
   char command[26];                       // ex. CR 5 255 19 33
   if(ic.contrast != -1)                   // If ther is a change set the values
   {
	   value[size++] = CONTRAST;
	   value[size++] = ic.contrast;
   }
   if(ic.brightness != -1)
   {
	   value[size++] = BRIGHTNESS;
	   value[size++] = ic.brightness;
   }
   if(ic.colormode != -1)
   {
	   value[size++] = COLORMODE;
	   if(ic.colormode == 0)
		   value[size++] = RGB_AWT_OFF;
	   if(ic.colormode == 1)
		   value[size++] = RGB_AWT_ON;
	   if(ic.colormode == 2)
		   value[size++] = YCRCB_AWT_OFF;
	   if(ic.colormode == 3)
		   value[size++] = YCRCB_AWT_ON;
   }
   if(ic.autogain != -1)
   {
	   value[size++] = AUTOGAIN;
	   if(ic.autogain == 0)
		   value[size++] = AUTOGAIN_OFF;
	   if(ic.autogain == 1)
		   value[size++] = AUTOGAIN_ON;
   }
   // Put the values into camera's command format:
   // ex. CR 6 105 18 44
   make_command("CR ", value, size, command);
   return write_check(fd, command, 5);         // send the command to the camera
}

/**************************************************************************
		number[i] = c;	                      *** GET T PACKET ***
**************************************************************************/
/* Description: This function puts the camera's output during tracking into
                a T packet, which contrains information about the blob.
   Parameters:  fd: serial port file descriptor
                tpacket: the packet that will contain the blob info
   Returns:     void
*/
int get_t_packet(int fd, packet_t *tpacket)
{
	char tpack_chars[T_PACKET_LENGTH];
	read_t_packet(fd, tpack_chars);             // read the output of the camera
	return set_t_packet(tpacket, tpack_chars);  // convert it into T packet
}

/**************************************************************************
			                      *** POLL MODE ***
**************************************************************************/
/* Description: This functions determines whether the camera should send a
                continuous stream of packets or just one packet.
   Parameters:  fd: serial port file descriptor
                on: if on == 1, only one packet is send
                    if on == 0, a continuous stream of packets is send
   Returns:     1: If the command was successfully sent to the camera
                0: Otherwise
*/
int poll_mode(int fd, int on)
{
	if(on)
		return write_check(fd, "PM 1\r", 5);
	else
		return write_check(fd, "PM 0\r", 5);
}
   
/**************************************************************************
			                      *** SET SERVO POSITION ***
**************************************************************************/
/* Description: This functions sets the servo position given the servo number
                and the angle (note: angle = 0 denotes servo position = 128
                in terms of camera's values)
   Parameters:  number[i] = c;fd: serial port file descriptor
                servo_num: the servo which we are setting the position
                I am using 0:pan  1:tilt
   Returns:     1: If the command was successfully sent to the camera
                0: Otherwise
*/
int set_servo_position(int fd, int servo_num, int angle)
{
	// change the angle into camera's format
	int position = ZERO_POSITION + angle;
	// for servo position. I am using angle 0
	char comm[10];
	// corresponding to the default servo pos. 128
	int value[] = {servo_num, position};
	// generate the command using the values
	make_command("SV ", value, sizeof(value)/sizeof(int), comm);
	printf("servo %d new position: %d\n", servo_num, angle);
	return write_check(fd, comm, 5);      // write the command to the camera
}

/**************************************************************************
			                      *** MAKE COMMAND ***
**************************************************************************/
/* Description: This function gets a sets of values and a camera command header
                number[i] = c;to generate the command for the camera.
   Parameters:  cmd: the command header, for example SF or CR (see CMUcam 2 user
                     guide)
                n: the set of values to be used in the command
                size: the number of values used
                full_command: the final command in characters to be send to the 
                              camera
   Returns:     void
*/
void make_command(char *cmd, int *n, size_t size, char *full_command)
{
	char value[3];                 // the values are all withing 3 digits
	int length, i;
	for(i = 0; i < 10; i++)        // set all to null so that if there are
		full_command[i] = '\0';    // unsed characters at the end, the camera
                                   // does not complain about the command.
                                   // there is probably a better way to do this!
	strcat(full_command, cmd);     // attach the command header, ex. SF
	for(i = 0; i < (int)size; i++) // for all the values, convert them into char
	{                              // and attach them to the end of the command
		length = sprintf(value, "%d", n[i]);    // plus a space
		strcat(full_command, value);
		strcat(full_command, " ");
	}
	strcat(full_command, "\r");    // attach the return character to the end
}

/**************************************************************************
			                      *** OPEN PORT ***
**************************************************************************/
/* Description: This function opens the serial port for communication with the 
                camera.
   Parameters:  NONE
   Returns:     the file descriptor
*/
int open_port(char *devicepath)
{
	int fd = open( devicepath, O_RDWR );             // open the serial port
	struct termios term;
	struct pollfd fds[1];
	char cam_response[5];
 
	if( tcgetattr( fd, &term ) < 0 )                 // get device attributes
	{
		puts( "Cmucam2: unable to get device attributes.");
		return -1;
	}
  
	cfmakeraw( &term );
	cfsetispeed( &term, B115200 );                   // set baudrate to 115200
	cfsetospeed( &term, B115200 );
  
	if( tcsetattr( fd, TCSAFLUSH, &term ) < 0 )
	{
		puts( "Cmucam2: unable to set device attributes");
		return -1;
	}

  // Make sure queue is empty
	tcflush(fd, TCIOFLUSH);
  
	write(fd, "\r", 1);
	fds[0].fd = fd;
	fds[0].events = 1;
	poll(fds, 1, 100);
	if(!fds[0].revents)
	{
		printf("ERROR: CMUCAM2 IS OFF!\n");
		return -1;
	}
	get_bytes(fd, cam_response, 5);

	return fd;
}

/**************************************************************************
			        *** CLOSE PORT ***
**************************************************************************/
/* Description: This function closes the serial port.
   Parameters:  fd: serial port handler
   Returns:     void
*/
void close_port(int fd)
{
	close(fd);
}

/**************************************************************************
			        *** GET BYTES  ***
**************************************************************************/
/* Description: This function reads a specified number of bytes from the serial 
                port
   Parameters:  fd: serial port handler, buf: bytes read, len: bytes to read
   Returns:     0: if could not read the number of bytes specified  1: otherwise
*/
int get_bytes(int fd, char *buf, size_t len)
{
	int bytes_read = 0, ret_val;
	while(bytes_read < (int)len)
	{
		ret_val = read(fd, buf+bytes_read, len-bytes_read);
		if(ret_val < 0)
		{
			perror("Cmucam2: getting bytes failed.\n");
			return 0;
		}
		else if(ret_val > 0)
			bytes_read += ret_val;
	}
	return bytes_read;
}
    
/**************************************************************************
		        *** GET SERVO POSITION  ***
**************************************************************************/
/* Description: This function gets the position of the specified servo.
   Parameters:  fd: serial port handler, servo_num: the servo number whose size 
                    is required
   Returns:     the position of the servo
*/
int get_servo_position(int fd, int servo_num)
{  
	int servo_position;
	int i;
	char number[3];
	char c = 0;

	if(servo_num)               // set position of servo 1