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

Example 71 with PointXYZ

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

the class GenericOdometry method updateWithTime.

/**
 * Update the position based on the system's current time (as dictated
 * by {@code Time#ms()} or {@code System#currentTimeMillis()}), the
 * angle of the gyroscope on the robot, and the current state of the robot.
 *
 * <p>
 * This method integrates the velocity of the robot (as determined by
 * the parameter of type {@code T}) over time to determine the position
 * of the robot.
 * </p>
 *
 * @param currentTimeMs the system's current time, in milliseconds.
 *                      Use either {@code Time#ms()} or
 *                      {@code System#currentTimeMillis()} to get this
 *                      value.
 * @param gyroAngle     the angle of the robot's gyroscope.
 * @param state         the current state of the robot. This value
 *                      should usually be determined by using encoders to
 *                      determine the robot's velocity.
 * @return the robot's updated position.
 */
public PointXYZ updateWithTime(double currentTimeMs, Angle gyroAngle, T state) {
    double period = previousTimeMs > -1 ? (currentTimeMs - previousTimeMs) : 0.0;
    // instead of updating the position
    if (period < updateIntervalMs) {
        previousTimeMs = currentTimeMs;
        return position;
    }
    previousTimeMs = currentTimeMs;
    period /= 1_000;
    Angle angle = gyroAngle.add(gyroOffset);
    Translation translation = kinematics.toTranslation(state);
    double dx = translation.vx() * period;
    double dy = translation.vy() * period;
    Angle dta = angle.subtract(previousAngle);
    double dt = dta.rad();
    double sin = dta.sin();
    double cos = dta.cos();
    double s;
    double c;
    if (Math.abs(dt) < 1E-9) {
        s = 1.0 - 1.0 / 6.0 * dt * dt;
        c = 0.5 * dt;
    } else {
        s = sin / dt;
        c = (1 - cos) / dt;
    }
    PointXYZ newPosition = position.add(new PointXYZ(dx * s - dy * c, dx * c + dy * s, 0)).withZ(angle);
    previousAngle = angle;
    position = newPosition;
    return position;
}

Example 72 with PointXYZ

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

the class ExampleFieldNavigation method moveAroundSquare.

public void moveAroundSquare() {
    pathfinder.goTo(new PointXYZ(0, 0, Angle.fromDeg(0)));
    pathfinder.tickUntil();
    pathfinder.goTo(new PointXYZ(10, 0, Angle.fromDeg(0)));
    pathfinder.tickUntil();
    pathfinder.goTo(new PointXYZ(10, 10, Angle.fromDeg(0)));
    pathfinder.tickUntil();
    pathfinder.goTo(new PointXYZ(0, 10, Angle.fromDeg(0)));
    pathfinder.tickUntil();
    pathfinder.goTo(new PointXYZ(0, 0, Angle.fromDeg(0)));
    pathfinder.tickUntil();
}

Example 73 with PointXYZ

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

the class ExampleMecanumDrive method autonomousDrive.

/**
 * Drive in autonomous mode! Very cool.
 * <p>
 * You don't have to do any looping - this method will execute until
 * Pathfinder has finished following its path. There's a variety of
 * ways to improve upon this that we can explore later.
 */
@SuppressWarnings("DuplicatedCode")
public void autonomousDrive() {
    // go in a big rectangle
    List<PointXYZ> path = new ArrayList<PointXYZ>() {

        {
            add(new PointXYZ(0, 0, 0));
            add(new PointXYZ(10, 0, 0));
            add(new PointXYZ(10, 10, 0));
            add(new PointXYZ(0, 10, 0));
            add(new PointXYZ(0, 0, 0));
        }
    };
    for (PointXYZ point : path) {
        pathfinder.goTo(point);
        while (pathfinder.isActive()) {
            // commented out to support jdk8... :(
            // Thread.onSpinWait();
            pathfinder.tick();
        }
    }
}

Example 74 with PointXYZ

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

the class ExamplePathfinder method loop.

/**
 * This is a traditional loop method - it's meant to be run dozens of times
 * per second, over and over and over again. The general premise for this
 * loop is as follows: if Pathfinder is NOT active (meaning it isn't
 * following any trajectories), it'll check for user input using the
 * A, B, X, and Y gamepad buttons. If any of those buttons are pressed,
 * the robot will begin automatically navigating to an associated position.
 * <p>
 * Let's say Pathfinder IS active... what happens then? There will be times
 * when Pathfinder is attempting to control your robot while you'd like to
 * be the one who's in control of it. In this case, you should clear
 * Pathfinder, meaning it will no longer try to follow any paths, meaning
 * you have control over the robot.
 */
@SuppressWarnings("UnnecessaryLocalVariable")
public void loop() {
    if (!pathfinder.isActive()) {
        // If Pathfinder isn't active, let's take a look at our
        // controller inputs.
        PointXYZ targetPoint = null;
        // Pretty cool, right?
        if (gamepadA)
            targetPoint = TARGET_A;
        else if (gamepadB)
            targetPoint = TARGET_B;
        else if (gamepadX)
            targetPoint = TARGET_X;
        else if (gamepadY)
            targetPoint = TARGET_Y;
        if (targetPoint != null) {
            pathfinder.goTo(targetPoint);
        } else {
            // Based on some joysticks, generate a translation.
            // This translation will then be used to drive the robot.
            double moveForwards = joystick1y;
            double moveStrafe = joystick1x;
            double moveRotate = joystick2x;
            Translation translation = new Translation(moveForwards, moveStrafe, moveRotate);
            pathfinder.getRobot().drive().setTranslation(translation);
        }
    }
    if (gamepadStart) {
        // Let's say we want to manually override Pathfinder and regain
        // control of the robot. All we'd have to do:
        pathfinder.clear();
    }
    // ... any other code that you would need in your main loop
    // ex. sensor updates, other motors, you know the deal
    // Tick or update Pathfinder once. Remember, this is absolutely
    // essential - if you don't tick Pathfinder, nothing can happen.
    pathfinder.tick();
}

Example 75 with PointXYZ

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

the class TestSimulatedChassis method testNonMonotonicMultiSplineTrajectory.

@Test
public void testNonMonotonicMultiSplineTrajectory() {
    MultiSplineBuilder builder = new MultiSplineBuilder().setDefaultStep(0.05).setDefaultSpeed(0.5).setDefaultTolerance(0.5).setDefaultAngleTolerance(Angle.fromDeg(5)).add(0, 0, Angle.fromDeg(0), 0.1).add(2, 3, Angle.fromDeg(0), 0.1).add(4, 6, Angle.fromDeg(0), 0.1).add(6, 9, Angle.fromDeg(0), 0.1).add(8, 6, Angle.fromDeg(0), 0.1).add(10, 3, Angle.fromDeg(0), 0.1).add(12, 6, Angle.fromDeg(0), 0.1).add(14, 9, Angle.fromDeg(0), 0.1).add(16, 6, Angle.fromDeg(0), 0.1).add(18, 3, Angle.fromDeg(0), 0.1).add(20, 0, Angle.fromDeg(0), 0.5, 0.1, 2, Angle.fromDeg(5));
    pathfinder.followTrajectory(builder.build());
    pathfinder.tickUntil(500);
    assertPositionIs(new PointXYZ(20, 0, 0));
}