예제 #1
0
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));
}
예제 #2
0
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;
}
예제 #3
0
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;
}