void tgCPGCableControl::onStep(tgSpringCableActuator& subject, double dt)
{
assert(&subject == m_PID->getControllable());
m_controlTime += dt;
m_totalTime += dt;
if (m_controlTime >= m_controlStep)
{
if (usePID)
{
m_commandedTension = motorControl().control(*m_PID, dt, controlLength(), getCPGValue());
}
else
{
tgBasicActuator* basicAct = tgCast::cast<tgSpringCableActuator, tgBasicActuator>(&subject);
m_commandedTension = motorControl().control(*basicAct, dt, controlLength(), getCPGValue());
}
m_controlTime = 0;
}
else
{
const double currentTension = subject.getTension();
if(usePID)
{
m_PID->control(dt, m_commandedTension, currentTension);
}
else
{
tgBasicActuator* basicAct = tgCast::cast<tgSpringCableActuator, tgBasicActuator>(&subject);
basicAct->moveMotors(dt);
}
}
}
std::vector<double> JSONMetricsFeedbackControl::getCableState(const tgSpringCableActuator& cable)
{
// For each string, scale value from -1 to 1 based on initial length or max tension of motor
std::vector<double> state;
// Scale length by starting length
const double startLength = cable.getStartLength();
state.push_back((cable.getCurrentLength() - startLength) / startLength);
const double maxTension = cable.getConfig().maxTens;
state.push_back((cable.getTension() - maxTension / 2.0) / maxTension);
return state;
}
