camera.cpp

人头跟踪算法

//#include <windows.h> // HCURSOR???
#include "assert.h"
#include "stdafx.h" // CDC

#include "camera.h"
#include "vcc1drv.h"
#include "base.h" // must come after 'vcc1drv.h', b/c of BOOL conflict

#define MAXPANSPEED	499
#define MAXTILTSPEED 499
#define MINPANSPEED	100
#define MINTILTSPEED 100

static HCURSOR  hWaitCursor = NULL; // for StartWait/EndWait
static int comport;
static char msg[80];
extern CDC *my_cdc;
extern BOOL zoom_control;

// comport should be 2


static void DisplayVCCError(int nErrorCode);

// Causes cursor to change to Wait Cursor, and saves old cursor.
static void StartWait(void)
{
    if (hWaitCursor == NULL) // Avoid losing old cursor.
        hWaitCursor = SetCursor(LoadCursor(NULL, IDC_WAIT));  // Only change if no old cursor
}

// Causes cursor to change back to old cursor from wait cursor..
static void EndWait(void)
{
    if (hWaitCursor) // If wait cursor exists.
    {
        SetCursor(hWaitCursor);	// Switch back
        hWaitCursor = NULL;			// Set old cursor to null
    }
}

// These two functions are called from within 'Vcc1drv.cpp', but I don't think they're
// very interesting for our purposes, so we will make them do nothing.
void Camera_SetInternalZoomPosition(int CamNum, DWORD value) {}
int Camera_SyncronizeInternalSetPTZStateAsync(int CamNum, DWORD CurrentCameraState, BOOL DisplayErrors) {return 0;}

void CloseCamera()
{
	VCC_Close(comport);
}


void SendCameraHome()
{
	int nErrorCode;

	if ((nErrorCode = VCC_Init(NULL, comport))==VCC_ERR_NONE) ;
}

BOOL OpenCamera(int comport_new) 
{
    int nErrorCode; // Used to record Error Code
	static char *comname[4]={"COM1","COM2","COM3","COM4"}; // Names For Comports, 

	// Bounds Check Comport
	comport = comport_new - 1;
	assert(comport >= 0 && comport <= 3);

	// Used for VCC1DRV.cpp.  
	bInitialized[comport] = FALSE;    

    StartWait();	// Changes cursor to hour glass.  

    // Initialize serial communications
    if ((nErrorCode = VCC_Open( comname[comport], comport )) != VCC_ERR_NONE )
	{
		// We Failed 
	    EndWait();	// Change Cursor Back
		// Display Error Message
		MessageBox(NULL,"The chosen COM port does not exist or is in use by another device.  Please select the correct COM port.",NULL,MB_ICONSTOP);
		// Clean Up
    	CloseCamera();
    	return FALSE;
	}
	
	// Fast check to see if unit is ready
	if ((nErrorCode = VCC_Ready(NULL, comport)) == VCC_ERR_NONE )
	{
		// Initialize pedestal after power has been recycled
#ifndef _DEBUG	// Don't do this in Debug mode.  Might want to keep camera where it is
//		SendCameraHome();
#endif
		bInitialized[comport] = TRUE;	// Mark Camera as initialized (VCC1DRV.CPP)
#ifndef _DEBUG	// Don't do this in Debug mode.  Might want to keep camera where it is
		// Set the Zoom Speed. Zoom All the Way Out
//		VCC_Command( VCC_SETHIGHSPEEDWIDEZOOM,  (WORD) 0, 0L, SYNCHRONOUS,Cameras[CamNum].comport-1 );
#endif
	}
	else 
	{
		// We failed
		// Clean up
    	CloseCamera();
		// Let user know
		MessageBox(NULL,"The VC-C1 is not detected.  Please check the power, serial cable, and the COM port setting.",NULL,MB_ICONSTOP);
		return FALSE;
	}
	// Wait until pedestal stops moving
	if ((nErrorCode = VCC_WaitIdle( VCC_STATUS_PANTILT_ACTIVE, comport ))== VCC_ERR_NONE )
		VCC_Command( VCC_HOME,  (WORD) 0, 0L, SYNCHRONOUS, comport ); // Goto Home Position

	// a kluge to fix a bug in the VCC: its internal status only shows the
	// zoom idle when a StopZoom has been issued...
	if ( (nErrorCode = VCC_Command( VCC_STOPZOOM, (WORD) 0, 0L, SYNCHRONOUS, comport )) !=  VCC_ERR_NONE) 
	   { DisplayVCCError( nErrorCode );	}

	nErrorCode = VCC_Command(VCC_SETBACKLIGHTCOMPOFF, (WORD) 0, 0L, SYNCHRONOUS, comport);

	EndWait();
    return TRUE;
}

