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