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