diff --git a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java index 35dc424..9891daf 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java @@ -74,8 +74,6 @@ public Elevator(ElevatorIO elevator) { extenderProfile = new TrapezoidProfile(extenderConstraints); extenderCurrent = extenderProfile.calculate(0, extenderCurrent, extenderGoal); - extenderkP.initDefault(15); - this.elevator.configurePID(extenderkP.get(), 0, 0); } @@ -135,7 +133,5 @@ public void periodic() { if (extenderkP.hasChanged(hashCode())) { elevator.configurePID(extenderkP.get(), 0, 0); } - - } } diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java index cde8a08..7c510ab 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; +import frc.robot.util.LoggedTunableNumber; import static edu.wpi.first.units.Units.Volts; @@ -17,14 +18,9 @@ public class Shooter extends SubsystemBase { /** Creates a new Shooter. */ private final FlywheelIO flywheels; - - // private final FlywheelIO shooterMotor2; - private final FeederIO feeder; private final FlywheelIOInputsAutoLogged fInputs = new FlywheelIOInputsAutoLogged(); - // private final FlywheelIOInputsAutoLogged s2Inputs = new FlywheelIOInputsAutoLogged(); - private final FeederIOInputsAutoLogged feedInputs = new FeederIOInputsAutoLogged(); private final SimpleMotorFeedforward flywheelFFModel; @@ -33,11 +29,17 @@ public class Shooter extends SubsystemBase { private final SysIdRoutine feedSysId; private final SysIdRoutine flywheelSysId; + private static final LoggedTunableNumber feederkP = new LoggedTunableNumber("feederkP"); + private static final LoggedTunableNumber flywheelkP = new LoggedTunableNumber("flywheelkP"); + public Shooter(FlywheelIO flywheels, FeederIO feeder) { switch (Constants.currentMode) { case REAL: flywheelFFModel = new SimpleMotorFeedforward(0, 3); feederFFModel = new SimpleMotorFeedforward(0, 0.3); + + flywheelkP.initDefault(0); + feederkP.initDefault(0); break; case REPLAY: flywheelFFModel = new SimpleMotorFeedforward(0, 0.03); @@ -54,7 +56,7 @@ public Shooter(FlywheelIO flywheels, FeederIO feeder) { } this.flywheels = flywheels; // TODO:: Make these constants - flywheels.configurePID(0.5, 0, 0); + flywheels.configurePID(flywheelkP.get(), 0, 0); // Configure SysId flywheelSysId = @@ -74,7 +76,7 @@ public Shooter(FlywheelIO flywheels, FeederIO feeder) { this.feeder = feeder; // TODO:: Make these constants - feeder.configurePID(0.5, 0, 0); + feeder.configurePID(feederkP.get(), 0, 0); // Configure SysId feedSysId = @@ -137,12 +139,12 @@ public Command feedSysIDDynamiCommand(SysIdRoutine.Direction direction) { /** Returns a command to run a quasistatic test in the specified direction. */ public Command flywheelSysIDQuasistic(SysIdRoutine.Direction direction) { - return feedSysId.quasistatic(direction); + return flywheelSysId.quasistatic(direction); } /** Returns a command to run a dynamic test in the specified direction. */ public Command flywheelSysIDDynamiCommand(SysIdRoutine.Direction direction) { - return feedSysId.dynamic(direction); + return flywheelSysId.dynamic(direction); } @Override @@ -154,5 +156,13 @@ public void periodic() { Logger.processInputs("Flywheels", fInputs); Logger.processInputs("Feeder", feedInputs); + + if (feederkP.hasChanged(hashCode())) { + feeder.configurePID(feederkP.get(), 0, 0); + } + + if (flywheelkP.hasChanged(hashCode())) { + flywheels.configurePID(flywheelkP.get(), 0, 0); + } } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index d94aa47..04cd2ca 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -21,7 +21,6 @@ public class Intake extends SubsystemBase { private final IntakeRollerIOInputsAutoLogged rInputs = new IntakeRollerIOInputsAutoLogged(); private static final LoggedTunableNumber intakekP = new LoggedTunableNumber("intake kP"); - private static final LoggedTunableNumber intakekD = new LoggedTunableNumber("intake kD"); private final SimpleMotorFeedforward ffModel; @@ -32,7 +31,6 @@ public Intake(IntakeRollerIO roller) { case REAL: ffModel = new SimpleMotorFeedforward(0, 0.003); intakekP.initDefault(0.0000000004); - intakekD.initDefault(0.007); break; case REPLAY: ffModel = new SimpleMotorFeedforward(0, 0); @@ -89,8 +87,8 @@ public void periodic() { // This method will be called once per scheduler run roller.updateInputs(rInputs); - if (intakekP.hasChanged(hashCode()) || intakekD.hasChanged(hashCode())) { - roller.configurePID(intakekP.get(), 0, intakekD.get()); + if (intakekP.hasChanged(hashCode())) { + roller.configurePID(intakekP.get(), 0, 0); } Logger.processInputs("Intake", rInputs); diff --git a/src/main/java/frc/robot/subsystems/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/pivot/Pivot.java index 78eb046..1622747 100644 --- a/src/main/java/frc/robot/subsystems/pivot/Pivot.java +++ b/src/main/java/frc/robot/subsystems/pivot/Pivot.java @@ -75,6 +75,8 @@ public Pivot(PivotIO pivot) { pivotProfile = new TrapezoidProfile(pivotConstraints); pivotCurrent = pivotProfile.calculate(0, pivotCurrent, pivotGoal); + + pivot.configurePID(pivotkP.get(), 0, 0); } public double getPivotPosition() {