void KeyPTU::minus(void)
{
myPTU.panSlew(myPTU.getPanSlew() - mySlewIncrement);
myPTU.tiltSlew(myPTU.getTiltSlew() - mySlewIncrement);
status();
}
// the important function
void KeyPTU::drive(void)
{
// if the PTU isn't initialized, initialize it here... it has to be
// done here instead of above because it needs to be done when the
// robot is connected
if (!myPTUInitRequested && !myPTU.isInitted() && myRobot->isConnected())
{
printf("\nWaiting for Camera to Initialize\n");
myAbsolute = true;
myPTUInitRequested = true;
myPTU.init();
}
// if the camera hasn't initialized yet, then just return
if (myPTUInitRequested && !myPTU.isInitted())
{
return;
}
if (myPTUInitRequested && myPTU.isInitted())
{
myPTUInitRequested = false;
myPanSlew = myPTU.getPanSlew();
myTiltSlew = myPTU.getTiltSlew();
printf("Done.\n");
question();
}
if (myExerciseTime.secSince() > 5 && myExercise)
{
int pan,tilt;
if (ArMath::random()%2)
pan = ArMath::random()%((int)myPTU.getMaxPosPan());
else
pan = -ArMath::random()%((int)myPTU.getMaxNegPan());
if (ArMath::random()%2)
tilt = ArMath::random()%((int)myPTU.getMaxPosTilt());
else
tilt = -ArMath::random()%((int)myPTU.getMaxNegTilt());
myPTU.panTilt(pan, tilt);
//printf("** %d\n", myRobot->getEstop());
//printf("--> %x\n", myRobot->getFlags());
myExerciseTime.setToNow();
}
}
void KeyPTU::status(void)
{
ArLog::log(ArLog::Normal, "\r\nStatus:\r\n_________________________\r\n");
ArLog::log(ArLog::Normal, "Pan Position = %d deg", myPTU.getPan());
ArLog::log(ArLog::Normal, "Tilt Position = %d deg", myPTU.getTilt());
ArLog::log(ArLog::Normal, "Zoom Position = %d", myPTU.getZoom());
ArLog::log(ArLog::Normal, "Pan Slew = %d deg/s", myPTU.getPanSlew());
ArLog::log(ArLog::Normal, "Tilt Slew = %d deg/s", myPTU.getTiltSlew());
ArLog::log(ArLog::Normal, "Position Increment = %d deg", myPosIncrement);
if (myPTU.getPower())
ArLog::log(ArLog::Normal, "Power is ON");
else
ArLog::log(ArLog::Normal, "Power is OFF");
ArLog::log(ArLog::Normal, "\r\n");
}