void Motion::playStep() {
// actuate
for (unsigned int i = 0; i < mMotors.size(); i++) {
Motor *motor = mMotors[i];
double beforeValue = 0.0;
int beforeTime = -1;
double afterValue = 0.0;
int afterTime = -1;
// find beforeValue & beforeTime
vector<Pose *>::iterator poseIt;
for (poseIt = mPoses.begin() ; poseIt != mPoses.end(); ++poseIt) {
Pose *pose = *poseIt;
if (pose->time <= mElapsed) {
MotorCommand *command = pose->commands[i];
if (command->defined) {
beforeTime = pose->time;
beforeValue = command->value;
}
} else
break;
}
// find afterValue & afterTime
vector<Pose *>::reverse_iterator poseRIt;
for (poseRIt = mPoses.rbegin() ; poseRIt != mPoses.rend(); ++poseRIt) {
Pose *pose = *poseRIt;
if (pose->time >= mElapsed) {
MotorCommand *command = pose->commands[i];
if (command->defined) {
afterTime = pose->time;
afterValue = command->value;
}
} else
break;
}
// compute position
bool setPos = false;
double pos = 0.0;
if (afterTime != -1 && mElapsed > mDuration) {
setPos = true;
pos = afterValue;
} else if (beforeTime != -1 && afterTime != -1) {
setPos = true;
if (afterTime == beforeTime)
pos = beforeValue;
else
pos = beforeValue + (mElapsed - beforeTime) * (afterValue - beforeValue) / (afterTime - beforeTime);
}
else if (beforeTime != -1) {
setPos = true;
pos = beforeValue;
}
// apply position
if (setPos)
motor->setPosition(pos);
}
// update internal variables
Robot *robot = Robot::getInstance();
int time = robot->getTime() * 1000.0;
int delta = time - mPreviousTime;
mPreviousTime = time;
if (mReverse) {
if (mElapsed <= 0) {
if (mLoop)
mElapsed = mDuration;
else {
mElapsed = 0;
mPlaying = false;
}
}
else
mElapsed -= delta;
}
else {
if (mElapsed >= mDuration) {
if (mLoop)
mElapsed = 0;
else {
mElapsed = mDuration;
mPlaying = false;
}
}
else
mElapsed += delta;
}
}
