Examples with SwerveControllerCommand edu.wpi.first.wpilibj2.command.SwerveControllerCommand used on opensource projects

Example 1 with SwerveControllerCommand

use of edu.wpi.first.wpilibj2.command.SwerveControllerCommand 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 2 with SwerveControllerCommand

use of edu.wpi.first.wpilibj2.command.SwerveControllerCommand in project FRC2022 by 2202Programming.

the class auto_drivePath_cmd method getPathCommand.

public Command getPathCommand() {
    if (path == null) {
        // no path selected
        return new InstantCommand();
    }
    SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(path, // Functional interface to feed supplier
    m_robotDrive::getPose, m_robotDrive.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
    m_robotDrive::drive, m_robotDrive);
    // Reset odometry to the starting pose of the trajectory.
    m_robotDrive.setPose(path.getInitialPose());
    // Run path following command, then stop at the end.
    return swerveControllerCommand.andThen(() -> m_robotDrive.stop()).withTimeout(20);
}

Example 3 with SwerveControllerCommand

use of edu.wpi.first.wpilibj2.command.SwerveControllerCommand in project FRC2022 by 2202Programming.

the class auto_pathPlanner_cmd method getPathCommand.

public Command getPathCommand() {
    if (path == null) {
        System.out.println("No path");
        // no path selected
        return new InstantCommand();
    }
    PIDController xController = new PIDController(4.0, 0.0, 0.0);
    PIDController yController = new PIDController(4.0, 0.0, 0.0);
    ProfiledPIDController thetaController = new ProfiledPIDController(4, 0, 0, new TrapezoidProfile.Constraints(3, 3));
    // Units are radians for thetaController; PPSwerveController is using radians internally.
    // prevent piroutte paths over continuity
    thetaController.enableContinuousInput(-Math.PI, Math.PI);
    PPSwerveControllerCommand swerveControllerCommand = new PPSwerveControllerCommand(path, // Functional interface to feed supplier
    m_robotDrive::getPose, m_robotDrive.getKinematics(), // Position controllers
    xController, yController, thetaController, m_robotDrive::drive, m_robotDrive);
    // Reset odometry to the starting pose of the trajectory.
    m_robotDrive.setPose(path.getInitialPose());
    // Run path following command, then stop at the end.
    return swerveControllerCommand.andThen(() -> m_robotDrive.stop()).withTimeout(20);
}

Example 4 with SwerveControllerCommand

use of edu.wpi.first.wpilibj2.command.SwerveControllerCommand in project FRC2022 by 2202Programming.

the class auto_pathPlanner_cmd method PathFactory.

public static Command PathFactory(SwerveDrivetrain m_robotDrive, String pathname) {
    var path = PathPlanner.loadPath(pathname, 3, 3);
    if (path == null) {
        // no path selected
        return new InstantCommand();
    }
    PIDController xController = new PIDController(4.0, 0.0, 0.0);
    PIDController yController = new PIDController(4.0, 0.0, 0.0);
    ProfiledPIDController thetaController = new ProfiledPIDController(4, 0, 0, new TrapezoidProfile.Constraints(3, 3));
    // Units are radians for thetaController; PPSwerveController is using radians internally.
    // prevent piroutte paths over continuity
    thetaController.enableContinuousInput(-Math.PI, Math.PI);
    PPSwerveControllerCommand swerveControllerCommand = new PPSwerveControllerCommand(path, // Functional interface to feed supplier
    m_robotDrive::getPose, m_robotDrive.getKinematics(), // Position controllers
    xController, yController, thetaController, m_robotDrive::drive, m_robotDrive);
    // Reset odometry to the starting pose of the trajectory.
    m_robotDrive.setPose(path.getInitialPose());
    // Run path following command, then stop at the end.
    return swerveControllerCommand.andThen(() -> m_robotDrive.stop()).withTimeout(20);
}