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); }