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