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;
}
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;
}
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);
}
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;
}
