From 6b51cafafb5b546b1c23f2236c6e9e8de45fd09f Mon Sep 17 00:00:00 2001 From: keckothedragon Date: Fri, 31 May 2024 18:24:45 -0400 Subject: [PATCH] Fixed drive and added more led stuff --- src/main/java/frc/robot/Constants.java | 20 +++++-------- src/main/java/frc/robot/RobotContainer.java | 6 ++-- src/main/java/frc/robot/RobotState.java | 29 +++++++++++++++++++ .../robot/subsystems/drive/GyroIOPigeon2.java | 2 +- .../subsystems/drive/ModuleIOTalonFX.java | 11 ++++--- .../frc/robot/subsystems/intake/Intake.java | 15 +++++----- .../java/frc/robot/subsystems/led/Led.java | 6 ++++ .../frc/robot/subsystems/shooter/Shooter.java | 6 ++++ 8 files changed, 64 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4505cb1..86a8511 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,16 +111,16 @@ public static final class ShooterConstants { new LoggedTunableNumber("Shooter Pivot Low Angle", 20); public static final LoggedTunableNumber kLeftFlywheelMidVelocity = - new LoggedTunableNumber("Shooter Left Flywheel Mid Velocity", 13.0); + new LoggedTunableNumber("Shooter Left Flywheel Mid Velocity", 9.0); public static final LoggedTunableNumber kRightFlywheelMidVelocity = - new LoggedTunableNumber("Shooter Right Flywheel Mid Velocity", 15.0); + new LoggedTunableNumber("Shooter Right Flywheel Mid Velocity", 11.0); public static final LoggedTunableNumber kPivotMidAngle = new LoggedTunableNumber("Shooter Pivot Mid Angle", 35); public static final LoggedTunableNumber kLeftFlywheelHighVelocity = - new LoggedTunableNumber("Shooter Left Flywheel High Velocity", 19.0); + new LoggedTunableNumber("Shooter Left Flywheel High Velocity", 9.0); public static final LoggedTunableNumber kRightFlywheelHighVelocity = - new LoggedTunableNumber("Shooter Right Flywheel High Velocity", 21.0); + new LoggedTunableNumber("Shooter Right Flywheel High Velocity", 11.0); public static final LoggedTunableNumber kPivotHighAngle = new LoggedTunableNumber("Shooter Pivot High Angle", 55); } @@ -143,15 +143,9 @@ public static final class IntakeConstants { public static final LoggedTunableNumber kPivotI = new LoggedTunableNumber("Intake Pivot I", 0.0); public static final LoggedTunableNumber kPivotD = - new LoggedTunableNumber("Intake Pivot D", 0.0); - public static final double kPivotVelocity = 25.0; - public static final double kPivotAcceleration = 30.0; - public static final ProfiledPIDController kPivotController = - new ProfiledPIDController( - kPivotP.get(), - kPivotI.get(), - kPivotD.get(), - new Constraints(kPivotVelocity, kPivotAcceleration)); + new LoggedTunableNumber("Intake Pivot D", 0.04); + public static final PIDController kPivotController = + new PIDController(kPivotP.get(), kPivotI.get(), kPivotD.get()); } public static class IndexerConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 600e038..f803fbb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -133,10 +133,9 @@ private void configureSubsystems() { m_led = new Led(Ports.kLed, 20); } - m_shooter.setPivotAngle(ShooterConstants.kHomeAngle); - m_intake.setPivotAngle(IntakeConstants.kHomeAngle); - m_robotState = RobotState.startInstance(m_drive, m_shooter, m_indexer, m_intake, m_led); + + m_robotState.resetSetpoints(); } private void configureControllers() { @@ -146,6 +145,7 @@ private void configureControllers() { } else { m_driverControls = new DriverControlsXbox(1); } + m_driverControls = new DriverControlsXbox(1); // TODO: remove after testing } private void configureButtonBindings() { diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index e503b3b..423ef4a 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -156,6 +156,16 @@ public void updateRobotState() { if (edu.wpi.first.wpilibj.RobotState.isDisabled()) { updatePrematchCheck(); } + + if (m_curAction == RobotAction.REV_SHOOTER_LOW + || m_curAction == RobotAction.REV_SHOOTER_MID + || m_curAction == RobotAction.REV_SHOOTER_HIGH) { + if (m_shooter.withinTolerance()) { + m_led.setState(LedState.SHOOTER_READY); + } else { + m_led.setState(LedState.SHOOTER_NOT_READY); + } + } } public void setCurAction(RobotAction action) { @@ -168,11 +178,15 @@ public void setCurAction(RobotAction action) { m_shooter.setShooter(new ShooterPosition(ShooterConstants.kHomeAngle, 0.0, 0.0)); m_intake.setPivotAngle(IntakeConstants.kHomeAngle); m_intake.setRollerVoltage(0.0); + m_indexer.setKickerVoltage(0.0); + m_indexer.setFeederVoltage(0.0); break; case INTAKE: + m_shooter.setShooter(new ShooterPosition(ShooterConstants.kHomeAngle, 0.0, 0.0)); m_intake.setPivotAngle(IntakeConstants.kDeployedAngle); m_intake.setRollerVoltage(IntakeConstants.kDeployRollerVoltage); m_indexer.setFeederVoltage(IndexerConstants.kFeederVoltage); + m_indexer.setKickerVoltage(0.0); break; case REV_SHOOTER_LOW: m_shooter.setShooter( @@ -181,6 +195,8 @@ public void setCurAction(RobotAction action) { ShooterConstants.kLeftFlywheelLowVelocity.get(), ShooterConstants.kRightFlywheelLowVelocity.get())); m_intake.setPivotAngle(IntakeConstants.kDeployedAngle); + m_indexer.setFeederVoltage(0.0); + m_indexer.setKickerVoltage(0.0); break; case REV_SHOOTER_MID: m_shooter.setShooter( @@ -189,6 +205,8 @@ public void setCurAction(RobotAction action) { ShooterConstants.kLeftFlywheelMidVelocity.get(), ShooterConstants.kRightFlywheelMidVelocity.get())); m_intake.setPivotAngle(IntakeConstants.kDeployedAngle); + m_indexer.setKickerVoltage(0.0); + m_indexer.setFeederVoltage(0.0); break; case REV_SHOOTER_HIGH: m_shooter.setShooter( @@ -197,6 +215,8 @@ public void setCurAction(RobotAction action) { ShooterConstants.kLeftFlywheelHighVelocity.get(), ShooterConstants.kRightFlywheelHighVelocity.get())); m_intake.setPivotAngle(IntakeConstants.kDeployedAngle); + m_indexer.setKickerVoltage(0.0); + m_indexer.setFeederVoltage(0.0); break; } } @@ -252,6 +272,7 @@ public void onEnableAutonomous() { public void onDisable() { m_led.setState(LedState.DISABLED_NOT_READY); + resetSetpoints(); } public void setPrematchCheckValue(String key, boolean value) { @@ -269,4 +290,12 @@ public void addVisionObservation(VisionObservation observation) { public Pose2d getOdometryPose() { return m_drive.getPose(); } + + public void resetSetpoints() { + m_shooter.setShooter(new ShooterPosition(ShooterConstants.kHomeAngle, 0.0, 0.0)); + m_intake.setPivotAngle(IntakeConstants.kHomeAngle); + m_intake.setRollerVoltage(0.0); + m_indexer.setFeederVoltage(0.0); + m_indexer.setKickerVoltage(0.0); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 01ccef2..96d1672 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -25,7 +25,7 @@ /** IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(20); + private final Pigeon2 pigeon = new Pigeon2(22, "Drivetrain"); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index df6d1c7..2b1eaf2 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -15,7 +15,6 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VoltageOut; @@ -72,25 +71,25 @@ public ModuleIOTalonFX(int index) { driveTalon = new TalonFX(10, "Drivetrain"); turnTalon = new TalonFX(11, "Drivetrain"); cancoder = new CANcoder(12, "Drivetrain"); - absoluteEncoderOffset = new Rotation2d(2.827); // MUST BE CALIBRATED + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED break; case 1: driveTalon = new TalonFX(7, "Drivetrain"); turnTalon = new TalonFX(8, "Drivetrain"); cancoder = new CANcoder(9, "Drivetrain"); - absoluteEncoderOffset = new Rotation2d(-2.945); // MUST BE CALIBRATED + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED break; case 2: driveTalon = new TalonFX(4, "Drivetrain"); turnTalon = new TalonFX(5, "Drivetrain"); cancoder = new CANcoder(6, "Drivetrain"); - absoluteEncoderOffset = new Rotation2d(0.175); // MUST BE CALIBRATED + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED break; case 3: driveTalon = new TalonFX(1, "Drivetrain"); turnTalon = new TalonFX(2, "Drivetrain"); cancoder = new CANcoder(3, "Drivetrain"); - absoluteEncoderOffset = new Rotation2d(1.226); // MUST BE CALIBRATED + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED break; default: throw new RuntimeException("Invalid module index"); @@ -108,7 +107,7 @@ public ModuleIOTalonFX(int index) { turnTalon.getConfigurator().apply(turnConfig); setTurnBrakeMode(true); - cancoder.getConfigurator().apply(new CANcoderConfiguration()); + // cancoder.getConfigurator().apply(new CANcoderConfiguration()); timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 7d3dd55..50ad5bb 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,6 +1,6 @@ package frc.robot.subsystems.intake; -import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; @@ -22,9 +22,9 @@ public class Intake extends SubsystemBase { public final RollerInputsAutoLogged m_rollerInputs; public final IntakePivotInputsAutoLogged m_pivotInputs; - private ProfiledPIDController m_pivotController; + private PIDController m_pivotController; - public Intake(RollerIO io, IntakePivotIO pivotIO, ProfiledPIDController pivotController) { + public Intake(RollerIO io, IntakePivotIO pivotIO, PIDController pivotController) { m_rollerIO = io; m_pivotIO = pivotIO; m_pivotController = pivotController; @@ -57,13 +57,12 @@ public void periodic() { Logger.recordOutput("Intake/Pivot/PIDVoltage", pivotPidVoltage); Logger.recordOutput( - "Intake/Pivot/DesiredAngle", - Units.radiansToDegrees(m_pivotController.getSetpoint().position)); + "Intake/Pivot/DesiredAngle", Units.radiansToDegrees(m_pivotController.getSetpoint())); Logger.recordOutput( "Intake/Pivot/CurrentAngle", Units.radiansToDegrees(m_pivotInputs.curAngle)); // ready for match - RobotState.getInstance().setPrematchCheckValue("IntakePivot", m_pivotController.atGoal()); + RobotState.getInstance().setPrematchCheckValue("IntakePivot", m_pivotController.atSetpoint()); RobotState.getInstance() .setPrematchCheckValue("RollersStill", Math.abs(m_rollerInputs.curVelocity) < 0.1); } @@ -73,7 +72,7 @@ public void setRollerVoltage(double voltage) { } public void setPivotAngle(Rotation2d angle) { - m_pivotController.setGoal(angle.getRadians()); + m_pivotController.setSetpoint(angle.getRadians()); } public Command runRollerCommand(double voltage) { @@ -81,7 +80,7 @@ public Command runRollerCommand(double voltage) { } public Command setPivotAngleCommand(Rotation2d angle) { - return Commands.runOnce(() -> m_pivotController.setGoal(angle.getRadians())); + return Commands.runOnce(() -> m_pivotController.setSetpoint(angle.getRadians())); } public Rotation2d getPivotAngle() { diff --git a/src/main/java/frc/robot/subsystems/led/Led.java b/src/main/java/frc/robot/subsystems/led/Led.java index f79d23c..f6b94f1 100644 --- a/src/main/java/frc/robot/subsystems/led/Led.java +++ b/src/main/java/frc/robot/subsystems/led/Led.java @@ -15,6 +15,8 @@ public static enum LedState { OFF, AUTONOMOUS, TELEOP, + SHOOTER_NOT_READY, + SHOOTER_READY, DISABLED_NOT_READY, DISABLED_READY } @@ -47,6 +49,9 @@ public void setState(LedState state) { case OFF: setSolidColor(Color.kBlack); break; + case SHOOTER_READY: + setSolidColor(Color.kYellow); + break; case AUTONOMOUS: setSolidColor(Color.kBlue); break; @@ -56,6 +61,7 @@ public void setState(LedState state) { case DISABLED_NOT_READY: setSolidColor(Color.kRed); break; + case SHOOTER_NOT_READY: case DISABLED_READY: setSolidColor(Color.kGreen); break; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 211500c..aff955d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -207,4 +207,10 @@ public Command setShooterCommand(ShooterPosition position) { public Rotation2d getPivotAngle() { return Rotation2d.fromRadians(m_pivotInputs.curAngle); } + + public boolean withinTolerance() { + return m_pivotController.atGoal() + && m_leftFlywheelController.atSetpoint() + && m_rightFlywheelController.atSetpoint(); + } }