void CameraLockWhiteBalance()
{
	int nErrorCode;

	nErrorCode = VCC_Command(VCC_SETLOCKWHITEBALANCE, (WORD) 0, 0L, SYNCHRONOUS, comport);
}


int sync = ASYNCHRONOUS;

enum {NOT_MOVING, MOVING_RIGHT, MOVING_LEFT, MOVING_UP, MOVING_DOWN};

void MoveCamera(EllipseState *state)
{
#define ZOOM_DESIRED 19
	int deadband_x = 10;
	int deadband_y = 8;
	int deadband_sz = 2;
	int diffx = state->x - ISIZEX/2;
	int diffy = state->y - ISIZEY/2;
	int diffsz = state->sz - ZOOM_DESIRED;
	int nErrorCode = VCC_ERR_NONE;
	float factpan = 0.4f;
	float facttilt = 0.4f;
	static int last_motion = NOT_MOVING;
	float speed;
	static int zoompos = ZOOM_DESIRED;
	static DWORD zoomposition=0;
	int zoomjump = 4;

//	sprintf(msg, "diffx %3d diffy %3d    ", diffx, diffy);
//	my_cdc->TextOut(5,220,msg);

//	nErrorCode = VCC_Command(VCC_GETZOOMPOSITION,  (WORD) ((LPINT) &zoomposition), 0L, SYNCHRONOUS, comport);
//	sprintf(msg, "zoompos %3d             ", zoomposition);
//	my_cdc->TextOut(200,220,msg);
	
	if (abs(diffx) > abs(diffy)) {
		if (diffx > deadband_x) {
			if (last_motion != MOVING_RIGHT)
				nErrorCode = VCC_Command(VCC_STOPPANTILT, (WORD) 0, 0L, sync, comport);
			speed = min(MAXPANSPEED, max(MINPANSPEED, factpan*(diffx*diffx)));
			nErrorCode = VCC_Command(VCC_SETPANMOTORSPEED,  (WORD) (speed), 0L, sync, comport);
			nErrorCode = VCC_Command(VCC_PANRIGHT, (WORD) 0, 0L, sync, comport);
			last_motion = MOVING_RIGHT;
		} else if (diffx < -deadband_x) {
			if (last_motion != MOVING_LEFT)
				nErrorCode = VCC_Command(VCC_STOPPANTILT, (WORD) 0, 0L, sync, comport);
			speed = min(MAXPANSPEED, max(MINPANSPEED, factpan*(diffx*diffx)));
			nErrorCode = VCC_Command(VCC_SETPANMOTORSPEED,  (WORD) (speed), 0L, sync, comport);
			nErrorCode = VCC_Command(VCC_PANLEFT, (WORD) 0, 0L, sync, comport);
			last_motion = MOVING_LEFT;
		} else {
			nErrorCode = VCC_Command(VCC_STOPPANTILT, (WORD) 0, 0L, sync, comport);
			last_motion = NOT_MOVING;
		}
	} else {
		if (diffy > deadband_y) {
			if (last_motion != MOVING_DOWN)
				nErrorCode = VCC_Command(VCC_STOPPANTILT, (WORD) 0, 0L, sync, comport);
			speed = min(MAXTILTSPEED, max(MINTILTSPEED, facttilt*(diffy*diffy)));
			nErrorCode = VCC_Command(VCC_SETTILTMOTORSPEED,  (WORD) (speed), 0L, sync, comport);
			nErrorCode = VCC_Command(VCC_TILTDOWN, (WORD) 0, 0L, sync, comport);
			last_motion = MOVING_DOWN;
		} else if (diffy < -deadband_y) {
			if (last_motion != MOVING_UP)
				nErrorCode = VCC_Command(VCC_STOPPANTILT, (WORD) 0, 0L, sync, comport);
			speed = min(MAXTILTSPEED, max(MINTILTSPEED, facttilt*(diffy*diffy)));
			nErrorCode = VCC_Command(VCC_SETTILTMOTORSPEED,  (WORD) (speed), 0L, sync, comport);
			nErrorCode = VCC_Command(VCC_TILTUP, (WORD) 0, 0L, sync, comport);
			last_motion = MOVING_UP;
		} else {
			nErrorCode = VCC_Command(VCC_STOPPANTILT, (WORD) 0, 0L, sync, comport);
			last_motion = NOT_MOVING;
		}
	}

//	sprintf(msg, "zoom_on %d  zoompos %d min %d max %d diffsz %d deadband %d",
//		zoom_control, zoompos, VCC1_MIN_ZOOM_POSITION, VCC1_MAX_ZOOM_POSITION,
//		diffsz, deadband_sz);
//	my_cdc->TextOut(5,220,msg);

//	my_cdc->TextOut(5,245,"not zooming               ");
	if (zoom_control) {
		if (diffsz > deadband_sz) {
			zoompos = max(zoompos-zoomjump, VCC1_MIN_ZOOM_POSITION);
//			nErrorCode = VCC_Command(VCC_SETZOOMPOSITION, (WORD) zoompos, 0L, sync, comport);
			nErrorCode = VCC_Command(VCC_SETLOWSPEEDWIDEZOOM, (WORD) zoompos, 0L, sync, comport);
//			Sleep(1);
//	my_cdc->TextOut(5,245,"ZOOMING OUT               ");
		} else if (diffsz < -deadband_sz) {
			zoompos = min(zoompos+zoomjump, VCC1_MAX_ZOOM_POSITION);
//			nErrorCode = VCC_Command(VCC_SETZOOMPOSITION, (WORD) zoompos, 0L, sync, comport);
			nErrorCode = VCC_Command(VCC_SETLOWSPEEDFARZOOM, (WORD) zoompos, 0L, sync, comport);
//			Sleep(1);
//	my_cdc->TextOut(5,245,"ZOOMING IN                ");
		} else {
			nErrorCode = VCC_Command(VCC_STOPZOOM, (WORD) 0, 0L, sync, comport);
		}
	}
#undef ZOOM_DESIRED
}

