Exemplo n.º 1
0
int ptLoop()
{
	int32_t panError, tiltError;
	uint16_t x, y;
	BlobA *blob, *blobs;
	BlobB *ccBlobs;
	uint32_t numBlobs, numCCBlobs;

	// create blobs
	g_blobs->blobify();

	blob = g_blobs->getMaxBlob();
	if (blob)
	{
		x = blob->m_left + (blob->m_right - blob->m_left)/2;
		y = blob->m_top + (blob->m_bottom - blob->m_top)/2;

		panError = X_CENTER-x;
		tiltError = y-Y_CENTER;

		g_panLoop.update(panError);
		g_tiltLoop.update(tiltError);
	}

	// send blobs
	g_blobs->getBlobs(&blobs, &numBlobs, &ccBlobs, &numCCBlobs);
	cc_sendBlobs(g_chirpUsb, blobs, numBlobs, ccBlobs, numCCBlobs);

	cc_setLED();
	
	return 0;
}
Exemplo n.º 2
0
int ptSetup()
{
	// setup camera mode
	cam_setMode(CAM_MODE1);

	// extend range of servos (handled in params)
	// rcs_setLimits(0, -200, 200);	(handled in rcservo params)
	// rcs_setLimits(1, -200, 200);	(handled in rcservo params)

	// Increasing the PWM frequency makes the servos zippier. 
	// Pixy updates at 50 Hz, so a default servo update freq of 50 Hz
	// adds significant latency to the control loop--- increasing to 100 Hz decreases this.
	// Increasing to more than 130 Hz or so creates buzzing, prob not good for the servo.
	// rcs_setFreq(100); (handled in rcservo params)
 
 	ptLoadParams();
	
	g_panLoop.reset();
	g_tiltLoop.reset();

	// load lut if we've grabbed any frames lately
	if (g_rawFrame.m_pixels)
		cc_loadLut();

	// setup qqueue and M0
	g_qqueue->flush();
	exec_runM0(0);

	return 0;
}
Exemplo n.º 3
0
void ptLoadParams()
{
	prm_add("Pan P gain", PRM_FLAG_SIGNED, 
		"@c Pan/tilt_Demo Pan axis proportional gain (default 350)", INT32(350), END);
	prm_add("Pan D gain", PRM_FLAG_SIGNED, 
		"@c Pan/tilt_Demo Pan axis derivative gain (default 600)", INT32(600), END);
	prm_add("Tilt P gain", PRM_FLAG_SIGNED, 
		"@c Pan/tilt_Demo Tilt axis proportional gain (default 500)", INT32(500), END);
	prm_add("Tilt D gain", PRM_FLAG_SIGNED, 
		"@c Pan/tilt_Demo Tilt axis derivative gain (default 700)", INT32(700), END);

	int32_t pgain, dgain; 

	prm_get("Pan P gain", &pgain, END);
	prm_get("Pan D gain", &dgain, END);
	g_panLoop.setGains(pgain, dgain);

	prm_get("Tilt P gain", &pgain, END);
	prm_get("Tilt D gain", &dgain, END);
	g_tiltLoop.setGains(pgain, dgain);
}
Exemplo n.º 4
0
void servo(uint32_t x, uint32_t y)
{
	static ServoLoop xloop(0, 400, 1000);
	static ServoLoop yloop(1, 400, 1000);
	static int lastLoop = 0;
	int32_t xerror, yerror;
#if 1
	if (lastLoop && !g_loop)
	{
		xloop.reset();
		yloop.reset();
		return;
	}
#endif
	xerror = x-XCENTER;
	yerror = -(y-YCENTER);
	xloop.update(xerror);
	yloop.update(yerror);

	lastLoop = g_loop;
}