camera.c

机器人仿真软件

		write_check(fd, "GS 1\r", 4);
	else		               // set position of servo 0
		write_check(fd, "GS 0\r", 4);

	for(i = 0; 1; i++)
	{
		read(fd, &c, 1);
		if(c == '\r')
			break;   
		number[i] = c;
	}
	read(fd, &c, 1);           // read the : at the end
	servo_position = atoi(number);
	return servo_position - ZERO_POSITION;
}

/**************************************************************************
		        *** TRACK BLOB  ***
**************************************************************************/
/* Description: This functions starts to Track a Color. It takes in the minimum 
                and maximum RGB values and outputs a type T packet. This packet 
                by default returns the middle mass x and y coordinates, the 
                bounding box, the number of pixles tracked, and a confidence 
                values.
   cc:          the min & max RGB values of the blob to be tracked.
*/
void track_blob( int fd, color_config cc )
{
	char cmd[28];
	int value[] = {cc.rmin, cc.rmax, cc.gmin, cc.gmax, cc.bmin, cc.bmax};
	range = cc;

	make_command("TC ", value, sizeof(value)/sizeof(int), cmd); 
	if(!write_check(fd, cmd, 4))
	{
		printf("ERROR; track color failed.\n");
		return;
	}
}

/**************************************************************************
		        *** GET SERVO POSITION  ***
**************************************************************************/
/* Description: The function enables/disables automatic servoing
   Parameters:  fd: serial port handler, on: determines whethere to enable(on=1)
                    or disable(on=0)
   Returns:    0: if the camera fails to write command  1: otherwise
*/
int auto_servoing(int fd, int on)
{
  // Enabling Auto Servoing Mode for both servos
	if(on)
	{
		if(!write_check(fd, "SM 15\r", 5))
		{
			printf("CMUCAM II ERROR: Enabling auto-servo failed.\n");
			return 0;
		}
		return 1;
	}
	else
	{
		if(!write_check(fd, "SM 0\r", 5))
		{
			printf("CMUCAM II ERROR: Disabling auto-servo faild.\n");
			return 0;
		}
		return 1;
	}
}

/**************************************************************************
		        *** STOP TRACKING  ***
**************************************************************************/
/* Description: The function stops the camera from tracking blobs and sending 
                data
   Parameters:  fd: serial port handler
   Returns:     none
*/
void stop_tracking(int fd)
{
	char c = 0;
	write(fd, "\r", 1);
	while(c != ':')
		read(fd, &c, 1);
}

/**************************************************************************
		        *** READ T PACKET  ***
**************************************************************************/
/* Description: The function reads a t-packet from camera, ex. when camera is 
                tracking
   Parameters:  fd: serial port handler, tpack_chars: the characters read
   Returns:     none
*/
void read_t_packet(int fd, char *tpack_chars)
{
	char c = 0;
	int k = 0;
	while(1)
	{
		read(fd, &c, 1);     
		tpack_chars[k++] = c;
		if(c == '\r')
			break;
	}
	if(tpack_chars[k-1] != '\r')
		printf("ERROR: reading T packet failed.\n");   
	tpack_chars[k] = '\0';
}

/**************************************************************************
		        *** READ T PACKET  ***
**************************************************************************/
/* Description: The function reads a t-packet from camera, ex. when camera is 
                tracking
   Parameters:  fd: serial port handler, output: 
   Returns:     none
*/
int set_t_packet( packet_t *tpacket, char tpack_chars[] )
{ 
	char packet_type;
	sscanf(tpack_chars, "%c %d %d %d %d %d %d %d %d", &packet_type, 
		   &tpacket->middle_x, &tpacket->middle_y,
		   &tpacket->left_x, &tpacket->left_y, &tpacket->right_x, 
		   &tpacket->right_y,
		   &tpacket->blob_area, &tpacket->confidence);
	if(packet_type != 'T')
	{
		printf("ERROR: cmucam2 failed to transmit t packet.\n");
		return 0;
	}
	return 1;
}

