Examples with ProfiledPIDController edu.wpi.first.math.controller.ProfiledPIDController used on opensource projects

Example 1 with ProfiledPIDController

use of edu.wpi.first.math.controller.ProfiledPIDController 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));
}

Example 2 with ProfiledPIDController

use of edu.wpi.first.math.controller.ProfiledPIDController in project FRC2022-Rapid-React by BitBucketsFRC4183.

the class AutonomousFollowPathCommand method createTrajectoryFollowerCommand.

private PPSwerveControllerCommand createTrajectoryFollowerCommand() {
    PIDController xyController = new PIDController(1, 0.1, 0);
    ProfiledPIDController thetaController = new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(this.autoConfig.maxPathFollowVelocity, this.autoConfig.maxPathFollowAcceleration));
    thetaController.enableContinuousInput(-Math.PI, Math.PI);
    return new PPSwerveControllerCommand(// Trajectory
    this.trajectory, // Robot Pose supplier
    () -> this.drive.odometry.getPoseMeters(), // Swerve Drive Kinematics
    this.drive.kinematics, // PID Controller: X
    xyController, // PID Controller: Y
    xyController, // PID Controller: Θ
    thetaController, // SwerveModuleState setter
    this.drive::setStates, this.drive);
}

Example 3 with ProfiledPIDController

use of edu.wpi.first.math.controller.ProfiledPIDController 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;
}

Example 4 with ProfiledPIDController

use of edu.wpi.first.math.controller.ProfiledPIDController 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 5 with ProfiledPIDController

use of edu.wpi.first.math.controller.ProfiledPIDController 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);
}