Examples with Odometry me.wobblyyyy.pathfinder2.robot.Odometry used on opensource projects

Example 1 with Odometry

use of me.wobblyyyy.pathfinder2.robot.Odometry in project Pathfinder2 by Wobblyyyy.

the class Pathfinder method newEmptyPathfinder.

/**
 * Create a new, "empty" instance of Pathfinder.
 *
 * <p>
 * This is pretty much only useful for debugging or testing purposes.
 * </p>
 *
 * @param coefficient the coefficient to use for the turn controller.
 * @return a new instance of Pathfinder that makes use of both the
 * {@link EmptyDrive} and {@link EmptyOdometry} classes.
 */
public static Pathfinder newEmptyPathfinder(double coefficient) {
    Drive drive = new EmptyDrive();
    Odometry odometry = new EmptyOdometry();
    Robot robot = new Robot(drive, odometry);
    return new Pathfinder(robot, coefficient);
}

Example 2 with Odometry

use of me.wobblyyyy.pathfinder2.robot.Odometry in project Pathfinder2 by Wobblyyyy.

the class EasyOdometry method buildOdometry.

public static Odometry buildOdometry(Supplier<Double> xPos, Supplier<Double> yPos, Supplier<Angle> zPos) {
    return new AbstractOdometry() {

        @Override
        public PointXYZ getRawPosition() {
            double x = xPos.get();
            double y = yPos.get();
            Angle z = zPos.get();
            return new PointXYZ(x, y, z);
        }
    };
}

Example 3 with Odometry

use of me.wobblyyyy.pathfinder2.robot.Odometry 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 4 with Odometry

use of me.wobblyyyy.pathfinder2.robot.Odometry in project Pathfinder2 by Wobblyyyy.

the class TestMovementRecorder method testMovementRecorder.

public void testMovementRecorder() {
    Pathfinder pf = Pathfinder.newSimulatedPathfinder(0.01);
    SimulatedOdometry odometry = (SimulatedOdometry) pf.getOdometry();
    Translation translation = new Translation(0.51, 0.51, 0);
    odometry.setVelocity(Angle.DEG_45, 0.5);
    odometry.setTranslation(translation);
    pf.setTranslation(translation);
    pf.getMovementRecorder().start();
    pf.tick();
    ElapsedTimer timer = new ElapsedTimer(true);
    while (timer.elapsedSeconds() < 2) pf.tick();
}

Example 5 with Odometry

use of me.wobblyyyy.pathfinder2.robot.Odometry in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedChassis method beforeEach.

@BeforeEach
public void beforeEach() {
    wrapper = new SimulatedWrapper(new SimulatedDrive(), new SimulatedOdometry());
    odometry = wrapper.getOdometry();
    robot = wrapper.getRobot();
    turnController = new ProportionalController(-0.05);
    pathfinder = new Pathfinder(robot, turnController).setSpeed(0.5).setTolerance(DEFAULT_TOLERANCE).setAngleTolerance(Angle.fromDeg(5));
    factory = new SplineBuilderFactory().setSpeed(0.5).setStep(0.1).setTolerance(DEFAULT_TOLERANCE).setAngleTolerance(Angle.fromDeg(5));
}