Search in sources :

Example 1 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder 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.getRecorder().start();
    pf.tick();
    ElapsedTimer timer = new ElapsedTimer(true);
    while (timer.elapsedSeconds() < 2) pf.tick();
}
Also used : Pathfinder(me.wobblyyyy.pathfinder2.Pathfinder) Translation(me.wobblyyyy.pathfinder2.geometry.Translation) ElapsedTimer(me.wobblyyyy.pathfinder2.time.ElapsedTimer) SimulatedOdometry(me.wobblyyyy.pathfinder2.robot.simulated.SimulatedOdometry)

Example 2 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder 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));
}
Also used : Pathfinder(me.wobblyyyy.pathfinder2.Pathfinder) SplineBuilderFactory(me.wobblyyyy.pathfinder2.trajectory.spline.SplineBuilderFactory) ProportionalController(me.wobblyyyy.pathfinder2.control.ProportionalController)

Example 3 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder in project Pathfinder2 by Wobblyyyy.

the class TestLinearTrajectory method testZeroPointTurn.

@Test
public void testZeroPointTurn() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    Trajectory trajectory = new LinearTrajectory(pathfinder.getPosition().withHeadingDegrees(45), 0.5, 2, Angle.fromDeg(2));
    SimulatedRobot odometry = (SimulatedRobot) pathfinder.getOdometry();
    pathfinder.followTrajectory(trajectory);
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(odometry.getPosition().withHeading(Angle.fromDeg(15)));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(odometry.getPosition().withHeading(Angle.fromDeg(30)));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(odometry.getPosition().withHeading(Angle.fromDeg(42)));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(odometry.getPosition().withHeading(Angle.fromDeg(48)));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(odometry.getPosition().withHeading(Angle.fromDeg(45)));
    pathfinder.tick();
    Assertions.assertFalse(pathfinder.isActive());
}
Also used : Pathfinder(me.wobblyyyy.pathfinder2.Pathfinder) SimulatedRobot(me.wobblyyyy.pathfinder2.robot.simulated.SimulatedRobot) Test(org.junit.jupiter.api.Test)

Example 4 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder in project Pathfinder2 by Wobblyyyy.

the class TestLinearTrajectory method testMultipleTrajectories.

