bool InterfaceAvr::openComPort(QString comPort, int baudrate)
{
// for QString to char* conversion
QByteArray ba = comPort.toLatin1();
// check if file (serial port) exists
if (QFile::exists(comPort) == false)
{
emit message(QString("<font color=\"#FF0000\">ERROR: Port '%1'' not found!</font>").arg(comPort));
// this tells other classes that the robot is OFF!
emit robotState(false);
return false;
}
// serial port config and flush also done in openAtmelPort!
if (serialPort->openAtmelPort( ba.data(), baudrate ) == -1)
{
// this tells other classes that the robot is OFF!
emit robotState(false);
return false;
}
return true;
}
//******************************************************************************
void LBRWrenchSineOverlayClient::onStateChange(ESessionState oldState, ESessionState newState)
{
LBRClient::onStateChange(oldState, newState);
switch (newState)
{
// (re)initialize sine parameters when entering Monitoring
case MONITORING_READY:
{
for(int i = 0; i<CART_VECTOR_DIM; i++){_wrench[i] = 0.0;}
_phiX = 0.0;
_phiY = 0.0;
_stepWidthX = 2 * M_PI * _freqHzX * robotState().getSampleTime();
_stepWidthY = 2 * M_PI * _freqHzY * robotState().getSampleTime();
break;
}
default:
{
break;
}
}
}
//******************************************************************************
void LBRWrenchSineOverlayClient::waitForCommand()
{
// In waitForCommand(), the joint values have to be mirrored. Which is done, by calling
// the base method.
LBRClient::waitForCommand();
// If we want to command wrenches, we have to command them all the time. Even in
// waitForCommand(). This has to be done due to consistency checks. In this state
// it is only necessary, that some wrench values are sent.
// The LBR does not take the specific value into account.
if (robotState().getClientCommandMode() == WRENCH)
{
robotCommand().setWrench(_wrench);
}
}
//******************************************************************************
void LBRWrenchSineOverlayClient::command()
{
// In command(), the joint values have to be sent. Which is done, by calling
// the base method.
LBRClient::command();
if (robotState().getClientCommandMode() == WRENCH)
{
// calculate new forces in x and y direction
_wrench[0] = _amplRadX * sin(_phiX);
_wrench[1] = _amplRadY * sin(_phiY);
_phiX += _stepWidthX;
_phiY += _stepWidthY;
if (_phiX >= 2 * M_PI) _phiX -= 2 * M_PI;
if (_phiY >= 2 * M_PI) _phiY -= 2 * M_PI;
robotCommand().setWrench(_wrench);
}
}
