Examples with Trajectory edu.wpi.first.math.trajectory.Trajectory used on opensource projects

Example 11 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project Entropy2022 by Team138Entropy.

the class TrajectoryLibrary method get_New_T35_B5.

public Trajectory get_New_T35_B5() {
    Trajectory traj = null;
    String fileName = "New-T35_B5.wpilib.json";
    try {
        Path trajectoryPath = getTrajectoryPath(fileName);
        traj = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
    } catch (IOException ex) {
        System.out.println("Unable to open trajectory: " + fileName);
    }
    return traj;
}

Example 12 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project FRC2022 by 2202Programming.

the class getTrajectoryFollowTest method initialize.

// Called when the command is initially scheduled.
@Override
public void initialize() {
    // An example trajectory to follow. All units in feet.
    Rotation2d current_angle = new Rotation2d(sensors.getYaw());
    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0.0, 0.0, current_angle), List.of(), new Pose2d(0, 3.0, current_angle), // max velocity, max accel
    new TrajectoryConfig(2.0, 0.5));
    SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(exampleTrajectory, // Functional interface to feed supplier
    drivetrain::getPose, drivetrain.getKinematics(), // Position controllers
    new PIDController(4.0, 0.0, 0.0), new PIDController(4.0, 0.0, 0.0), new ProfiledPIDController(4, 0, 0, new TrapezoidProfile.Constraints(.3, .3)), // per second squared
    drivetrain::drive, drivetrain);
    // Reset odometry to the starting pose of the trajectory.
    drivetrain.setPose(exampleTrajectory.getInitialPose());
    // Run path following command, then stop at the end.
    work = swerveControllerCommand.andThen(() -> drivetrain.stop()).withTimeout(10);
}

Example 13 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project FRC2022 by Team2338.

the class FiveBallTerminalRight method pickupTerminal.

public Command pickupTerminal() {
    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(List.of(new Pose2dFeet().set(-1.5, 13.0, 55.0), // 3rd cargo (terminal) location
    new Pose2dFeet().set(-5.5, 21.0, 43.0)), RobotTrajectory.getInstance().configReverseFast);
    // create the command using the trajectory
    RamseteCommand rc = RobotTrajectory.getInstance().createRamseteCommand(trajectory);
    // Run path following command, then stop at the end.
    return rc.andThen(() -> Robot.drivetrain.tankDriveVolts(0, 0));
}

Example 14 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project FRC2022 by Team2338.

the class FiveBallTerminalRight method reverse.

public Command reverse() {
    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(List.of(new Pose2dFeet().set(0.0, 0.0, 0.0), // 1st cargo location
    new Pose2dFeet().set(-3.4, 0.0, 0.0)), RobotTrajectory.getInstance().configReverseSlow);
    // create the command using the trajectory
    RamseteCommand rc = RobotTrajectory.getInstance().createRamseteCommand(trajectory);
    // Run path following command, then stop at the end.
    return rc.andThen(() -> Robot.drivetrain.tankDriveVolts(0, 0));
}

Example 15 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project FRC2022 by Team2338.

the class FiveBallTerminalRight method forward.

public Command forward() {
    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(List.of(new Pose2dFeet().set(-5.5, 21.0, 43.0), // shooting location
    new Pose2dFeet().set(0.0, 13.0, 55.0)), RobotTrajectory.getInstance().configForwardFast);
    // create the command using the trajectory
    RamseteCommand rc = RobotTrajectory.getInstance().createRamseteCommand(trajectory);
    // Run path following command, then stop at the end.
    return rc.andThen(() -> Robot.drivetrain.tankDriveVolts(0, 0));
}