Examples with Trajectory com.team254.lib_2014.trajectory.Trajectory used on opensource projects

Example 36 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class TestExecutorManager method testExecutorManager.

@Test
public void testExecutorManager() {
    SimulatedOdometry odometry = new SimulatedOdometry();
    SimulatedDrive drive = new SimulatedDrive();
    Robot robot = new Robot(drive, odometry);
    Trajectory trajectory = new LinearTrajectory(new PointXYZ(10, 10, 0), 1.0, 0.1, Angle.fromDeg(3));
    Controller controller = new GenericTurnController(0.1);
    GenericFollowerGenerator generator = new GenericFollowerGenerator(controller);
    Follower follower = generator.generate(robot, trajectory);
    List<Follower> list = new ArrayList<Follower>() {

        {
            add(follower);
        }
    };
    FollowerExecutor executor = new FollowerExecutor(odometry, drive, list);
    Assertions.assertFalse(executor.tick());
    ExecutorManager manager = new ExecutorManager(robot);
    Assertions.assertFalse(manager.isActive());
    manager.addExecutor(list);
    Assertions.assertTrue(manager.isActive());
    Assertions.assertFalse(manager.isInactive());
    Assertions.assertFalse(manager.tick());
    odometry.setRawPosition(new PointXYZ(10, 10, 0));
    Assertions.assertEquals(1, manager.howManyExecutors());
    Assertions.assertTrue(manager.tick());
    Assertions.assertFalse(manager.isActive());
    Assertions.assertTrue(manager.isInactive());
}

Example 37 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class CircleSurrounder method surround.

/**
 * Surround the circle. This will find the closest point, create a new
 * {@link me.wobblyyyy.pathfinder2.trajectory.LinearTrajectory} to that
 * point, and then instruct Pathfinder to follow it.
 */
public void surround() {
    PointXYZ robotPosition = pathfinder.getPosition();
    double speed = pathfinder.getSpeed();
    double tolerance = pathfinder.getTolerance();
    Angle angleTolerance = pathfinder.getAngleTolerance();
    Trajectory trajectory = CircleSurround.trajectoryToClosestPoint(robotPosition, center, radius, speed, tolerance, angleTolerance);
    pathfinder.followTrajectory(trajectory);
}

Example 38 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class CircleSurrounder method fastSurround.

/**
 * Surround the circle. This will find the closest point, create a new
 * {@link me.wobblyyyy.pathfinder2.trajectory.FastTrajectory} to that
 * point, and then instruct Pathfinder to follow it.
 */
public void fastSurround() {
    PointXYZ robotPosition = pathfinder.getPosition();
    double speed = pathfinder.getSpeed();
    double tolerance = pathfinder.getTolerance();
    Angle angleTolerance = pathfinder.getAngleTolerance();
    Trajectory trajectory = CircleSurround.fastTrajectoryToClosestPoint(robotPosition, center, radius, speed);
    pathfinder.followTrajectory(trajectory);
}

Example 39 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class PathBuilder method fastPath.

public List<Trajectory> fastPath(PointXYZ start, double speed) {
    List<Trajectory> trajectories = new ArrayList<>(targets.size());
    PointXYZ previousPoint = start;
    for (PointXYZ target : targets) {
        trajectories.add(new FastTrajectory(previousPoint, target, speed));
        previousPoint = target;
    }
    return trajectories;
}

Example 40 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Pathfinder2 by Wobblyyyy.

the class Scheduler method tick.

/**
 * Tick the scheduler once.
 *
 * @see Scheduler
 */
public void tick() {
    if (this.tasks.size() == 0)
        return;
    Task currentTask = this.tasks.get(0);
    double currentTimeMilliseconds = Time.ms();
    if (!currentTask.hasStarted()) {
        currentTask.start(currentTimeMilliseconds);
        this.pathfinder.followTrajectory(currentTask.getTrajectory());
    } else {
        if (!(Math.abs(currentTask.getMinTimeMilliseconds()) > 1_000_000)) {
            if (!currentTask.isMinimumTimeLimitValid(currentTimeMilliseconds)) {
                this.pathfinder.tick();
                return;
            }
            Trajectory currentTrajectory = currentTask.getTrajectory();
            PointXYZ currentRobotPosition = this.pathfinder.getOdometry().getPosition();
            boolean isMaximumTimeInvalid = !currentTask.isMaximumTimeLimitValid(currentTimeMilliseconds);
            boolean isTrajectoryCompleted = currentTrajectory.isDone(currentRobotPosition);
            if (isMaximumTimeInvalid || isTrajectoryCompleted) {
                this.dequeueTask(currentTask);
            } else {
                this.pathfinder.tick();
            }
        }
    }
}