From ddf9bafb9ed5451160e4d2026b8a699aa051c6e8 Mon Sep 17 00:00:00 2001 From: ACat701 <90939494+ACat701@users.noreply.github.com> Date: Sat, 19 Oct 2024 15:14:35 -0700 Subject: [PATCH 1/7] Add Intake Note Sensor (No logic yet) --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/RobotMap.java | 1 + .../frc/robot/commands/Autos/Centerline.java | 16 +++++++------- .../frc/robot/commands/Autos/PreloadOnly.java | 4 ++-- .../frc/robot/commands/Autos/PreloadTaxi.java | 8 +++---- .../frc/robot/commands/Autos/WingOnly.java | 22 +++++++++---------- .../frc/robot/commands/States/Climbing.java | 2 +- .../robot/commands/States/IntakeSource.java | 4 ++-- .../frc/robot/commands/States/Intaking.java | 2 +- .../frc/robot/commands/States/Shooting.java | 2 +- .../java/frc/robot/subsystems/Intake.java | 10 ++++++++- .../java/frc/robot/subsystems/Transfer.java | 4 ++-- 13 files changed, 45 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 928aea6..c672638 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -528,6 +528,7 @@ public static class constIntake { public static final Measure INTAKING_SPEED = Units.Percent.of(1); public static final Measure EJECTING_SPEED = Units.Percent.of(-1); public static final InvertedValue MOTOR_INVERT = InvertedValue.Clockwise_Positive; + public static final boolean NOTE_SENSOR_INVERT = true; // -- Current Limiting -- public static final boolean ENABLE_CURRENT_LIMITING = true; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aec299a..4590a85 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -69,10 +69,10 @@ public class RobotContainer { private final static Transfer subTransfer = new Transfer(); private final static Limelight subLimelight = new Limelight(); - private final Trigger gamePieceTrigger = new Trigger(() -> subTransfer.getGamePieceCollected()); + private final Trigger gamePieceTrigger = new Trigger(() -> subTransfer.getGamePieceStored()); private final Trigger readyToShoot = new Trigger(() -> subDrivetrain.isDrivetrainFacingSpeaker() && subShooter.readyToShoot() && subStateMachine.isCurrentStateTargetState() - && subTransfer.getGamePieceCollected()); + && subTransfer.getGamePieceStored()); private final IntakeSource comIntakeSource = new IntakeSource(subStateMachine, subShooter, subTransfer); SendableChooser autoChooser = new SendableChooser<>(); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index a2ca1ca..3e58a08 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -44,6 +44,7 @@ public static class mapShooter { // MOTORS: 20 -> 29 public static class mapIntake { public static final int ROLLER_CAN = 20; + public static final int NOTE_SENSOR_DIO = 2; } // MOTORS: 30 -> 39 diff --git a/src/main/java/frc/robot/commands/Autos/Centerline.java b/src/main/java/frc/robot/commands/Autos/Centerline.java index 5866983..b40f722 100644 --- a/src/main/java/frc/robot/commands/Autos/Centerline.java +++ b/src/main/java/frc/robot/commands/Autos/Centerline.java @@ -39,11 +39,11 @@ public class Centerline extends SequentialCommandGroup { BooleanSupplier readyToShoot = (() -> subDrivetrain.isDrivetrainFacingSpeaker() && subShooter.readyToShoot() && subStateMachine.isCurrentStateTargetState() - && subTransfer.getGamePieceCollected()); + && subTransfer.getGamePieceStored()); // TODO: Move this into its own command so we can use it everywhere :) SequentialCommandGroup shootSequence = new SequentialCommandGroup( - Commands.waitUntil(() -> subTransfer.getGamePieceCollected()), + Commands.waitUntil(() -> subTransfer.getGamePieceStored()), Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)), Commands.parallel( @@ -64,7 +64,7 @@ public class Centerline extends SequentialCommandGroup { .tryState(RobotState.SHOOTING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subTransfer, subShooter) - .until(() -> !subTransfer.getGamePieceCollected())), + .until(() -> !subTransfer.getGamePieceStored())), // Reset subsystems to chill Commands.deferredProxy(() -> subStateMachine @@ -89,7 +89,7 @@ public Centerline(StateMachine subStateMachine, Climber subClimber, Drivetrain s // -- PRELOAD -- Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subTransfer, subShooter)) - .until(() -> subTransfer.getGamePieceCollected()), + .until(() -> subTransfer.getGamePieceStored()), Commands.deferredProxy(() -> shootSequence), @@ -109,7 +109,7 @@ public Centerline(StateMachine subStateMachine, Climber subClimber, Drivetrain s // It wasnt there :< new PathPlannerAuto(determineHopPathName() + ".1"), - () -> subTransfer.getGamePieceCollected()), + () -> subTransfer.getGamePieceStored()), // -- CURRENTLY AT C4 -- Commands.waitSeconds(0.5), @@ -125,7 +125,7 @@ public Centerline(StateMachine subStateMachine, Climber subClimber, Drivetrain s // It wasnt there :< new PathPlannerAuto(determineHopPathName() + ".2"), - () -> subTransfer.getGamePieceCollected()), + () -> subTransfer.getGamePieceStored()), // -- CURRENTLY AT C3 Commands.waitSeconds(0.5), @@ -141,7 +141,7 @@ public Centerline(StateMachine subStateMachine, Climber subClimber, Drivetrain s // It wasnt there :< new PathPlannerAuto(determineHopPathName() + ".3"), - () -> subTransfer.getGamePieceCollected()), + () -> subTransfer.getGamePieceStored()), // -- CURRENTLY AT C2 Commands.waitSeconds(0.5), @@ -157,7 +157,7 @@ public Centerline(StateMachine subStateMachine, Climber subClimber, Drivetrain s // It wasnt there :< new PathPlannerAuto(determineHopPathName() + ".4"), - () -> subTransfer.getGamePieceCollected()), + () -> subTransfer.getGamePieceStored()), // -- CURRENTLY AT C1 Commands.waitSeconds(0.5), diff --git a/src/main/java/frc/robot/commands/Autos/PreloadOnly.java b/src/main/java/frc/robot/commands/Autos/PreloadOnly.java index 3f11939..635b4e3 100644 --- a/src/main/java/frc/robot/commands/Autos/PreloadOnly.java +++ b/src/main/java/frc/robot/commands/Autos/PreloadOnly.java @@ -76,7 +76,7 @@ public PreloadOnly(StateMachine subStateMachine, Climber subClimber, Drivetrain Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subTransfer, subShooter)) - .until(() -> subTransfer.getGamePieceCollected()), + .until(() -> subTransfer.getGamePieceStored()), Commands.waitUntil(() -> subShooter.readyToShoot()), @@ -85,7 +85,7 @@ public PreloadOnly(StateMachine subStateMachine, Climber subClimber, Drivetrain .tryState(RobotState.SHOOTING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subTransfer, subShooter) - .until(() -> !subTransfer.getGamePieceCollected())), + .until(() -> !subTransfer.getGamePieceStored())), // Reset subsystems to chill Commands.deferredProxy(() -> subStateMachine diff --git a/src/main/java/frc/robot/commands/Autos/PreloadTaxi.java b/src/main/java/frc/robot/commands/Autos/PreloadTaxi.java index bcc89fa..5acec83 100644 --- a/src/main/java/frc/robot/commands/Autos/PreloadTaxi.java +++ b/src/main/java/frc/robot/commands/Autos/PreloadTaxi.java @@ -43,10 +43,10 @@ public class PreloadTaxi extends SequentialCommandGroup { BooleanSupplier readyToShoot = (() -> subDrivetrain.isDrivetrainFacingSpeaker() && subShooter.readyToShoot() && subStateMachine.isCurrentStateTargetState() - && subTransfer.getGamePieceCollected()); + && subTransfer.getGamePieceStored()); SequentialCommandGroup shootSequence = new SequentialCommandGroup( - Commands.waitUntil(() -> subTransfer.getGamePieceCollected()), + Commands.waitUntil(() -> subTransfer.getGamePieceStored()), Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)), Commands.deferredProxy(() -> subStateMachine @@ -65,7 +65,7 @@ public class PreloadTaxi extends SequentialCommandGroup { .tryState(RobotState.SHOOTING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subTransfer, subShooter) - .until(() -> !subTransfer.getGamePieceCollected())), + .until(() -> !subTransfer.getGamePieceStored())), // Reset subsystems to chill Commands.deferredProxy(() -> subStateMachine @@ -95,7 +95,7 @@ public PreloadTaxi(StateMachine subStateMachine, Climber subClimber, Drivetrain // -- PRELOAD -- Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subTransfer, subShooter)) - .until(() -> subTransfer.getGamePieceCollected()), + .until(() -> subTransfer.getGamePieceStored()), Commands.deferredProxy(() -> shootSequence), diff --git a/src/main/java/frc/robot/commands/Autos/WingOnly.java b/src/main/java/frc/robot/commands/Autos/WingOnly.java index db24c53..58f9058 100644 --- a/src/main/java/frc/robot/commands/Autos/WingOnly.java +++ b/src/main/java/frc/robot/commands/Autos/WingOnly.java @@ -43,7 +43,7 @@ public class WingOnly extends SequentialCommandGroup { BooleanSupplier readyToShoot = (() -> subDrivetrain.isDrivetrainFacingSpeaker() && subShooter.readyToShoot() && subStateMachine.isCurrentStateTargetState() - && subTransfer.getGamePieceCollected()); + && subTransfer.getGamePieceStored()); // TODO: Move this into its own command so we can use it everywhere :) SequentialCommandGroup shootSequence = new SequentialCommandGroup( @@ -67,7 +67,7 @@ public class WingOnly extends SequentialCommandGroup { .tryState(RobotState.SHOOTING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subTransfer, subShooter) - .until(() -> !subTransfer.getGamePieceCollected())), + .until(() -> !subTransfer.getGamePieceStored())), // Reset subsystems to chill Commands.deferredProxy(() -> subStateMachine @@ -97,31 +97,31 @@ public WingOnly(StateMachine subStateMachine, Climber subClimber, Drivetrain sub // -- PRELOAD -- Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subTransfer, subShooter)) - .until(() -> subTransfer.getGamePieceCollected()).withTimeout(1), + .until(() -> subTransfer.getGamePieceStored()).withTimeout(1), - Commands.waitUntil(() -> subTransfer.getGamePieceCollected()).withTimeout(2), - Commands.deferredProxy(() -> shootSequence).unless(() -> !subTransfer.getGamePieceCollected()), + Commands.waitUntil(() -> subTransfer.getGamePieceStored()).withTimeout(2), + Commands.deferredProxy(() -> shootSequence).unless(() -> !subTransfer.getGamePieceStored()), // -- W1 / W3 -- // Drive to first note (Intaking is within the path) new PathPlannerAuto(determinePathName() + ".1"), - Commands.waitUntil(() -> subTransfer.getGamePieceCollected()).withTimeout(2), - Commands.deferredProxy(() -> shootSequence).unless(() -> !subTransfer.getGamePieceCollected()), + Commands.waitUntil(() -> subTransfer.getGamePieceStored()).withTimeout(2), + Commands.deferredProxy(() -> shootSequence).unless(() -> !subTransfer.getGamePieceStored()), // -- W2 -- // Drive to first note (Intaking is within the path) new PathPlannerAuto(determinePathName() + ".2"), - Commands.waitUntil(() -> subTransfer.getGamePieceCollected()).withTimeout(2), - Commands.deferredProxy(() -> shootSequence).unless(() -> !subTransfer.getGamePieceCollected()), + Commands.waitUntil(() -> subTransfer.getGamePieceStored()).withTimeout(2), + Commands.deferredProxy(() -> shootSequence).unless(() -> !subTransfer.getGamePieceStored()), // -- W3 / W1 -- // Drive to first note (Intaking is within the path) new PathPlannerAuto(determinePathName() + ".3"), - Commands.waitUntil(() -> subTransfer.getGamePieceCollected()).withTimeout(2), - Commands.deferredProxy(() -> shootSequence).unless(() -> !subTransfer.getGamePieceCollected()), + Commands.waitUntil(() -> subTransfer.getGamePieceStored()).withTimeout(2), + Commands.deferredProxy(() -> shootSequence).unless(() -> !subTransfer.getGamePieceStored()), // Reset subsystems to chill Commands.deferredProxy(() -> subStateMachine diff --git a/src/main/java/frc/robot/commands/States/Climbing.java b/src/main/java/frc/robot/commands/States/Climbing.java index 90dd38a..9bc769c 100644 --- a/src/main/java/frc/robot/commands/States/Climbing.java +++ b/src/main/java/frc/robot/commands/States/Climbing.java @@ -48,7 +48,7 @@ public Climbing(Climber subClimber, Elevator subElevator, StateMachine subStateM Commands.runOnce(() -> subElevator.setDrainpipeSpeed(0))), Commands.print("Climb was initiated, but no Game Piece was found!"), - () -> subTransfer.getGamePieceCollected()), + () -> subTransfer.getGamePieceStored()), // Move the elevator and shooter to a point where we can safely climb Commands.runOnce(() -> subElevator.setElevatorPosition(desiredShooterPosition.elevatorPosition)), diff --git a/src/main/java/frc/robot/commands/States/IntakeSource.java b/src/main/java/frc/robot/commands/States/IntakeSource.java index ec2aa9f..702d6c5 100644 --- a/src/main/java/frc/robot/commands/States/IntakeSource.java +++ b/src/main/java/frc/robot/commands/States/IntakeSource.java @@ -42,7 +42,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (subTransfer.getGamePieceCollected()) { + if (subTransfer.getGamePieceStored()) { hasGamePiece = true; } } @@ -61,7 +61,7 @@ public boolean isFinished() { // Since the sensor senses the back of the note, command will end when we know // we have a game piece but when the sensor doesn't sense the note anymore (it's // far back enough to not touch the flywheels) - return (hasGamePiece && !subTransfer.getGamePieceCollected()); + return (hasGamePiece && !subTransfer.getGamePieceStored()); } public boolean getIntakeSourceGamePiece() { diff --git a/src/main/java/frc/robot/commands/States/Intaking.java b/src/main/java/frc/robot/commands/States/Intaking.java index 11ab978..3c15b3d 100644 --- a/src/main/java/frc/robot/commands/States/Intaking.java +++ b/src/main/java/frc/robot/commands/States/Intaking.java @@ -61,6 +61,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return subTransfer.getGamePieceCollected(); + return subTransfer.getGamePieceStored(); } } diff --git a/src/main/java/frc/robot/commands/States/Shooting.java b/src/main/java/frc/robot/commands/States/Shooting.java index b285837..c79242f 100644 --- a/src/main/java/frc/robot/commands/States/Shooting.java +++ b/src/main/java/frc/robot/commands/States/Shooting.java @@ -59,7 +59,7 @@ public void execute() { @Override public void end(boolean interrupted) { // If we don't have a game piece anymore, set the target state back to NONE - if (!subTransfer.getGamePieceCollected()) { + if (!subTransfer.getGamePieceStored()) { subStateMachine.setTargetState(TargetState.PREP_NONE); subShooter.setShootingNeutralOutput(); subShooter.setPivotNeutralOutput(); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index d0accf7..9c2c722 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -7,6 +7,8 @@ import edu.wpi.first.units.Dimensionless; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.constIntake; import frc.robot.RobotMap.mapIntake; @@ -17,12 +19,14 @@ public class Intake extends SubsystemBase { TalonFX rollerMotor; + DigitalInput noteSensor; TalonFXConfiguration rollerConfig = new TalonFXConfiguration(); /** Creates a new Intake. */ public Intake() { rollerMotor = new TalonFX(mapIntake.ROLLER_CAN, "rio"); + noteSensor = new DigitalInput(mapIntake.NOTE_SENSOR_DIO); configure(); } @@ -49,10 +53,14 @@ public void setRollerNeutralOutput() { rollerMotor.setControl(new NeutralOut()); } + public boolean getGamePieceCollected() { + return (constIntake.NOTE_SENSOR_INVERT) ? !noteSensor.get() : noteSensor.get(); + } + @Override public void periodic() { // This method will be called once per scheduler run - + SmartDashboard.putBoolean("Game Piece Detected", getGamePieceCollected()); } } diff --git a/src/main/java/frc/robot/subsystems/Transfer.java b/src/main/java/frc/robot/subsystems/Transfer.java index 38b82d1..88dfdb7 100644 --- a/src/main/java/frc/robot/subsystems/Transfer.java +++ b/src/main/java/frc/robot/subsystems/Transfer.java @@ -50,7 +50,7 @@ public void setFeederNeutralOutput() { feederMotor.setControl(new NeutralOut()); } - public boolean getGamePieceCollected() { + public boolean getGamePieceStored() { boolean noteSensorValue = (constTransfer.NOTE_SENSOR_INVERT) ? !noteSensor.get() : noteSensor.get(); return (noteSensorValue || hasGamePiece); } @@ -62,6 +62,6 @@ public void setGamePieceCollected(boolean isCollected) { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putBoolean("Game Piece Detected", getGamePieceCollected()); + SmartDashboard.putBoolean("Game Piece Stored", getGamePieceStored()); } } From badf759ab3ca588949b196b045fac34f8c5fe59f Mon Sep 17 00:00:00 2001 From: ACat701 <90939494+ACat701@users.noreply.github.com> Date: Sat, 19 Oct 2024 15:23:03 -0700 Subject: [PATCH 2/7] Remove Climber References --- src/main/java/frc/robot/RobotContainer.java | 19 +------------- .../frc/robot/subsystems/StateMachine.java | 25 +++---------------- 2 files changed, 4 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4590a85..84cb00a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -130,21 +130,6 @@ private void configureDriverBindings(SN_XboxController controller) { controller.btn_North.onTrue( Commands.runOnce(() -> subDrivetrain.resetPoseToPose(constField.getFieldPositions().get()[6].toPose2d()))); - // Climb up - controller.btn_RightTrigger.onTrue( - Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_AMP))) - .onTrue(Commands.deferredProxy( - () -> subStateMachine.tryState(RobotState.CLIMBING, subStateMachine, - subClimber, subDrivetrain, subElevator, - subIntake, subTransfer, subShooter))) - .whileTrue(Commands.runOnce(() -> subClimber.setClimberSpeed(controller.getRightTriggerAxis()))) - .onFalse(Commands.runOnce(() -> subClimber.setClimberSpeed(0))); - - // Climb down - controller.btn_LeftTrigger - .whileTrue(Commands.runOnce(() -> subClimber.setClimberSpeed(-controller.getLeftTriggerAxis()))) - .onFalse(Commands.runOnce(() -> subClimber.setClimberSpeed(0))); - // Intake from source controller.btn_East.whileTrue(Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKE_SOURCE, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subTransfer, subShooter))) @@ -257,8 +242,7 @@ private void configureOperatorBindings(SN_XboxController controller) { controller.btn_Start.whileTrue(new ManualElevator(subElevator, controller.axis_LeftY)); // Zero Elevator and Climber - controller.btn_LeftStick.onTrue(new ZeroElevator(subElevator)) - .onTrue(new ZeroClimber(subClimber)); + controller.btn_LeftStick.onTrue(new ZeroElevator(subElevator)); // Zero Shooter controller.btn_RightStick.onTrue(new ZeroShooterPivot(subShooter)); @@ -331,7 +315,6 @@ public Command getAutonomousCommand() { */ public static Command zeroSubsystems() { Command returnedCommand = new ParallelCommandGroup( - new ZeroClimber(subClimber).withTimeout(constClimber.ZEROING_TIMEOUT.in(Units.Seconds)), new ZeroElevator(subElevator).withTimeout(constElevator.ZEROING_TIMEOUT.in(Units.Seconds)), new ZeroShooterPivot(subShooter).withTimeout(constShooter.ZEROING_TIMEOUT.in(Units.Seconds))) .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming); diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java index 9da526b..aee1548 100644 --- a/src/main/java/frc/robot/subsystems/StateMachine.java +++ b/src/main/java/frc/robot/subsystems/StateMachine.java @@ -115,22 +115,10 @@ public Command tryState(RobotState desiredState, StateMachine subStateMachine, C } break; + // No more climber :( case CLIMBING: - switch (currentState) { - case NONE: - case STORE_FEEDER: - case PREP_SPEAKER: - case PREP_VISION: - case PREP_AMP_SHOOTER: - case PREP_SPIKE: - case PREP_WING: - case PREP_AMP: - case PREP_NONE: - case PREP_SUB_BACKWARDS: - return new Climbing(subClimber, subElevator, subStateMachine, subShooter, subTransfer); - } - break; + case EJECTING: switch (currentState) { case NONE: @@ -319,15 +307,8 @@ public Command tryState(RobotState desiredState, StateMachine subStateMachine, C case PREP_NONE: case PREP_SUB_BACKWARDS: return new PrepTargetState(subElevator, subStateMachine, subShooter, TargetState.PREP_NONE); - case CLIMBING: - if (subClimber.isClimberAtPosition(constClimber.BACKWARD_LIMIT)) { - return new NoneState(subStateMachine, subClimber, subElevator, subIntake, subShooter, subTransfer); - } else { - return Commands - .print("Attempted to cancel CLIMBING, but the CLIMBER is UP! Please move the climber first :p"); - } - } + break; } return Commands.print("ITS SO OVER D: Invalid State Provided :3"); } From 9470f4433bb1207fd746cf1188333b221c96a401 Mon Sep 17 00:00:00 2001 From: ACat701 <90939494+ACat701@users.noreply.github.com> Date: Sat, 19 Oct 2024 17:05:53 -0700 Subject: [PATCH 3/7] Refactor Shooter PID and tune Right Flywheel --- src/main/java/frc/robot/Constants.java | 48 ++++++++++++++ src/main/java/frc/robot/RobotPreferences.java | 64 +++++++++++++++---- .../java/frc/robot/subsystems/Shooter.java | 31 ++++----- 3 files changed, 109 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c672638..564fd4d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -11,6 +11,8 @@ import java.util.Optional; import java.util.function.Supplier; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -250,6 +252,52 @@ public static class constShooter { public static final NeutralModeValue PIVOT_NEUTRAL_MODE = NeutralModeValue.Brake; public static final GravityTypeValue PIVOT_GRAVITY_TYPE = GravityTypeValue.Arm_Cosine; + // - PID - + public static Slot0Configs LEFT_PID_SLOT_0 = new Slot0Configs(); + public static Slot1Configs LEFT_PID_SLOT_1 = new Slot1Configs(); + public static Slot0Configs RIGHT_PID_SLOT_0 = new Slot0Configs(); + public static Slot1Configs RIGHT_PID_SLOT_1 = new Slot1Configs(); + public static Slot0Configs PIVOT_PID = new Slot0Configs(); + static { + // Left 0 + LEFT_PID_SLOT_0.kS = 0.28; + LEFT_PID_SLOT_0.kV = 0.13; + LEFT_PID_SLOT_0.kA = 0; + LEFT_PID_SLOT_0.kP = 0.5; + LEFT_PID_SLOT_0.kI = 0; + LEFT_PID_SLOT_0.kD = 0; + + // Left 1 + LEFT_PID_SLOT_1.kS = 0.28; + LEFT_PID_SLOT_1.kV = 0.13; + LEFT_PID_SLOT_1.kA = 0; + LEFT_PID_SLOT_1.kP = 0.5; + LEFT_PID_SLOT_1.kI = 0; + LEFT_PID_SLOT_1.kD = 0; + + // Right 0 -- Tuned! + RIGHT_PID_SLOT_0.kS = 0.36; + RIGHT_PID_SLOT_0.kV = 0.115; + RIGHT_PID_SLOT_0.kA = 0; + RIGHT_PID_SLOT_0.kP = 0.7; + RIGHT_PID_SLOT_0.kI = 0; + RIGHT_PID_SLOT_0.kD = 0; + // Right 1 + RIGHT_PID_SLOT_1.kS = 0.36; + RIGHT_PID_SLOT_1.kV = 0.115; + RIGHT_PID_SLOT_1.kA = 0; + RIGHT_PID_SLOT_1.kP = 0.7; + RIGHT_PID_SLOT_1.kI = 0; + RIGHT_PID_SLOT_1.kD = 0; + // Pivot + PIVOT_PID.kS = 0.4; + PIVOT_PID.kV = 0; + PIVOT_PID.kG = 0.53; + PIVOT_PID.kA = 0; + PIVOT_PID.kP = 90; + PIVOT_PID.kD = 0; + } + // - Angles - public static final Measure PIVOT_FORWARD_LIMIT = Units.Degrees.of(155); public static final Measure PIVOT_BACKWARD_LIMIT = Units.Degrees.of(0); diff --git a/src/main/java/frc/robot/RobotPreferences.java b/src/main/java/frc/robot/RobotPreferences.java index 45e4056..4fb0a22 100644 --- a/src/main/java/frc/robot/RobotPreferences.java +++ b/src/main/java/frc/robot/RobotPreferences.java @@ -1,5 +1,7 @@ package frc.robot; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; import com.frcteam3255.preferences.SN_DoublePreference; import edu.wpi.first.units.Angle; @@ -81,6 +83,7 @@ public static final class prefDrivetrain { public static final class prefShooter { // - PID - + public static final double leftShooterS = 0.28; public static final double leftShooterV = 0.13; public static final double leftShooterA = 0.0; @@ -88,20 +91,53 @@ public static final class prefShooter { public static final double leftShooterI = 0; public static final double leftShooterD = 0.0; - public static final double rightShooterS = 0.4; - public static final double rightShooterV = 0.11; - public static final double rightShooterA = 0; - public static final double rightShooterP = 0.4; // 0.6 ? - public static final double rightShooterI = 0; - public static final double rightShooterD = 0; - - public static final double pivotShooterS = 0.4; - public static final double pivotShooterV = 0.0; - public static final double pivotShooterG = 0.53; - public static final double pivotShooterA = 0.0; - public static final double pivotShooterP = 90; - public static final double pivotShooterI = 0; - public static final double pivotShooterD = 0; + public static Slot0Configs leftPIDSlot0 = new Slot0Configs(); + public static Slot1Configs leftPIDSlot1 = new Slot1Configs(); + + public static Slot0Configs rightPIDSlot0 = new Slot0Configs(); + public static Slot1Configs rightPIDSlot1 = new Slot1Configs(); + + public static Slot0Configs pivotPID = new Slot0Configs(); + + static { + // Left 0 + leftPIDSlot0.kS = 0.28; + leftPIDSlot0.kV = 0.13; + leftPIDSlot0.kA = 0; + leftPIDSlot0.kP = 0.5; + leftPIDSlot0.kI = 0; + leftPIDSlot0.kD = 0; + + // Left 1 + leftPIDSlot1.kS = 0.28; + leftPIDSlot1.kV = 0.13; + leftPIDSlot1.kA = 0; + leftPIDSlot1.kP = 0.5; + leftPIDSlot1.kI = 0; + leftPIDSlot1.kD = 0; + + // Right 0 -- Tuned! + rightPIDSlot0.kS = 0.36; + rightPIDSlot0.kV = 0.115; + rightPIDSlot0.kA = 0; + rightPIDSlot0.kP = 0.7; + rightPIDSlot0.kI = 0; + rightPIDSlot0.kD = 0; + // Right 1 + rightPIDSlot1.kS = 0.36; + rightPIDSlot1.kV = 0.115; + rightPIDSlot1.kA = 0; + rightPIDSlot1.kP = 0.7; + rightPIDSlot1.kI = 0; + rightPIDSlot1.kD = 0; + // Pivot + pivotPID.kS = 0.4; + pivotPID.kV = 0; + pivotPID.kG = 0.53; + pivotPID.kA = 0; + pivotPID.kP = 90; + pivotPID.kD = 0; + } } public static final class prefElevator { diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 335b5de..0262ac7 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -41,6 +41,9 @@ public class Shooter extends SubsystemBase { private Measure> desiredRightVelocity = Units.RotationsPerSecond.of(0); private Measure lastDesiredPivotAngle = Units.Degrees.of(-3255); + int currentRightSlot = 0; + int currentLeftSlot = 0; + public Shooter() { leftMotor = new TalonFX(mapShooter.SHOOTER_LEFT_MOTOR_CAN, "rio"); rightMotor = new TalonFX(mapShooter.SHOOTER_RIGHT_MOTOR_CAN, "rio"); @@ -61,12 +64,8 @@ public Shooter() { public void configure() { // -- Left Motor -- leftConfig.MotorOutput.Inverted = constShooter.LEFT_INVERT; - leftConfig.Slot0.kV = prefShooter.leftShooterV; - leftConfig.Slot0.kS = prefShooter.leftShooterS; - leftConfig.Slot0.kA = prefShooter.leftShooterA; - leftConfig.Slot0.kP = prefShooter.leftShooterP; - leftConfig.Slot0.kI = prefShooter.leftShooterI; - leftConfig.Slot0.kD = prefShooter.leftShooterD; + leftConfig.Slot0 = constShooter.LEFT_PID_SLOT_0; + leftConfig.Slot1 = constShooter.LEFT_PID_SLOT_1; leftConfig.MotionMagic.MotionMagicAcceleration = 400; leftConfig.MotionMagic.MotionMagicJerk = 4000; @@ -74,12 +73,8 @@ public void configure() { // -- Right Motor -- rightConfig.MotorOutput.Inverted = constShooter.RIGHT_INVERT; - rightConfig.Slot0.kV = prefShooter.rightShooterV; - rightConfig.Slot0.kS = prefShooter.rightShooterS; - rightConfig.Slot0.kA = prefShooter.rightShooterA; - rightConfig.Slot0.kP = prefShooter.rightShooterP; - rightConfig.Slot0.kI = prefShooter.rightShooterI; - rightConfig.Slot0.kD = prefShooter.rightShooterD; + rightConfig.Slot0 = constShooter.RIGHT_PID_SLOT_0; + rightConfig.Slot1 = constShooter.RIGHT_PID_SLOT_1; rightConfig.MotionMagic.MotionMagicAcceleration = 400; rightConfig.MotionMagic.MotionMagicJerk = 4000; @@ -89,14 +84,8 @@ public void configure() { pivotConfig.Feedback.SensorToMechanismRatio = constShooter.PIVOT_GEAR_RATIO; pivotConfig.MotorOutput.Inverted = constShooter.PIVOT_INVERT; pivotConfig.MotorOutput.NeutralMode = constShooter.PIVOT_NEUTRAL_MODE; - pivotConfig.Slot0.kS = prefShooter.pivotShooterS; - pivotConfig.Slot0.kV = prefShooter.pivotShooterV; - pivotConfig.Slot0.kG = prefShooter.pivotShooterG; - pivotConfig.Slot0.kA = prefShooter.pivotShooterA; - pivotConfig.Slot0.kP = prefShooter.pivotShooterP; - pivotConfig.Slot0.kI = prefShooter.pivotShooterI; - pivotConfig.Slot0.kD = prefShooter.pivotShooterD; pivotConfig.Slot0.GravityType = constShooter.PIVOT_GRAVITY_TYPE; + pivotConfig.Slot0 = constShooter.PIVOT_PID; pivotConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; pivotConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = constShooter.PIVOT_FORWARD_LIMIT.in(Units.Rotations); @@ -128,8 +117,10 @@ public void getUpToSpeed() { && desiredRightVelocity.in(Units.RotationsPerSecond) == 0) { setShootingNeutralOutput(); } else { + leftMotor.setControl(motionMagicRequest.withVelocity(desiredLeftVelocity.in(Units.RotationsPerSecond))); - rightMotor.setControl(motionMagicRequest.withVelocity(desiredRightVelocity.in(Units.RotationsPerSecond))); + rightMotor + .setControl(motionMagicRequest.withVelocity(desiredRightVelocity.in(Units.RotationsPerSecond)).withSlot(0)); } } From 5e8deaacb533220f82b0af7537ba534b743af6f0 Mon Sep 17 00:00:00 2001 From: ACat701 <90939494+ACat701@users.noreply.github.com> Date: Sat, 19 Oct 2024 18:26:23 -0700 Subject: [PATCH 4/7] tuned slow right PID --- src/main/java/frc/robot/Constants.java | 62 ++++++++++--------- src/main/java/frc/robot/RobotPreferences.java | 59 ------------------ .../java/frc/robot/subsystems/Shooter.java | 29 +++++---- 3 files changed, 50 insertions(+), 100 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 564fd4d..5b897b4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -253,42 +253,42 @@ public static class constShooter { public static final GravityTypeValue PIVOT_GRAVITY_TYPE = GravityTypeValue.Arm_Cosine; // - PID - - public static Slot0Configs LEFT_PID_SLOT_0 = new Slot0Configs(); - public static Slot1Configs LEFT_PID_SLOT_1 = new Slot1Configs(); - public static Slot0Configs RIGHT_PID_SLOT_0 = new Slot0Configs(); - public static Slot1Configs RIGHT_PID_SLOT_1 = new Slot1Configs(); + public static Slot0Configs LEFT_PID_SLOT_0_FAST = new Slot0Configs(); + public static Slot1Configs LEFT_PID_SLOT_1_SLOW = new Slot1Configs(); + public static Slot0Configs RIGHT_PID_SLOT_0_FAST = new Slot0Configs(); + public static Slot1Configs RIGHT_PID_SLOT_1_SLOW = new Slot1Configs(); public static Slot0Configs PIVOT_PID = new Slot0Configs(); static { // Left 0 - LEFT_PID_SLOT_0.kS = 0.28; - LEFT_PID_SLOT_0.kV = 0.13; - LEFT_PID_SLOT_0.kA = 0; - LEFT_PID_SLOT_0.kP = 0.5; - LEFT_PID_SLOT_0.kI = 0; - LEFT_PID_SLOT_0.kD = 0; + LEFT_PID_SLOT_0_FAST.kS = 0.28; + LEFT_PID_SLOT_0_FAST.kV = 0.13; + LEFT_PID_SLOT_0_FAST.kA = 0; + LEFT_PID_SLOT_0_FAST.kP = 0.7; + LEFT_PID_SLOT_0_FAST.kI = 0; + LEFT_PID_SLOT_0_FAST.kD = 0; // Left 1 - LEFT_PID_SLOT_1.kS = 0.28; - LEFT_PID_SLOT_1.kV = 0.13; - LEFT_PID_SLOT_1.kA = 0; - LEFT_PID_SLOT_1.kP = 0.5; - LEFT_PID_SLOT_1.kI = 0; - LEFT_PID_SLOT_1.kD = 0; - - // Right 0 -- Tuned! - RIGHT_PID_SLOT_0.kS = 0.36; - RIGHT_PID_SLOT_0.kV = 0.115; - RIGHT_PID_SLOT_0.kA = 0; - RIGHT_PID_SLOT_0.kP = 0.7; - RIGHT_PID_SLOT_0.kI = 0; - RIGHT_PID_SLOT_0.kD = 0; + LEFT_PID_SLOT_1_SLOW.kS = 0.28; + LEFT_PID_SLOT_1_SLOW.kV = 0.13; + LEFT_PID_SLOT_1_SLOW.kA = 0; + LEFT_PID_SLOT_1_SLOW.kP = 0.5; + LEFT_PID_SLOT_1_SLOW.kI = 0; + LEFT_PID_SLOT_1_SLOW.kD = 0; + + // Right 0 + RIGHT_PID_SLOT_0_FAST.kS = 0.36; + RIGHT_PID_SLOT_0_FAST.kV = 0.115; + RIGHT_PID_SLOT_0_FAST.kA = 0; + RIGHT_PID_SLOT_0_FAST.kP = 0.7; + RIGHT_PID_SLOT_0_FAST.kI = 0; + RIGHT_PID_SLOT_0_FAST.kD = 0; // Right 1 - RIGHT_PID_SLOT_1.kS = 0.36; - RIGHT_PID_SLOT_1.kV = 0.115; - RIGHT_PID_SLOT_1.kA = 0; - RIGHT_PID_SLOT_1.kP = 0.7; - RIGHT_PID_SLOT_1.kI = 0; - RIGHT_PID_SLOT_1.kD = 0; + RIGHT_PID_SLOT_1_SLOW.kS = 0.36; + RIGHT_PID_SLOT_1_SLOW.kV = 0.112; + RIGHT_PID_SLOT_1_SLOW.kA = 0; + RIGHT_PID_SLOT_1_SLOW.kP = 0.5; + RIGHT_PID_SLOT_1_SLOW.kI = 0; + RIGHT_PID_SLOT_1_SLOW.kD = 0; // Pivot PIVOT_PID.kS = 0.4; PIVOT_PID.kV = 0; @@ -297,6 +297,8 @@ public static class constShooter { PIVOT_PID.kP = 90; PIVOT_PID.kD = 0; } + public static final Measure> RIGHT_SLOT_1_THRESH = Units.RotationsPerSecond.of(30); + public static final Measure> LEFT_SLOT_1_THRESH = Units.RotationsPerSecond.of(30); // - Angles - public static final Measure PIVOT_FORWARD_LIMIT = Units.Degrees.of(155); diff --git a/src/main/java/frc/robot/RobotPreferences.java b/src/main/java/frc/robot/RobotPreferences.java index 4fb0a22..ee622c9 100644 --- a/src/main/java/frc/robot/RobotPreferences.java +++ b/src/main/java/frc/robot/RobotPreferences.java @@ -81,65 +81,6 @@ public static final class prefDrivetrain { public static final double yawSnapD = 0; } - public static final class prefShooter { - // - PID - - - public static final double leftShooterS = 0.28; - public static final double leftShooterV = 0.13; - public static final double leftShooterA = 0.0; - public static final double leftShooterP = 0.5; // 0.75 ? - public static final double leftShooterI = 0; - public static final double leftShooterD = 0.0; - - public static Slot0Configs leftPIDSlot0 = new Slot0Configs(); - public static Slot1Configs leftPIDSlot1 = new Slot1Configs(); - - public static Slot0Configs rightPIDSlot0 = new Slot0Configs(); - public static Slot1Configs rightPIDSlot1 = new Slot1Configs(); - - public static Slot0Configs pivotPID = new Slot0Configs(); - - static { - // Left 0 - leftPIDSlot0.kS = 0.28; - leftPIDSlot0.kV = 0.13; - leftPIDSlot0.kA = 0; - leftPIDSlot0.kP = 0.5; - leftPIDSlot0.kI = 0; - leftPIDSlot0.kD = 0; - - // Left 1 - leftPIDSlot1.kS = 0.28; - leftPIDSlot1.kV = 0.13; - leftPIDSlot1.kA = 0; - leftPIDSlot1.kP = 0.5; - leftPIDSlot1.kI = 0; - leftPIDSlot1.kD = 0; - - // Right 0 -- Tuned! - rightPIDSlot0.kS = 0.36; - rightPIDSlot0.kV = 0.115; - rightPIDSlot0.kA = 0; - rightPIDSlot0.kP = 0.7; - rightPIDSlot0.kI = 0; - rightPIDSlot0.kD = 0; - // Right 1 - rightPIDSlot1.kS = 0.36; - rightPIDSlot1.kV = 0.115; - rightPIDSlot1.kA = 0; - rightPIDSlot1.kP = 0.7; - rightPIDSlot1.kI = 0; - rightPIDSlot1.kD = 0; - // Pivot - pivotPID.kS = 0.4; - pivotPID.kV = 0; - pivotPID.kG = 0.53; - pivotPID.kA = 0; - pivotPID.kP = 90; - pivotPID.kD = 0; - } - } - public static final class prefElevator { public static final double elevatorG = 0.3; public static final double elevatorS = 0.4; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 0262ac7..891e08c 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -25,7 +25,6 @@ import frc.robot.Constants.constShooter; import frc.robot.Constants.constShooter.ShooterPositionGroup; import frc.robot.RobotMap.mapShooter; -import frc.robot.RobotPreferences.prefShooter; public class Shooter extends SubsystemBase { TalonFX leftMotor, rightMotor, pivotMotor; @@ -64,20 +63,22 @@ public Shooter() { public void configure() { // -- Left Motor -- leftConfig.MotorOutput.Inverted = constShooter.LEFT_INVERT; - leftConfig.Slot0 = constShooter.LEFT_PID_SLOT_0; - leftConfig.Slot1 = constShooter.LEFT_PID_SLOT_1; + leftConfig.Slot0 = constShooter.LEFT_PID_SLOT_0_FAST; + leftConfig.Slot1 = constShooter.LEFT_PID_SLOT_1_SLOW; - leftConfig.MotionMagic.MotionMagicAcceleration = 400; - leftConfig.MotionMagic.MotionMagicJerk = 4000; + leftConfig.MotionMagic.MotionMagicCruiseVelocity = 60; + leftConfig.MotionMagic.MotionMagicAcceleration = 600; + leftConfig.MotionMagic.MotionMagicJerk = 6000; leftMotor.getConfigurator().apply(leftConfig); // -- Right Motor -- rightConfig.MotorOutput.Inverted = constShooter.RIGHT_INVERT; - rightConfig.Slot0 = constShooter.RIGHT_PID_SLOT_0; - rightConfig.Slot1 = constShooter.RIGHT_PID_SLOT_1; + rightConfig.Slot0 = constShooter.RIGHT_PID_SLOT_0_FAST; + rightConfig.Slot1 = constShooter.RIGHT_PID_SLOT_1_SLOW; - rightConfig.MotionMagic.MotionMagicAcceleration = 400; - rightConfig.MotionMagic.MotionMagicJerk = 4000; + rightConfig.MotionMagic.MotionMagicCruiseVelocity = 60; + rightConfig.MotionMagic.MotionMagicAcceleration = 600; + rightConfig.MotionMagic.MotionMagicJerk = 6000; rightMotor.getConfigurator().apply(rightConfig); // -- Pivot Motor -- @@ -117,10 +118,14 @@ public void getUpToSpeed() { && desiredRightVelocity.in(Units.RotationsPerSecond) == 0) { setShootingNeutralOutput(); } else { + currentLeftSlot = (desiredLeftVelocity.lte(constShooter.LEFT_SLOT_1_THRESH)) ? 1 : 0; + currentRightSlot = (desiredRightVelocity.lte(constShooter.RIGHT_SLOT_1_THRESH)) ? 1 : 0; - leftMotor.setControl(motionMagicRequest.withVelocity(desiredLeftVelocity.in(Units.RotationsPerSecond))); + leftMotor.setControl( + motionMagicRequest.withVelocity(desiredLeftVelocity.in(Units.RotationsPerSecond)).withSlot(currentLeftSlot)); rightMotor - .setControl(motionMagicRequest.withVelocity(desiredRightVelocity.in(Units.RotationsPerSecond)).withSlot(0)); + .setControl(motionMagicRequest.withVelocity(desiredRightVelocity.in(Units.RotationsPerSecond)) + .withSlot(currentRightSlot)); } } @@ -316,10 +321,12 @@ public void periodic() { SmartDashboard.putNumber("Shooter/Left/Velocity RPS", getLeftShooterVelocity().in(Units.RotationsPerSecond)); SmartDashboard.putNumber("Shooter/Left/Desired Velocity RPS", desiredLeftVelocity.in(Units.RotationsPerSecond)); SmartDashboard.putBoolean("Shooter/Left/Up to Speed", isLeftShooterUpToSpeed()); + SmartDashboard.putNumber("Shooter/Left/PID Slot", currentLeftSlot); SmartDashboard.putNumber("Shooter/Right/Velocity RPS", getRightShooterVelocity().in(Units.RotationsPerSecond)); SmartDashboard.putNumber("Shooter/Right/Desired Velocity RPS", desiredRightVelocity.in(Units.RotationsPerSecond)); SmartDashboard.putBoolean("Shooter/Right/Up to Speed", isRightShooterUpToSpeed()); + SmartDashboard.putNumber("Shooter/Right/PID Slot", currentRightSlot); SmartDashboard.putNumber("Shooter/Pivot", getShooterPosition().in(Units.Degrees)); SmartDashboard.putNumber("Shooter/Pivot Velocity", pivotMotor.getVelocity().getValueAsDouble()); From 5aefd06845dc24bacfb3a0f58018dc22499d83ce Mon Sep 17 00:00:00 2001 From: ACat701 <90939494+ACat701@users.noreply.github.com> Date: Sat, 19 Oct 2024 19:47:52 -0700 Subject: [PATCH 5/7] very scrumptous flywheel values from the hoot files --- src/main/java/frc/robot/Constants.java | 18 ++++----- src/main/java/frc/robot/RobotContainer.java | 39 ++++++++----------- .../java/frc/robot/subsystems/Shooter.java | 27 +++++++++++++ 3 files changed, 52 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5b897b4..15e8ced 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -260,17 +260,17 @@ public static class constShooter { public static Slot0Configs PIVOT_PID = new Slot0Configs(); static { // Left 0 - LEFT_PID_SLOT_0_FAST.kS = 0.28; - LEFT_PID_SLOT_0_FAST.kV = 0.13; - LEFT_PID_SLOT_0_FAST.kA = 0; - LEFT_PID_SLOT_0_FAST.kP = 0.7; + LEFT_PID_SLOT_0_FAST.kS = 0.13419; + LEFT_PID_SLOT_0_FAST.kV = 0.12443; + LEFT_PID_SLOT_0_FAST.kA = 0.035067; + LEFT_PID_SLOT_0_FAST.kP = 1; LEFT_PID_SLOT_0_FAST.kI = 0; LEFT_PID_SLOT_0_FAST.kD = 0; // Left 1 - LEFT_PID_SLOT_1_SLOW.kS = 0.28; - LEFT_PID_SLOT_1_SLOW.kV = 0.13; - LEFT_PID_SLOT_1_SLOW.kA = 0; + LEFT_PID_SLOT_1_SLOW.kS = 0.13419; + LEFT_PID_SLOT_1_SLOW.kV = 0.12443; + LEFT_PID_SLOT_1_SLOW.kA = 0.035067; LEFT_PID_SLOT_1_SLOW.kP = 0.5; LEFT_PID_SLOT_1_SLOW.kI = 0; LEFT_PID_SLOT_1_SLOW.kD = 0; @@ -297,8 +297,8 @@ public static class constShooter { PIVOT_PID.kP = 90; PIVOT_PID.kD = 0; } - public static final Measure> RIGHT_SLOT_1_THRESH = Units.RotationsPerSecond.of(30); - public static final Measure> LEFT_SLOT_1_THRESH = Units.RotationsPerSecond.of(30); + public static final Measure> RIGHT_SLOT_1_THRESH = Units.RotationsPerSecond.of(40); + public static final Measure> LEFT_SLOT_1_THRESH = Units.RotationsPerSecond.of(40); // - Angles - public static final Measure PIVOT_FORWARD_LIMIT = Units.Degrees.of(155); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 84cb00a..e3208b9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import java.util.function.DoubleSupplier; +import com.ctre.phoenix6.SignalLogger; import com.frcteam3255.joystick.SN_XboxController; import com.pathplanner.lib.auto.NamedCommands; @@ -19,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.constClimber; import frc.robot.Constants.constControllers; import frc.robot.Constants.constElevator; @@ -249,29 +251,20 @@ private void configureOperatorBindings(SN_XboxController controller) { } private void configureTestBindings(SN_XboxController controller) { - controller.btn_LeftTrigger.onTrue(Commands.runOnce(() -> subStateMachine.setRobotState(RobotState.INTAKING))) - .whileTrue(new Intaking(subStateMachine, subIntake, subShooter, subTransfer)) - .onFalse(new NoneState(subStateMachine, subClimber, subElevator, subIntake, - subShooter, subTransfer)); - - controller.btn_RightTrigger.onTrue(Commands.runOnce(() -> subStateMachine.setRobotState(RobotState.SHOOTING))) - .whileTrue(new Shooting(subStateMachine, subElevator, subShooter, - subTransfer)) - .onFalse(new NoneState(subStateMachine, subClimber, subElevator, subIntake, - subShooter, subTransfer)); - - controller.btn_X.onTrue(Commands.runOnce(() -> subStateMachine.setRobotState(RobotState.PREP_SHUFFLE))) - .onTrue(new PrepTargetState(subElevator, subStateMachine, subShooter, - TargetState.PREP_SHUFFLE)); - - controller.btn_A.onTrue(Commands.runOnce(() -> subElevator.setElevatorPosition(constElevator.AMP_POSITION))); - controller.btn_Y.onTrue(Commands.runOnce(() -> subElevator.setElevatorPosition(constElevator.BACKWARD_LIMIT))); - - controller.btn_West.onTrue(Commands.runOnce(() -> subStateMachine.setRobotState(RobotState.EJECTING))) - .whileTrue(new Ejecting(subStateMachine, subIntake, subElevator, - subTransfer)) - .onFalse(new NoneState(subStateMachine, subClimber, subElevator, subIntake, - subShooter, subTransfer)); + + controller.btn_LeftBumper.onTrue(Commands.runOnce(SignalLogger::start)); + controller.btn_RightBumper.onTrue(Commands.runOnce(SignalLogger::stop)); + + /* + * Joystick Y = quasistatic forward + * Joystick A = quasistatic reverse + * Joystick B = dynamic forward + * Joystick X = dyanmic reverse + */ + controller.btn_Y.whileTrue(subShooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + controller.btn_A.whileTrue(subShooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + controller.btn_B.whileTrue(subShooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); + controller.btn_X.whileTrue(subShooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)); } private void configureAutoSelector() { diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 891e08c..138b592 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -4,6 +4,7 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.NeutralOut; @@ -11,6 +12,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.SysIdSwerveRotation; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -21,7 +23,12 @@ import edu.wpi.first.units.Velocity; import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.Constants.constShooter; import frc.robot.Constants.constShooter.ShooterPositionGroup; import frc.robot.RobotMap.mapShooter; @@ -315,6 +322,26 @@ public boolean readyToShoot() { return isLeftShooterUpToSpeed() && isRightShooterUpToSpeed() && isShooterAtPosition(lastDesiredPivotAngle); } + final SysIdRoutine m_sysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Units.Volts.of(7), // Reduce dynamic step voltage to 4 to prevent brownout + null, // Use default timeout (10 s) + // Log state with Phoenix SignalLogger class + (state) -> SignalLogger.writeString("state", state.toString())), + new SysIdRoutine.Mechanism( + (volts) -> leftMotor.setControl(voltageRequest.withOutput(volts.in(Units.Volts))), + null, + this)); + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.dynamic(direction); + } + @Override public void periodic() { // This method will be called once per scheduler run From c7ccda2f9c1a5097cb662002d242ba79607dcdba Mon Sep 17 00:00:00 2001 From: ACat701 <90939494+ACat701@users.noreply.github.com> Date: Sat, 19 Oct 2024 20:23:24 -0700 Subject: [PATCH 6/7] Sped up intake --- src/main/java/frc/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 15e8ced..14ee81e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -592,9 +592,9 @@ public static class constTransfer { public static final InvertedValue MOTOR_INVERT = InvertedValue.Clockwise_Positive; public static final NeutralModeValue FEEDER_NEUTRAL_MODE = NeutralModeValue.Brake; - public static final double INTAKING_SPEED = 0.3; + public static final double INTAKING_SPEED = 0.4; public static final double INTAKE_SOURCE_SPEED = -0.2; - public static final double EJECTING_SPEED = -0.3; + public static final double EJECTING_SPEED = -0.4; public static final double PREP_TO_AMP_SPEED = 0.2; From 0891f8d9e90ec5332de843a77a48e10d6c236b47 Mon Sep 17 00:00:00 2001 From: ACat701 <90939494+ACat701@users.noreply.github.com> Date: Sat, 19 Oct 2024 21:06:28 -0700 Subject: [PATCH 7/7] tune something i forgot --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/subsystems/Elevator.java | 1 + src/main/java/frc/robot/subsystems/Shooter.java | 9 ++++++++- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 14ee81e..58e6324 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -523,7 +523,7 @@ public static class constElevator { public static final double MOTOR_ROTATION_TO_METERS = 1 / 0.0456842368; public static final Measure AMP_POSITION = Units.Meters.of(0.38); - public static final Measure SHOOTER_ABLE_TO_MOVE_LIMIT = Units.Meters.of(0.37); + public static final Measure SHOOTER_ABLE_TO_MOVE_LIMIT = Units.Meters.of(0.35); public static final Measure AT_POSITION_TOLERANCE = Units.Meters.of(0.05); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 89ad30e..c35ecbc 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -143,6 +143,7 @@ public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("Elevator/Position", getElevatorPosition().in(Units.Meters)); + SmartDashboard.putBoolean("Elevator/Safe To Move Shooter", isSafeToMoveShooterAboveLimit()); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 138b592..ac2d50f 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -39,6 +40,7 @@ public class Shooter extends SubsystemBase { MotionMagicVelocityVoltage motionMagicRequest; PositionVoltage positionRequest; + MotionMagicVoltage motionMagicPivotRequest; VelocityVoltage velocityRequest; VoltageOut voltageRequest; @@ -62,6 +64,7 @@ public Shooter() { voltageRequest = new VoltageOut(0); velocityRequest = new VelocityVoltage(0).withSlot(0); motionMagicRequest = new MotionMagicVelocityVoltage(0); + motionMagicPivotRequest = new MotionMagicVoltage(0); positionRequest = new PositionVoltage(0).withSlot(0); configure(); @@ -101,6 +104,10 @@ public void configure() { pivotConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; pivotConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = constShooter.PIVOT_BACKWARD_LIMIT.in(Units.Rotations); + pivotConfig.MotionMagic.MotionMagicCruiseVelocity = 80; + pivotConfig.MotionMagic.MotionMagicAcceleration = 160; + pivotConfig.MotionMagic.MotionMagicJerk = 1600; + // - Current Limits - pivotConfig.CurrentLimits.SupplyCurrentLimitEnable = constShooter.PIVOT_ENABLE_CURRENT_LIMITING; pivotConfig.CurrentLimits.SupplyCurrentThreshold = constShooter.PIVOT_CURRENT_THRESH; @@ -280,7 +287,7 @@ public void setIgnoreFlywheelSpeed(boolean ignoreFlywheelSpeed) { public void setPivotPosition(Measure position) { lastDesiredPivotAngle = position; - pivotMotor.setControl(positionRequest.withPosition(position.in(Units.Rotations))); + pivotMotor.setControl(motionMagicPivotRequest.withPosition(position.in(Units.Rotations))); } /**