void MotionPlayer::step()
{
//Just for debug
//ROS_WARN_THROTTLE(0.1, "Playing motion '%s', time is %lf", m_playingMotion->motionName.c_str(), m_playingMotion->player[0].getCurrentState().t);
kf_player::Keyframe cmdFrame;
for (unsigned i = 0; i < m_playingMotion->player.size(); i++)
{
if (m_playingMotion->player[i].atEnd())
{
finished();
return;
}
cmdFrame = m_playingMotion->player[i].step(m_sysItTime);
int index = m_playingMotion->motionToModel[i];
m_cmdPositions[index] = cmdFrame.x;
m_cmdEffort[index] = cmdFrame.eff;
}
m_model->resetSupport();
writeCommands();
}
void OLED_SSD1331::goTo(int x, int y) {
if ((x >= WIDTH) || (y >= HEIGHT)) return;
uint8_t cmd[] = {_CMD_SETCOLUMN,(uint8_t)x,OLED_MW,_CMD_SETROW,(uint8_t)y,OLED_MH};
writeCommands(cmd, 6);
}
