Planning::MotionInstant Ball::predict(RJ::Time estimateTime) const {
if (estimateTime < time) {
debugThrow("Estimated Time can't be before observation time.");
return MotionInstant();
}
MotionInstant instant;
auto t = RJ::Seconds(estimateTime - time);
const auto s0 = vel.mag();
// Based on sim ball
// v = v0 * e^-0.2913t
// d = v0 * -3.43289 (-1 + e^(-0.2913 t))
auto part = std::exp(-0.2913f * t.count());
auto speed = s0 * part;
auto distance = s0 * -3.43289f * (part - 1.0f);
return MotionInstant(pos + vel.normalized(distance), vel.normalized(speed));
}
void OurRobot::moveDirect(Geometry2d::Point goal, float endSpeed) {
if (!visible) return;
// sets flags for future movement
if (verbose)
cout << " in OurRobot::moveDirect(goal): adding a goal (" << goal.x()
<< ", " << goal.y() << ")" << endl;
_motionCommand = std::make_unique<Planning::DirectPathTargetCommand>(
MotionInstant(goal, (goal - pos).normalized() * endSpeed));
*_cmdText << "moveDirect(" << goal << ")" << endl;
*_cmdText << "endSpeed(" << endSpeed << ")" << endl;
}
void OurRobot::move(Geometry2d::Point goal, Geometry2d::Point endVelocity) {
if (!visible) return;
// sets flags for future movement
if (verbose)
cout << " in OurRobot::move(goal): adding a goal (" << goal.x() << ", "
<< goal.y() << ")" << std::endl;
_motionCommand = std::make_unique<Planning::PathTargetCommand>(
MotionInstant(goal, endVelocity));
*_cmdText << "move(" << goal.x() << ", " << goal.y() << ")" << endl;
*_cmdText << "endVelocity(" << endVelocity.x() << ", " << endVelocity.y()
<< ")" << endl;
}
