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

Example 1 with PIDController

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

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

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

use of edu.wpi.first.math.controller.PIDController in project 3128-robot-2022 by Team3128.

the class RobotContainer method initAutos.

private void initAutos() {
    try {
        for (int i = 0; i < trajJson.length; i++) {
            // Get a path from the string specified in trajJson, and load it into trajectory[i]
            Path path = Filesystem.getDeployDirectory().toPath().resolve("paths").resolve(trajJson[i]);
            trajectory[i] = TrajectoryUtil.fromPathweaverJson(path);
            Log.info("InitAutos", "Trajectory" + i + " = path" + path.toString());
        }
    } catch (IOException ex) {
        DriverStation.reportError("IOException opening trajectory:", ex.getStackTrace());
    }
    initialPoses = new HashMap<Command, Pose2d>();
    climbCommand = new CmdClimb(m_climber);
    climbTraversalCommand = new CmdClimbTraversal(m_climber);
    extendIntakeAndReverse = new SequentialCommandGroup(new CmdExtendIntake(m_intake).withTimeout(0.1), new CmdReverseIntake(m_intake, m_hopper));
    // this shoot command is the ideal one with all capabilities
    shootCommand = new SequentialCommandGroup(new InstantCommand(m_shooterLimelight::turnLEDOn), new CmdRetractHopper(m_hopper), new InstantCommand(() -> m_shooter.setState(ShooterState.UPPERHUB)), // new CmdExtendIntake(m_intake),
    new ParallelCommandGroup(// new RunCommand(m_intake::runIntake, m_intake),
    new CmdAlign(m_drive, m_shooterLimelight), new CmdHopperShooting(m_hopper, m_shooter::isReady), new CmdShootDist(m_shooter, m_shooterLimelight)));
    // use this shoot command for testing
    manualShoot = new SequentialCommandGroup(new InstantCommand(m_shooterLimelight::turnLEDOn), new CmdRetractHopper(m_hopper), new InstantCommand(() -> m_shooter.setState(ShooterState.UPPERHUB)), new ParallelCommandGroup(new CmdHopperShooting(m_hopper, m_shooter::isReady), new CmdShootDist(m_shooter, m_shooterLimelight)));
    lowerHubShoot = new SequentialCommandGroup(new CmdRetractHopper(m_hopper), new InstantCommand(() -> m_shooter.setState(ShooterState.LOWERHUB)), // new CmdExtendIntake(m_intake),
    new ParallelCommandGroup(// new RunCommand(m_intake::runIntake, m_intake),
    new RunCommand(m_drive::stop, m_drive), new CmdHopperShooting(m_hopper, m_shooter::isReady), new CmdShootRPM(m_shooter, 1100)));
    // AUTONOMOUS ROUTINES
    auto_1Ball = new SequentialCommandGroup(retractHopperAndShootCmdLL(3750), new RamseteCommand(Trajectories.driveBack30In, m_drive::getPose, new RamseteController(Constants.DriveConstants.RAMSETE_B, Constants.DriveConstants.RAMSETE_ZETA), new SimpleMotorFeedforward(Constants.DriveConstants.kS, Constants.DriveConstants.kV, Constants.DriveConstants.kA), Constants.DriveConstants.DRIVE_KINEMATICS, m_drive::getWheelSpeeds, new PIDController(Constants.DriveConstants.RAMSETE_KP, 0, 0), new PIDController(Constants.DriveConstants.RAMSETE_KP, 0, 0), m_drive::tankDriveVolts, m_drive));
    auto_2BallBot = new SequentialCommandGroup(// pick up 1 ball
    new ParallelDeadlineGroup(trajectoryCmd(0).andThen(m_drive::stop, m_drive), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot preloaded + first
    retractHopperAndShootCmd(3250));
    auto_2BallMid = new SequentialCommandGroup(// pick up 1 ball
    new ParallelDeadlineGroup(trajectoryCmd(1).andThen(m_drive::stop, m_drive), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot first + preloaded
    retractHopperAndShootCmd(3250));
    auto_2BallTop = new SequentialCommandGroup(new CmdExtendIntake(m_intake), new ParallelDeadlineGroup(trajectoryCmd(2).andThen(m_drive::stop, m_drive), new CmdIntakeCargo(m_intake, m_hopper)), new InstantCommand(() -> m_intake.retractIntake()), // shoot first + preloaded
    retractHopperAndShootCmdLL(3750));
    auto_3BallHook = new SequentialCommandGroup(// shoot preloaded ball
    retractHopperAndShootCmd(3350), // pick up two balls
    new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(3), trajectoryCmd(4), new InstantCommand(m_drive::stop, m_drive)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot two balls
    retractHopperAndShootCmd(3250));
    auto_3BallHersheyKiss = new SequentialCommandGroup(// shoot preload
    retractHopperAndShootCmd(3500), // pick up two balls
    new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(5), trajectoryCmd(6), new InstantCommand(m_drive::stop, m_drive)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot two balls
    retractHopperAndShootCmdLL(3500));
    auto_3BallTerminal = new SequentialCommandGroup(// shoot preloaded ball
    retractHopperAndShootCmd(3000), trajectoryCmd(7), new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(8), new InstantCommand(m_drive::stop, m_drive), new WaitCommand(1)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), trajectoryCmd(19), trajectoryCmd(20), new InstantCommand(m_drive::stop, m_drive), // shoot two balls
    retractHopperAndShootCmd(3000));
    auto_4BallE = new SequentialCommandGroup(// pick up first ball
    new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(11), new InstantCommand(m_drive::stop, m_drive)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot first + preloaded
    retractHopperAndShootCmd(3000), // pick up two more balls
    new CmdExtendIntake(m_intake).withTimeout(0.1), new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(12), new InstantCommand(m_drive::stop, m_drive)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), // shoot two more balls
    retractHopperAndShootCmd(3250));
    auto_4BallTerm = new SequentialCommandGroup(new CmdExtendIntake(m_intake), new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(13), new InstantCommand(m_drive::stop, m_drive)), new CmdIntakeCargo(m_intake, m_hopper)), new CmdRetractHopper(m_hopper), retractHopperAndShootCmd(3750), new CmdExtendIntake(m_intake), new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(14), new InstantCommand(m_drive::stop, m_drive), new WaitCommand(0.5)), new CmdIntakeCargo(m_intake, m_hopper)), new CmdRetractHopper(m_hopper), trajectoryCmd(15), trajectoryCmd(16), new InstantCommand(m_drive::stop, m_drive), // shoot two balls
    retractHopperAndShootCmd(3750));
    auto_5Ball = new SequentialCommandGroup(new SequentialCommandGroup(new CmdRetractHopper(m_hopper).withTimeout(0.5), new ParallelCommandGroup(new CmdHopperShooting(m_hopper, m_shooter::isReady), new CmdShootRPM(m_shooter, 3250)).withTimeout(1)), // pick up first ball
    new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(15), trajectoryCmd(16), new InstantCommand(m_drive::stop, m_drive)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), retractHopperAndShootCmd(3750), trajectoryCmd(17), new ParallelDeadlineGroup(new SequentialCommandGroup(trajectoryCmd(18), new InstantCommand(m_drive::stop, m_drive), new WaitCommand(0.5)), new CmdExtendIntakeAndRun(m_intake, m_hopper)), trajectoryCmd(19), trajectoryCmd(20), new InstantCommand(m_drive::stop, m_drive), // shoot two balls
    retractHopperAndShootCmd(3750));
    // Setup auto-selector
    NarwhalDashboard.addAuto("1 Ball", auto_1Ball);
    NarwhalDashboard.addAuto("2 Ball Bottom", auto_2BallBot);
    NarwhalDashboard.addAuto("2 Ball Mid", auto_2BallMid);
    NarwhalDashboard.addAuto("2 Ball Top", auto_2BallTop);
    NarwhalDashboard.addAuto("3 Ball Hook", auto_3BallHook);
    NarwhalDashboard.addAuto("3 Ball Terminal", auto_3BallTerminal);
    NarwhalDashboard.addAuto("3 Ball Hershey Kiss", auto_3BallHersheyKiss);
    NarwhalDashboard.addAuto("4 Ball E", auto_4BallE);
    NarwhalDashboard.addAuto("4 Ball Terminal", auto_4BallTerm);
    NarwhalDashboard.addAuto("5 Ball", auto_5Ball);
}

Example 5 with PIDController

use of edu.wpi.first.math.controller.PIDController in project FRC6823_2022 by 6823Code.

the class RotateToZero method initialize.

@Override
public void initialize() {
    angleController = new PIDController(.3, 0, 0);
    angleController.enableContinuousInput(0, Math.PI * 2);
    angleController.setSetpoint(initialDegrees);
}