@Test
public void testMultipleTrajectories() {
    // look... nobody said tests have to be written WELL...
    // i am sincerely sorry for whatever this mess is
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    Trajectory a = new LinearTrajectory(new PointXYZ(0, 0, 0), 0.5, 2, Angle.fromDeg(5));
    Trajectory b = new LinearTrajectory(new PointXYZ(10, 0, 0), 0.5, 2, Angle.fromDeg(5));
    Trajectory c = new LinearTrajectory(new PointXYZ(10, 10, 0), 0.5, 2, Angle.fromDeg(5));
    Trajectory d = new LinearTrajectory(new PointXYZ(0, 10, 0), 0.5, 2, Angle.fromDeg(5));
    Trajectory e = new LinearTrajectory(new PointXYZ(0, 0, 0), 0.5, 2, Angle.fromDeg(5));
    SimulatedRobot o = (SimulatedRobot) pathfinder.getOdometry();
    pathfinder.followTrajectories(a, b, c, d, e);
    int lastFollowers = pathfinder.getExecutorManager().howManyFollowers();
    int followers;
    setPos(o, new PointXYZ(0, 0, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers - 1, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(5, 5, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(10, 0, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers - 1, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(10, 5, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(10, 10, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers - 1, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(0, 10, 0));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers - 1, followers);
    lastFollowers = followers;
    setPos(o, new PointXYZ(0, 0, 0));
    pathfinder.tick();
    Assertions.assertFalse(pathfinder.isActive());
    followers = pathfinder.getExecutorManager().howManyFollowers();
    Assertions.assertEquals(lastFollowers - 1, followers);
}
Also used : Pathfinder(me.wobblyyyy.pathfinder2.Pathfinder) SimulatedRobot(me.wobblyyyy.pathfinder2.robot.simulated.SimulatedRobot) PointXYZ(me.wobblyyyy.pathfinder2.geometry.PointXYZ) Test(org.junit.jupiter.api.Test)

Example 5 with Pathfinder

use of me.wobblyyyy.pathfinder2.Pathfinder in project Pathfinder2 by Wobblyyyy.

the class TestLinearTrajectory method testSimulation.

@Test
public void testSimulation() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    Trajectory a = new LinearTrajectory(new PointXYZ(10, 10, 0), 0.5, 2, Angle.fromDeg(5));
    SimulatedRobot odometry = (SimulatedRobot) pathfinder.getOdometry();
    pathfinder.followTrajectory(a);
    odometry.setPosition(new PointXYZ(0, 0, 0));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.353, 0.353, 0.0), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(0, 0, 15));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 45));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.45), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 40));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.4), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 3));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.0), pathfinder.getTranslation());
    Assertions.assertFalse(pathfinder.isActive());
    Trajectory b = new LinearTrajectory(new PointXYZ(10, 10, 45), 0.5, 2, Angle.fromDeg(5));
    pathfinder.followTrajectory(b);
    odometry.setPosition(new PointXYZ(10, 10, 0));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, -0.45), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 10));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, -0.35), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 20));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, -0.25), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(12, 12, 0));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(-0.353, -0.353, -0.45), pathfinder.getTranslation());
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(12, 12, 45));
    pathfinder.tick();
    Assertions.assertTrue(pathfinder.isActive());
    odometry.setPosition(new PointXYZ(10, 10, 45));
    pathfinder.tick();
    Assertions.assertEquals(new Translation(0.0, 0.0, 0.0), pathfinder.getTranslation());
    Assertions.assertFalse(pathfinder.isActive());
}
Also used : Pathfinder(me.wobblyyyy.pathfinder2.Pathfinder) Translation(me.wobblyyyy.pathfinder2.geometry.Translation) SimulatedRobot(me.wobblyyyy.pathfinder2.robot.simulated.SimulatedRobot) PointXYZ(me.wobblyyyy.pathfinder2.geometry.PointXYZ) Test(org.junit.jupiter.api.Test)

Aggregations

Pathfinder (me.wobblyyyy.pathfinder2.Pathfinder)17 PointXYZ (me.wobblyyyy.pathfinder2.geometry.PointXYZ)14 Trajectory (me.wobblyyyy.pathfinder2.trajectory.Trajectory)7 Translation (me.wobblyyyy.pathfinder2.geometry.Translation)6 SimulatedRobot (me.wobblyyyy.pathfinder2.robot.simulated.SimulatedRobot)6 LinearTrajectory (me.wobblyyyy.pathfinder2.trajectory.LinearTrajectory)6 SimulatedOdometry (me.wobblyyyy.pathfinder2.robot.simulated.SimulatedOdometry)5 TaskTrajectory (me.wobblyyyy.pathfinder2.trajectory.TaskTrajectory)4 Test (org.junit.jupiter.api.Test)4 ProportionalController (me.wobblyyyy.pathfinder2.control.ProportionalController)3 Robot (me.wobblyyyy.pathfinder2.robot.Robot)3 SimulatedDrive (me.wobblyyyy.pathfinder2.robot.simulated.SimulatedDrive)3 ElapsedTimer (me.wobblyyyy.pathfinder2.time.ElapsedTimer)3 SplineBuilderFactory (me.wobblyyyy.pathfinder2.trajectory.spline.SplineBuilderFactory)3 Angle (me.wobblyyyy.pathfinder2.geometry.Angle)2 PointXY (me.wobblyyyy.pathfinder2.geometry.PointXY)2 AdvancedSplineTrajectoryBuilder (me.wobblyyyy.pathfinder2.trajectory.spline.AdvancedSplineTrajectoryBuilder)2 RandomString (me.wobblyyyy.pathfinder2.utils.RandomString)2 Shifter (me.wobblyyyy.pathfinder2.utils.Shifter)2 TimedRobot (edu.wpi.first.wpilibj.TimedRobot)1