Search in sources :

Example 1 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project 6591-2022 by jafillmore.

the class RobotContainer method getAutonomousCommand.

/**
 * Use this to pass the autonomous command to the main {@link Robot} class.
 *
 * @return the command to run in autonomous
 */
public Command getAutonomousCommand() {
    // Create config for trajectory
    TrajectoryConfig config = new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond, AutoConstants.kMaxAccelerationMetersPerSecondSquared).setKinematics(DriveConstants.kDriveKinematics);
    // An example trajectory to follow.  All units in meters.
    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(// Start at the origin facing the +X direction
    new Pose2d(0, 0, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path
    List.of(new Translation2d(1, 1), new Translation2d(2, -1)), // End 3 meters straight ahead of where we started, facing forward
    new Pose2d(3, 0, new Rotation2d(0)), config);
    MecanumControllerCommand mecanumControllerCommand = new MecanumControllerCommand(exampleTrajectory, m_robotDrive::getPose, DriveConstants.kFeedforward, DriveConstants.kDriveKinematics, // Position contollers
    new PIDController(AutoConstants.kPXController, 0, 0), new PIDController(AutoConstants.kPYController, 0, 0), new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints), // Needed for normalizing wheel speeds
    AutoConstants.kMaxSpeedMetersPerSecond, // Velocity PID's
    new PIDController(DriveConstants.kPFrontLeftVel, 0, 0), new PIDController(DriveConstants.kPRearLeftVel, 0, 0), new PIDController(DriveConstants.kPFrontRightVel, 0, 0), new PIDController(DriveConstants.kPRearRightVel, 0, 0), m_robotDrive::getCurrentWheelSpeeds, // Consumer for the output motor voltages
    m_robotDrive::setDriveMotorControllersVolts, m_robotDrive);
    // Reset odometry to the starting pose of the trajectory.
    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
    // Run path following command, then stop at the end.
    return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
}
Also used : Translation2d(edu.wpi.first.math.geometry.Translation2d) Rotation2d(edu.wpi.first.math.geometry.Rotation2d) Pose2d(edu.wpi.first.math.geometry.Pose2d) TrajectoryConfig(edu.wpi.first.math.trajectory.TrajectoryConfig) Trajectory(edu.wpi.first.math.trajectory.Trajectory) ProfiledPIDController(edu.wpi.first.math.controller.ProfiledPIDController) MecanumControllerCommand(edu.wpi.first.wpilibj2.command.MecanumControllerCommand) PIDController(edu.wpi.first.math.controller.PIDController) ProfiledPIDController(edu.wpi.first.math.controller.ProfiledPIDController)

Example 2 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project 2022RobotCode by AusTINCANsProgrammingTeam.

the class AutonModes method getTrajectories.

private Trajectory[] getTrajectories(String... pathName) {
    Trajectory[] trajectories = new Trajectory[pathName.length];
    for (int i = 0; i < pathName.length; i++) {
        Path path = Filesystem.getDeployDirectory().toPath().resolve(// goes to scr/main/deploy/paths
        pathName[i]);
        try {
            trajectories[i] = TrajectoryUtil.fromPathweaverJson(path);
            System.out.println("Success: " + pathName[i] + " created.");
        } catch (IOException e) {
            System.out.println("Trajectory: " + pathName[i] + " not created.");
        }
    }
    return trajectories;
}
Also used : Path(java.nio.file.Path) Trajectory(edu.wpi.first.math.trajectory.Trajectory) IOException(java.io.IOException)

Example 3 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project RapidReact2022 by robototes.

the class AutonomousCommand method getAutonomousCommand.

public Command getAutonomousCommand() {
    // Create config for trajectory
    TrajectoryConfig config = new TrajectoryConfig(AutoConstants.MAX_SPEED_METERS_PER_SECOND, AutoConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED).setKinematics(AutoConstants.driveKinematics);
    // creating trajectory path (right now is a square)
    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0, 0, Rotation2d.fromDegrees(0)), List.of(new Translation2d(1, 0), new Translation2d(1, 1), new Translation2d(0, 1)), new Pose2d(0, 0.0, Rotation2d.fromDegrees(0)), config);
    // creates thetacontroller (rotation)
    ProfiledPIDController thetaController = new ProfiledPIDController(0.000000005, 0, 0, AutoConstants.K_THETA_CONTROLLER_CONSTRAINTS);
    thetaController.enableContinuousInput(-Math.PI, Math.PI);
    exampleTrajectory.relativeTo(drivebaseSubsystem.getPoseAsPoseMeters());
    SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(exampleTrajectory, // Functional interface to feed supplier
    drivebaseSubsystem::getPoseAsPoseMeters, AutoConstants.driveKinematics, // Position controllers
    new PIDController(AutoConstants.PX_CONTROLLER, 0, 0), new PIDController(AutoConstants.PY_CONTROLLER, 0, 0), thetaController, drivebaseSubsystem::updateModules, drivebaseSubsystem);
    return swerveControllerCommand;
}
Also used : Translation2d(edu.wpi.first.math.geometry.Translation2d) Pose2d(edu.wpi.first.math.geometry.Pose2d) TrajectoryConfig(edu.wpi.first.math.trajectory.TrajectoryConfig) Trajectory(edu.wpi.first.math.trajectory.Trajectory) ProfiledPIDController(edu.wpi.first.math.controller.ProfiledPIDController) PIDController(edu.wpi.first.math.controller.PIDController) ProfiledPIDController(edu.wpi.first.math.controller.ProfiledPIDController)

Example 4 with Trajectory

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

the class TrajectoryLibrary method get_New_T2_Terminal.

public Trajectory get_New_T2_Terminal() {
    Trajectory traj = null;
    String fileName = "New-T2_terminal.wpilib.json";
    try {
        Path trajectoryPath = getTrajectoryPath(fileName);
        traj = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
    } catch (IOException ex) {
        System.out.println("Unable to open trajectory: " + fileName);
    }
    return traj;
}
Also used : Path(java.nio.file.Path) Trajectory(edu.wpi.first.math.trajectory.Trajectory) IOException(java.io.IOException)

Example 5 with Trajectory

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

the class TrajectoryLibrary method get_New_T1_B2.

public Trajectory get_New_T1_B2() {
    Trajectory traj = null;
    String fileName = "New-T1_B2.wpilib.json";
    try {
        Path trajectoryPath = getTrajectoryPath(fileName);
        traj = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
    } catch (IOException ex) {
        System.out.println("Unable to open trajectory: " + fileName);
    }
    return traj;
}
Also used : Path(java.nio.file.Path) Trajectory(edu.wpi.first.math.trajectory.Trajectory) IOException(java.io.IOException)

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