/**************************************************************************
		        *** READ F PACKET  ***
**************************************************************************/
/* Description: The function reads a f-packet from camera
   Parameters:  fd: serial port handler, fpack_chars: the characters read
   Returns:     none
*/
int read_f_packet (int fd, char *fpack_chars)
{
	char c = 0;
	int k = 0;
   
	while ((c != 0) || (c != 1))
	{
		read(fd, &c, 1);

		if (c == 0) {
			printf ("Cmucam2: frame grab failed.\n");
			return -1;
		}
    
		if (c == 1) {
			fpack_chars[k++] = c;
			read(fd, &c, 1);
			fpack_chars[k++] = c;
      //char xsize = c;
			read(fd, &c, 1);     
			fpack_chars[k++] = c;
      //char ysize = c;
      //printf ("Cmucam2: getting a frame of X=%d and Y=%d pixels.\n",
        //(uint8_t)xsize*2, 
        //(uint8_t)ysize);
			break;
		}
	}

	while(1)
	{
		read (fd, &c, 1);
		fpack_chars[k++] = c;
		if (c == 3) {
      //printf ("Cmucam2: got a frame of %d bytes.\n", k);
			break;
		}
	}
  
	if (fpack_chars[k-1] != 3) {
		printf ("ERROR: reading F packet failed.\n");   
		return -1;
	}
	fpack_chars[k] = '\0';
	return 0;
}

/**************************************************************************
		        *** READ IMAGE ***
**************************************************************************/
/* Description: This function gets an image from the camera using the 
                specified channel as a filter.
   Parameters:  fd: serial port handler, chan_num: the channel number
   Returns:     the image as an F packet
*/
int read_image (int fd, int chan_num, packet_f *fpacket)
{  
	char fpack_chars [F_PACKET_LENGTH];
	switch (chan_num)
	{
		case 0:
		{
			write_check (fd, "SF 0\r", 4);
			break;
		}
		case 1:
		{
			write_check (fd, "SF 1\r", 4);
			break;
		}
		case 2:
		{
			write_check (fd, "SF 2\r", 4);
			break;
		}
		case -1:
		{
			write_check (fd, "SF \r", 4);
			break;
		}
		default:
		{
			printf ("Cmucam2: invalid channel number!\n");
			break;
		}
	}
   
  
	if (read_f_packet (fd, fpack_chars) != 0)
		return -1;

	return set_f_packet (fpacket, fpack_chars, chan_num);
}

/**************************************************************************
		        *** SET F PACKET  ***
**************************************************************************/
int set_f_packet (packet_f *fpacket, char fpack_chars[], int chan_num)
{ 
	fpacket->first = (uint8_t)fpack_chars[0];
	fpacket->xsize = (uint8_t)fpack_chars[1];
	fpacket->ysize = (uint8_t)fpack_chars[2];
  
	switch (chan_num)
	{
		case -1:
		{
			int i = 0;
			int j = 0;
       
			for (i = 0; i < IMAGE_HEIGHT; i++)
			{
				fpacket->rows[i].rowbyte = 
						(uint8_t)fpack_chars[3 + (i*IMAGE_WIDTH/2)];
				for (j = 0; j < IMAGE_WIDTH/2; j++)
				{
					fpacket->rows[i].rgb[j].r = (uint8_t)fpack_chars[4 + i*j];
					fpacket->rows[i].rgb[j].g = (uint8_t)fpack_chars[5 + i*j];
					fpacket->rows[i].rgb[j].b = (uint8_t)fpack_chars[6 + i*j];
				}
			}
  
			fpacket->last = (uint8_t)
					fpack_chars[3 + IMAGE_HEIGHT*(IMAGE_WIDTH/2*3 + 1)];

			break;
		}
		default:
		{
			int i = 0;
			int j = 0;
      
			for (i = 0; i < IMAGE_HEIGHT; i++)
			{
				fpacket->rows[i].rowbyte = 
						(uint8_t)fpack_chars[3 + (i*IMAGE_WIDTH/2)];
				for (j = 0; j < IMAGE_WIDTH/2; j++)
				{
					fpacket->rows[i].rgb[j].r = (uint8_t)fpack_chars[4 + i*j];
					fpacket->rows[i].rgb[j].g = (uint8_t)fpack_chars[4 + i*j];
					fpacket->rows[i].rgb[j].b = (uint8_t)fpack_chars[4 + i*j];
				}
			}
  
			fpacket->last = (uint8_t)
					fpack_chars[3 + IMAGE_HEIGHT*(IMAGE_WIDTH/2 + 1)];
			break;
		}
	}
	return 0;
}