void StopCamera()
{
	int nErrorCode;

	nErrorCode = VCC_Command(VCC_STOPPANTILT, (WORD) 0, 0L, sync, comport);
}













// Brings up error Message, letting user know of difficulty with VCC-1	
static void ErrorMessage2(LPSTR errStr)
{
	MessageBox(NULL, errStr, "VCC Error Detected", MB_ICONSTOP);
}

// Displays Message Box using text associated with particular error code.
static void DisplayVCCError(int nErrorCode)
{
    switch(nErrorCode) {
    case    VCC_ERR_NONE:
        break;

    case    VCC_ERR_INIT_OPEN:
        ErrorMessage2( 
            (LPSTR)"Communications device is already in use" );
        break;

    case    VCC_ERR_INIT_HARDWARE:
        ErrorMessage2( 
            (LPSTR)"Communications device is not available" );
        break;

    case    VCC_ERR_INIT_SET:
    case    VCC_ERR_INIT_FORMAT:
        ErrorMessage2( 
            (LPSTR)"Error initializing driver parameters" );
        break;

    case    VCC_ERR_DRIVER_INIT:
        ErrorMessage2( 
            (LPSTR)"Communications has not been initialized" );
        break;

    case    VCC_ERR_SEND_TMO:
        ErrorMessage2( (LPSTR)"Send character timeout" );
        break;

/*    case    VCC_ERR_RECV_TMO:
        ErrorMessage2( (LPSTR)"Receive character timeout" );
        break;
*/
    case    VCC_ERR_RECV_IO:
        ErrorMessage2( (LPSTR)"Receive character I/O error" );
        break;

    case    VCC_ERR_SEND_IO:
        ErrorMessage2( (LPSTR)"Send character I/O error" );
        break;

    case    VCC_ERR_BUSY:
        ErrorMessage2( (LPSTR)"Busy timeout" );
        break;

    case    VCC_ERR_MODE:
        ErrorMessage2( (LPSTR)"Mode error" );
        break;

    case    VCC_ERR_CMD:
        ErrorMessage2( (LPSTR)"Command error" );
        break;

    case    VCC_ERR_PARAM:
        ErrorMessage2( (LPSTR)"Parameter error" );
        break;

    case    VCC_ERR_INVALID_CMD:
        ErrorMessage2( (LPSTR)"Invalid VC-C1 command" );
        break;

    case    VCC_ERR_MEMORY:
        ErrorMessage2( (LPSTR)"Out of memory" );
        break;

    case    VCC_ERR_IDLE_TIMEOUT:
        ErrorMessage2( (LPSTR)"Timeout waiting for idle state" );
        break;

    case    VCC_ERR_UNKNOWN:
        ErrorMessage2( (LPSTR)"Unknown error [%d]" /* , nErrorCode */ );
        break;
    }
}