Search in sources :

Example 16 with Trajectory

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

the class FourBallTerminalRight method forward.

public Command forward() {
    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(List.of(new Pose2dFeet().set(-5.5, 20.5, 54.0), // shooting location
    new Pose2dFeet().set(0.0, 11.0, 50.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));
}
Also used : Pose2dFeet(team.gif.lib.Pose2dFeet) Trajectory(edu.wpi.first.math.trajectory.Trajectory) RobotTrajectory(team.gif.lib.RobotTrajectory)

Example 17 with Trajectory

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

the class FourBallTerminalRight method pickUpNext2.

public Command pickUpNext2() {
    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(List.of(new Pose2dFeet().set(-3.4, 0.0, 0.0), // ~turn in place
    new Pose2dFeet().set(-3.4, 2.0, 95.0), // 2nd cargo location
    new Pose2dFeet().set(-1.5, 8.0, 95.0), // 3rd cargo (terminal) location
    new Pose2dFeet().set(-5.5, 20.5, 54.0)), RobotTrajectory.getInstance().configReverse);
    // 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));
}
Also used : Pose2dFeet(team.gif.lib.Pose2dFeet) Trajectory(edu.wpi.first.math.trajectory.Trajectory) RobotTrajectory(team.gif.lib.RobotTrajectory)

Example 18 with Trajectory

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

the class Mobility method reverse.

public Command reverse() {
    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(List.of(new Pose2dFeet().set(0.0, 0.0, 0.0), new Pose2dFeet().set(-5.0, 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));
}
Also used : Pose2dFeet(team.gif.lib.Pose2dFeet) Trajectory(edu.wpi.first.math.trajectory.Trajectory) RobotTrajectory(team.gif.lib.RobotTrajectory)

Example 19 with Trajectory

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

the class ThreeBallTerminalMiddle method forward.

public Command forward() {
    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(List.of(new Pose2dFeet().set(-17.0, 4, 0), // shooting position
    new Pose2dFeet().set(-6.0, 2, -5)), 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));
}
Also used : Pose2dFeet(team.gif.lib.Pose2dFeet) Trajectory(edu.wpi.first.math.trajectory.Trajectory) RobotTrajectory(team.gif.lib.RobotTrajectory)

Example 20 with Trajectory

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

the class ThreeBallTerminalMiddle 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(-4.5, 0, 0.0)), RobotTrajectory.getInstance().configReverseSlow);
    // create the command using the trajectory
    RamseteCommand rc = RobotTrajectory.getInstance().createRamseteCommand(trajectory);
    // return rc.andThen(() -> Robot.drivetrain.tankDriveVolts(0, 0));
    return rc;
}
Also used : Pose2dFeet(team.gif.lib.Pose2dFeet) Trajectory(edu.wpi.first.math.trajectory.Trajectory) RobotTrajectory(team.gif.lib.RobotTrajectory)

Aggregations

Trajectory (edu.wpi.first.math.trajectory.Trajectory)48 IOException (java.io.IOException)24 Path (java.nio.file.Path)24 Pose2dFeet (team.gif.lib.Pose2dFeet)19 RobotTrajectory (team.gif.lib.RobotTrajectory)19 PIDController (edu.wpi.first.math.controller.PIDController)5 Pose2d (edu.wpi.first.math.geometry.Pose2d)5 TrajectoryConfig (edu.wpi.first.math.trajectory.TrajectoryConfig)5 ProfiledPIDController (edu.wpi.first.math.controller.ProfiledPIDController)3 Rotation2d (edu.wpi.first.math.geometry.Rotation2d)3 Translation2d (edu.wpi.first.math.geometry.Translation2d)3 RamseteController (edu.wpi.first.math.controller.RamseteController)2 SimpleMotorFeedforward (edu.wpi.first.math.controller.SimpleMotorFeedforward)2 RamseteCommand (edu.wpi.first.wpilibj2.command.RamseteCommand)2 DifferentialDriveKinematics (edu.wpi.first.math.kinematics.DifferentialDriveKinematics)1 DifferentialDriveVoltageConstraint (edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint)1 Waypoint (edu.wpi.first.pathweaver.Waypoint)1 MecanumControllerCommand (edu.wpi.first.wpilibj2.command.MecanumControllerCommand)1 SwerveControllerCommand (edu.wpi.first.wpilibj2.command.SwerveControllerCommand)1 File (java.io.File)1