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);
}
}
}
void tgCPGActuatorControl::onStep(tgSpringCableActuator& subject, double dt)
{
m_controlTime += dt;
m_totalTime += dt;
/// @todo this fails if its attached to multiple controllers!
/// is there a way to track _global_ time at this level
// Workaround until we implement PID
tgBasicActuator& m_sca = *(tgCast::cast<tgSpringCableActuator, tgBasicActuator>(subject));
if (m_controlTime >= m_controlStep)
{
m_commandedTension = motorControl().control(m_sca, m_controlTime, controlLength(), getCPGValue());
m_controlTime = 0;
}
else
{
m_sca.moveMotors(dt);
}
}
