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