Examples with Angle me.wobblyyyy.pathfinder2.geometry.Angle used on opensource projects

Example 6 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class ExampleSpline method run.

public void run() {
    // before anything else, we have to get some stuff set up.
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    // alright! trajectory time! let's see what's up.
    // as you can see, this is mostly pretty self-explanatory.
    // you create an AdvancedSplineTrajectoryBuilder and use the
    // add methods provided by that class to construct a trajectory.
    // this method works well, but it's a bit verbose - there's a
    // solution to that problem you'll see in just a moment.
    Trajectory trajectory1 = new AdvancedSplineTrajectoryBuilder().setSpeed(0.5).setStep(0.1).setTolerance(2).setAngleTolerance(Angle.fromDeg(5)).add(new PointXYZ(0, 0, 0)).add(new PointXYZ(4, 6, 0)).add(new PointXYZ(6, 12, 0)).add(new PointXYZ(8, 24, 0)).build();
    Trajectory trajectory2 = new AdvancedSplineTrajectoryBuilder().setSpeed(0.5).setStep(0.1).setTolerance(2).setAngleTolerance(Angle.fromDeg(5)).add(new PointXYZ(8, 24, 0)).add(new PointXYZ(6, 36, 0)).add(new PointXYZ(4, 40, 0)).add(new PointXYZ(0, 42, 0)).build();
    // thankfully, there's an easier way to create trajectories just like
    // that - we can make use of a "SplineBuilderFactory"
    SplineBuilderFactory factory = new SplineBuilderFactory().setSpeed(0.5).setStep(0.1).setTolerance(2).setAngleTolerance(Angle.fromDeg(5));
    // set the default speed, step, tolerance, and angle tolerance
    // values for the factory. all of the spline builders produced by
    // the factory will have have these values by default.
    // now we can create new trajectories, without having to repeat
    // the same 4 lines for each of the trajectories.
    Trajectory trajectory3 = factory.builder().add(0, 60, Angle.fromDeg(0)).add(new PointXYZ(20, 60, 0)).add(new PointXYZ(30, 60, 0)).add(new PointXYZ(40, 70, 0)).build();
    Trajectory trajectory4 = factory.builder().add(new PointXYZ(40, 70, 0)).add(new PointXYZ(30, 60, 0)).add(new PointXYZ(20, 60, 0)).add(0, 60, Angle.fromDeg(0)).build();
    // time to actually make the robot move now! once again, most of
    // these methods are fairly self-explanatory. basically, follow
    // the first two trajectories, come to a complete stop, and then
    // follow the next two trajectories, but each of those trajectories
    // should have an individual timeout of 10 seconds.
    pathfinder.followTrajectories(trajectory1, trajectory2).andThen(pf -> {
        // any other code you want to be executed after the
        // trajectory is finished
        pf.setTranslation(new Translation(0, 0, 0));
    }).followTrajectory(trajectory3).tickUntil(10_000).followTrajectory(trajectory4).tickUntil(10_000);
}

Example 7 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class TankKinematics method calculate.

@Override
public TankState calculate(Translation translation) {
    Angle translationAngle = translation.angle();
    double turnDistance = Min.magnitude(Angle.minimumDelta(Angle.DEG_90, translationAngle), Angle.minimumDelta(Angle.DEG_270, translationAngle)) * translation.magnitude();
    double turn = translation.vz() + (turnDistance * turnCoefficient);
    double right = translation.vy() + turn;
    double left = translation.vy() - turn;
    Logger.trace(TankKinematics.class, "r: <%s> l: <%s>", right, left);
    return new TankState(right, left);
}

Example 8 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class Average method of.

public static PointXYZ of(PointXYZ... points) {
    double x = 0;
    double y = 0;
    Angle z = Angle.fromDeg(0);
    for (PointXYZ point : points) {
        x += point.x();
        y += point.y();
        z = z.add(point.z());
    }
    x /= points.length;
    y /= points.length;
    z = Angle.fromDeg(z.deg() / points.length);
    return new PointXYZ(x, y, z);
}

Example 9 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class Pathfinder method goTo.

/**
 * Go to a specific point. This method will create a new linear trajectory.
 *
 * @param point the target point to go to.
 * @return this instance of Pathfinder, used for method chaining.
 * @see #setSpeed(double)
 * @see #setTolerance(double)
 * @see #setAngleTolerance(Angle)
 */
public Pathfinder goTo(PointXYZ point) {
    NullPointException.throwIfInvalid("Attempted to navigate to a null point.", point);
    checkForMissingDefaultValues();
    followTrajectory(new LinearTrajectory(point, speed, tolerance, angleTolerance));
    return this;
}

Example 10 with Angle

use of me.wobblyyyy.pathfinder2.geometry.Angle in project Pathfinder2 by Wobblyyyy.

the class ButtonMapper method getTrajectory.

public Trajectory getTrajectory() {
    if (!gamepad.areAnyButtonsPressed())
        throw new RuntimeException("Cannot get a trajectory without any buttons being pressed!");
    PointXYZ point = null;
    if (gamepad.a())
        point = gamepad.getMappedPoint(Gamepad.InputButton.BUTTON_A);
    else if (gamepad.b())
        point = gamepad.getMappedPoint(Gamepad.InputButton.BUTTON_B);
    else if (gamepad.x())
        point = gamepad.getMappedPoint(Gamepad.InputButton.BUTTON_X);
    else if (gamepad.y())
        point = gamepad.getMappedPoint(Gamepad.InputButton.BUTTON_Y);
    if (point != null) {
        double speed = pathfinder.getSpeed();
        double tolerance = pathfinder.getTolerance();
        Angle angleTolerance = pathfinder.getAngleTolerance();
        return new LinearTrajectory(point, speed, tolerance, angleTolerance);
    }
    return null;
}