// Main function of the task int CameraServo::Main(int a2, int a3, int a4, int a5, int a6, int a7, int a8, int a9, int a10) { Proxy *proxy; // Handle to proxy Robot *lHandle; // Local handle CameraServoLog sl; // log #if 0 unsigned timer = 0; // Timer to only do certain things so often int largestParticleIndex = 0; #endif // Let the world know we're in DPRINTF(LOG_DEBUG,"In the 166 Template task\n"); // Wait for Robot go-ahead (e.g. entering Autonomous or Tele-operated mode) WaitForGoAhead(); // Register our logger lHandle = Robot::getInstance(); lHandle->RegisterLogger(&sl); // Register the proxy proxy = Proxy::getInstance(); float CamJoystickX = 0.0; float CamJoystickY = 0.0; float CamX = 0.5; float CamY = 0.5; #define DEADBAND (0.2) #define CAMMOVE (.5 / (1000/CAMERA_SERVO_CYCLE_TIME)) // General main loop (while in Autonomous or Tele mode) while (true) { if(!proxy->get("Joy3BT")) { CamJoystickX = proxy->get("Joy3X"); CamJoystickY = proxy->get("Joy3Y"); if(CamJoystickX >= DEADBAND) { CamX += CAMMOVE; } else if(CamJoystickX <= -DEADBAND) { CamX -= CAMMOVE; } if(CamJoystickY >= DEADBAND) { CamY -= CAMMOVE; } else if(CamJoystickY <= -DEADBAND) { CamY += CAMMOVE; } if(CamX > 1.0) { CamX = 1.0; } else if(CamX < .0) { CamX = .0; } if(CamY > 1.0) { CamY = 1.0; } else if(CamY < .0) { CamY = .0; } } else { CamX = CamY = 0.5; } #if 0 if ((++timer) % (500/CAMERA_SERVO_CYCLE_TIME) == 0 && camera.IsFreshImage()) { timer = 0; camera.GetImage(srcimage); frcColorThreshold(destimage, srcimage, IMAQ_HSL, &Hue_Range, &Sat_Range, &Lum_Range); GetLargestParticle(destimage,&largestParticleIndex); if(largestParticleIndex != 0) { frcParticleAnalysis(destimage, largestParticleIndex, &Particle_Report); printf("\tLargest Area: %f\n", Particle_Report.particleArea); } else { printf("No particle found...\n"); } } #endif cameraX.Set(CamX); cameraY.Set(CamY); // Logging any values sl.PutOne(); // Wait for our next lap WaitForNextLoop(); } return (0); };