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

Example 6 with Translation

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

the class TestStatTracker method testTicksPerSecond.

public void testTicksPerSecond() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01).setSpeed(0.75).setTolerance(2).setAngleTolerance(Angle.fixedDeg(10));
    pathfinder.loadPlugin(new StatTracker());
    pathfinder.goTo(new PointXYZ(10, 10, 0));
    pathfinder.splineTo(new PointXYZ(10, 10, 90), new PointXYZ(12, 11, 45), new PointXYZ(13, 15, 180));
    ElapsedTimer timer = new ElapsedTimer(true);
    SimulatedOdometry odometry = (SimulatedOdometry) pathfinder.getOdometry();
    AtomicInteger i = new AtomicInteger(0);
    try {
        while (timer.elapsedSeconds() < 5) {
            if (Math.random() > 0.5)
                Thread.sleep(2);
            pathfinder.tick();
            Translation translation = pathfinder.getTranslation();
            odometry.setRawPosition(odometry.getRawPosition().add(new PointXYZ(translation.vx() / 50, translation.vy() / 50, Angle.fixedRad(translation.vz() / 50))));
        }
    } catch (Exception ignored) {
    }
    pathfinder.tick();
    System.out.println("tps: " + pathfinder.ticksPerSecond());
    System.out.println("ticks: " + pathfinder.getData("pf_ticks"));
    System.out.println("position: " + pathfinder.getPosition());
    System.out.println("condition met: " + i.get());
    System.out.println("total PointXY: " + PointXY.COUNT);
    System.out.println("total PointXYZ: " + PointXYZ.COUNT);
    System.out.println("total Angle: " + Angle.COUNT);
}

Example 7 with Translation

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

the class ExampleMecanumDrive method loopDrive.

public void loopDrive() {
    if (!pathfinder.isActive()) {
        if (gamepad.areAnyButtonsPressed()) {
            PointXYZ target = null;
            if (gamepad.a()) {
                target = new PointXYZ(10, 10, 0);
            } else if (gamepad.b()) {
                target = new PointXYZ(10, 10, 0);
            } else if (gamepad.x()) {
                target = new PointXYZ(10, 10, 0);
            } else if (gamepad.y()) {
                target = new PointXYZ(10, 10, 0);
            }
            if (target != null) {
                pathfinder.goTo(target);
            }
        }
        Translation translation = joysticks.translation(DualJoystick.Axis.LEFT_Y, DualJoystick.Axis.LEFT_X, DualJoystick.Axis.RIGHT_X);
        pathfinder.getDrive().setTranslation(translation);
    } else {
        if (gamepad.start()) {
            pathfinder.clear();
        }
    }
    pathfinder.tick();
}

Example 8 with Translation

use of me.wobblyyyy.pathfinder2.geometry.Translation 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 9 with Translation

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

the class TestScript method testSetTranslationScript.

@Test
public void testSetTranslationScript() {
    Script script = Script.load(registry, "me/wobblyyyy/pathfinder2/commands/testSetTranslation.pf");
    script.execute();
    Assertions.assertEquals(new Translation(0.5, -0.5, 0.3), pathfinder.getTranslation());
}

Example 10 with Translation

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

the class Pathfinder method moveFor.

/**
 * Move the robot in a certain direction for a certain amount of time.
 * This is a blocking method call, meaning it will block the current thread
 * until its execution has finished (whatever {@code timeoutMs} is).
 *
 * @param translation the translation that will be set to the robot.
 *                    This value may not be null.
 * @param timeoutMs   how long the robot should move for. This value is
 *                    represented in milliseconds and must be greater
 *                    than 0. This value may also not be infinite.
 * @return {@code this, used for method chaining}
 */
@SuppressWarnings("BusyWait")
public Pathfinder moveFor(Translation translation, double timeoutMs) {
    if (translation == null)
        throw new NullPointerException("Cannot use a null translation!");
    if (timeoutMs <= 0 || Double.isInfinite(timeoutMs) || timeoutMs == Double.MAX_VALUE)
        throw new IllegalArgumentException("Invalid timeout!");
    ElapsedTimer timer = new ElapsedTimer(true);
    setTranslation(translation);
    try {
        while (timer.isElapsedLessThan(timeoutMs)) Thread.sleep(Core.pathfinderWaitSleepTimeMs);
    } catch (Exception ignored) {
    }
    return this;
}