void KeyPTU::minus(void)
{
myPTU.panSlew(myPTU.getPanSlew() - mySlewIncrement);
myPTU.tiltSlew(myPTU.getTiltSlew() - mySlewIncrement);
status();
}
void KeyPTU::autoupdate(void)
{
if (myPTU.getAutoUpdate())
myPTU.disableAutoUpdate();
else
myPTU.enableAutoUpdate();
}
void KeyPTU::greater(void)
{
myPosIncrement += 1;
if (myPosIncrement > myPTU.getMaxPosPan()) //Use pan range as reference for largest allowable positional increment
myPosIncrement = myPTU.getMaxPosPan();
status();
}
void KeyPTU::p(void)
{
if (myPTU.getPower())
myPTU.power(0);
else
myPTU.power(1);
status();
}
virtual ArActionDesired *fire (ArActionDesired currentDesired)
{
Bottle CAMBottle;
NewData = Mycopyofmodule->GetBottleData("CAMin",&CAMBottle,SamgarModule::NoStep);
if(NewData){WhereInList=0;PrecBottle=CAMBottle;} // if theres new data reset the system
// if its reached the desired tilt/pan get the next lot of data if there is more data in the list
if(ptz.getPan()==pan && ptz.getTilt() == tilt && WhereInList<PrecBottle.size())
{
pan = PrecBottle.get(WhereInList).asDouble();WhereInList++;
tilt = PrecBottle.get(WhereInList).asDouble();WhereInList++;
}
else // if its not reached the desired pan/tilt send the command again.
{
ptz.pan(pan);
ptz.tilt(tilt);
}
return &myDesired;
}
void KeyPTU::status(void)
{
ArLog::log(ArLog::Normal, "\r\nStatus:\r\n_________________________\r\n");
ArLog::log(ArLog::Normal, "Pan Position = %.0f deg", myPTU.getPan());
ArLog::log(ArLog::Normal, "Tilt Position = %.0f 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);
ArLog::log(ArLog::Normal, "Autoupdate = %d", myPTU.getAutoUpdate());
ArLog::log(ArLog::Normal, "Exercise = %d", myExercise);
if (myPTU.getPower())
ArLog::log(ArLog::Normal, "Power is ON");
else
ArLog::log(ArLog::Normal, "Power is OFF");
ArLog::log(ArLog::Normal, "\r\n");
}
void KeyPTU::h(void)
{
myPTU.haltPanTilt();
myPTU.haltZoom();
}
void KeyPTU::z(void)
{
myPTU.panTilt(0,0);
myPTU.zoom(0);
status();
}
void KeyPTU::i(void)
{
myPTU.init();
}
void KeyPTU::c(void)
{
myPTU.zoom(myPTU.getZoom() - myZoomIncrement);
}
void KeyPTU::r(void)
{
myPTU.reset();
}
void KeyPTU::down(void)
{
myPTU.tiltRel(-myPosIncrement);
}
void KeyPTU::up(void)
{
myPTU.tiltRel(myPosIncrement);
}
void KeyPTU::left(void)
{
myPTU.panRel(-myPosIncrement);
}
void KeyPTU::right(void)
{
myPTU.panRel(myPosIncrement);
}
// 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()%((long) myPTU.getMaxPosPan());
else
pan = -ArMath::random()%((long) myPTU.getMaxNegPan());
if (ArMath::random()%2)
tilt = ArMath::random()%((long) myPTU.getMaxPosTilt());
else
tilt = -ArMath::random()%((long) myPTU.getMaxNegTilt());
myPTU.panTilt(pan, tilt);
myExerciseTime.setToNow();
}
}
void KeyPTU::x(void)
{
myPTU.zoom(myPTU.getZoom() + myZoomIncrement);
}
