void KeyPTU::down(void)
{
myDesiredTiltPos -= myPosIncrement;
if (myDesiredTiltPos < myPTU.getMaxNegTilt())
myDesiredTiltPos = myPTU.getMaxNegTilt();
}
void KeyPTU::up(void)
{
myDesiredTiltPos += myPosIncrement;
if (myDesiredTiltPos > myPTU.getMaxPosTilt())
myDesiredTiltPos = myPTU.getMaxPosTilt();
}
void KeyPTU::right(void)
{
myDesiredPanPos -= myPosIncrement;
if (myDesiredPanPos < myPTU.getMaxNegPan())
myDesiredPanPos = myPTU.getMaxNegPan();
}
void KeyPTU::left(void)
{
myDesiredPanPos += myPosIncrement;
if (myDesiredPanPos > myPTU.getMaxPosPan())
myDesiredPanPos = myPTU.getMaxPosPan();
}
void KeyPTU::greater(void)
{
myPosIncrement += posIncIncrement;
if (myPosIncrement > myPTU.getMaxPosPan())
myPosIncrement = myPTU.getMaxPosPan();
status();
}
void KeyPTU::minus(void)
{
mySlew -= mySlewIncrement;
if (mySlew < myPTU.getMinPanSlew())
mySlew = myPTU.getMinPanSlew();
status();
}
void KeyPTU::plus(void)
{
mySlew += mySlewIncrement;
if (mySlew > myPTU.getMaxPanSlew())
mySlew = myPTU.getMaxPanSlew();
status();
}
void KeyPTU::greater(void)
{
myPosIncrement += POS_INCREMENT_ADJUSTMENT;
if (myPosIncrement > myPTU.getMaxPosPan())
myPosIncrement = myPTU.getMaxPosPan();
status();
}
void KeyPTU::z(void)
{
myPTU.pan(0);
myPTU.awaitExec();
myPTU.tilt(0);
myPTU.awaitExec();
myDesiredPanPos = 0;
myDesiredTiltPos = 0;
status();
}
void KeyPTU::m(void)
{
if (!myMonitor)
{
ArLog::log(ArLog::Normal, "Entering Monitor mode - hit 'M' to disable");
myMonitor = true;
myPTU.initMon(-60,60,30,-30);
}
else
{
myPTU.blank(); //Blank packet exits monitor mode
myMonitor = false;
}
}
void KeyPTU::status(void)
{
ArLog::log(ArLog::Normal, "\r\nStatus:\r\n_________________\r\n");
ArLog::log(ArLog::Normal, "Pan Position = %.1f deg", myPTU.getPan());
ArLog::log(ArLog::Normal, "Tilt Position = %.1f deg", myPTU.getTilt());
ArLog::log(ArLog::Normal, "Pan Slew Rate = %d deg/sec", myPTU.getPanSlew());
ArLog::log(ArLog::Normal, "Tilt Slew Rate = %d deg/sec", myPTU.getTiltSlew());
ArLog::log(ArLog::Normal, "Position Increment = %d deg", myPosIncrement);
if (myAbsolute)
ArLog::log(ArLog::Normal, "Positional-movements using absolute commands");
else
ArLog::log(ArLog::Normal, "Positional-movements using relative commands");
ArLog::log(ArLog::Normal, "\r\n");
}
// 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 (!myPTUInited && myRobot->isConnected())
{
myPTU.init();
myPTU.resetCalib();
myPTU.awaitExec();
myPTU.regStatPower();
myPTU.regMotPower();
mySlew = myPTU.getPanSlew(); //uses only pan slew rate
myPTU.awaitExec();
myPTUInited = true;
myInit = false;
myAbsolute = true;
}
if (myInit == true)
{
myPTU.init();
myInit = false;
myDesiredPanPos = myPTU.getPan();
myDesiredTiltPos = myPTU.getTilt();
mySlew = myPTU.getPanSlew(); //uses only pan slew rate
myReset = false;
}
if (myReset == true)
{
myPTU.resetCalib();
myPTU.awaitExec();
myDesiredPanPos = myPTU.getPan();
myDesiredTiltPos = myPTU.getTilt();
myReset = false;
}
else
{
if (myDesiredPanPos != myPTU.getPan())
{
if (myAbsolute)
myPTU.pan(myDesiredPanPos);
else
myPTU.panRel(myDesiredPanPos - myPTU.getPan());
}
if (myDesiredTiltPos != myPTU.getTilt())
{
if (myAbsolute)
myPTU.tilt(myDesiredTiltPos);
else
myPTU.tiltRel(myDesiredTiltPos - myPTU.getTilt());
}
if (mySlew != myPTU.getPanSlew())
{
myPTU.panSlew(mySlew);
myPTU.tiltSlew(mySlew);
}
}
}
void KeyPTU::h(void)
{
myPTU.haltAll();
}
void KeyPTU::a(void)
{
myPTU.awaitExec();
ArLog::log(ArLog::Normal, "AwaitExecution command sent");
}
// 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 (!myPTUInited && myRobot->isConnected())
{
ArLog::log(ArLog::Normal, "Initializing ArDPPTU...");
myPTU.init();
ArLog::log(ArLog::Normal, "Resetting PTU and performing self-calibration...");
myPTU.resetCalib();
myPTU.awaitExec(); // DPPTU will wait for self-calibration to end before executing the following commands (though they will still be sent)
mySlew = myPTU.getPanSlew(); //uses only pan slew rate
myPTU.awaitExec();
myPTUInited = true;
myInit = false;
myAbsolute = true;
}
if (myInit == true) // User hit initialization key
{
ArLog::log(ArLog::Normal, "Initializing PTU...");
myPTU.init();
myInit = false;
myDesiredPanPos = myPTU.getPan();
myDesiredTiltPos = myPTU.getTilt();
mySlew = myPTU.getPanSlew(); //uses only pan slew rate
myReset = false;
}
if (myReset == true) // User hit reset key
{
ArLog::log(ArLog::Normal, "Resetting PTU and performing self-calibration...");
myPTU.resetCalib();
myPTU.awaitExec();
myDesiredPanPos = myPTU.getPan();
myDesiredTiltPos = myPTU.getTilt();
myReset = false;
}
else // User did nothing, or hit a key that changed myDesiredPanPos, myDesiredTiltPos, or mySlew (so request PTU to move if those changed since last request)
{
// Some PTUs can determine their current position (with encoders, etc) and return that.
// canGetRealPanTilt() will return true in this case, and getPan() and
// getTilt() will return those received values. Otherwise, getPan() and
// getTilt() return the last commanded values. getLastPanRequest() and
// getLastTiltRequest() will always return the last commanded values sent by
// ArDPPTU (so in the case that canGetRealPanTilt() is false, getPan() and
// getTilt() return the same pair of values as getLastPanRequest() and
// getLastTiltRequest(). ArDPPTU::canGetRealPanTilt() is initialally false,
// but once the first set of pan and tilt positions is read back from the
// PTU device, it becomes true.
if(myPTU.canGetRealPanTilt())
printf("Position (%.1f deg, %.1f deg) [Incr. %d deg] Press ? for help \r", myPTU.getPan(), myPTU.getTilt(), myPosIncrement);
else
printf("Requested (%.1f deg, %.1f deg) [Incr. %d deg] Press ? for help \r", myPTU.getPan(), myPTU.getTilt(), myPosIncrement);
if (myDesiredPanPos != myPTU.getLastPanRequest())
{
if (myAbsolute)
myPTU.pan(myDesiredPanPos);
else
myPTU.panRel(myDesiredPanPos - myPTU.getPan());
}
if (myDesiredTiltPos != myPTU.getLastTiltRequest())
{
if (myAbsolute)
myPTU.tilt(myDesiredTiltPos);
else
myPTU.tiltRel(myDesiredTiltPos - myPTU.getTilt());
}
if (mySlew != myPTU.getPanSlew())
{
myPTU.panSlew(mySlew);
myPTU.tiltSlew(mySlew);
}
}
}
