diff --git a/src/main/java/frc/robot/RobotConstants.java b/src/main/java/frc/robot/RobotConstants.java index 5e682d5..fc90fe6 100644 --- a/src/main/java/frc/robot/RobotConstants.java +++ b/src/main/java/frc/robot/RobotConstants.java @@ -377,28 +377,28 @@ public static class ElevatorConstants { public static final double ELEVATOR_SPOOL_DIAMETER = 0.04 + 0.003; //0.04m for spool diameter, 0.003 for rope diameter public static final double ELEVATOR_GEAR_RATIO = 3.0; - public static final double DEADZONE_DISTANCE = 0.02; + public static final double DEADZONE_DISTANCE = 0.0; public static final TunableNumber motionAcceleration = new TunableNumber("Elevator/MotionAcceleration", - 20.0); + 200); public static final TunableNumber motionCruiseVelocity = new TunableNumber( - "Elevator/MotionCruiseVelocity", 20.0); + "Elevator/MotionCruiseVelocity", 100); public static final TunableNumber motionJerk = new TunableNumber("Elevator/MotionJerk", 0.0); public static final TunableNumber MAX_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/max", - 2); + 1.57); public static final TunableNumber IDLE_EXTENSION_METERS = new TunableNumber( "ELEVATOR GOALS/idle", 0); public static final TunableNumber L1_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L1", - 0.1); + 0.45); public static final TunableNumber L2_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L2", - 0.2); + 0.53); public static final TunableNumber L3_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L3", - 0.3);// 0.107853495 + 0.95);// 0.107853495 public static final TunableNumber L4_EXTENSION_METERS = new TunableNumber("ELEVATOR SETPOINTS/L4", - 0.4); + 1.55); public static final TunableNumber ELEVATOR_ZEROING_CURRENT = new TunableNumber("Elevator zeroing current", - 15); + 20); } @@ -407,13 +407,14 @@ public static class ElevatorConstants { * Constants for the elevator motor gains. */ public static class ElevatorGainsClass { - public static final TunableNumber ELEVATOR_KP = new TunableNumber("ELEVATOR PID/kp", 0.2); + public static final TunableNumber ELEVATOR_KP = new TunableNumber("ELEVATOR PID/kp", 15); public static final TunableNumber ELEVATOR_KI = new TunableNumber("ELEVATOR PID/ki", 0); - public static final TunableNumber ELEVATOR_KD = new TunableNumber("ELEVATOR PID/kd", 0.001); + public static final TunableNumber ELEVATOR_KD = new TunableNumber("ELEVATOR PID/kd", 0); public static final TunableNumber ELEVATOR_KA = new TunableNumber("ELEVATOR PID/ka", - 0.0037512677); - public static final TunableNumber ELEVATOR_KV = new TunableNumber("ELEVATOR PID/kv", 0.113);// 0.107853495 + 0); + public static final TunableNumber ELEVATOR_KV = new TunableNumber("ELEVATOR PID/kv", 0);// 0.107853495 public static final TunableNumber ELEVATOR_KS = new TunableNumber("ELEVATOR PID/ks", - 0.28475008); + 0); + public static final TunableNumber ELEVATOR_KG = new TunableNumber("ELEVATOR PID/kg", 0.3); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c46caa1..dfd5f9b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,6 +34,7 @@ import static edu.wpi.first.units.Units.Seconds; import static frc.robot.RobotConstants.BeamBreakConstants.*; +import static frc.robot.RobotConstants.ElevatorConstants.*; import java.io.IOException; import java.util.function.*; @@ -137,10 +138,10 @@ private void configureTesterBindings(CommandXboxController controller) { new Trigger(controller.start()) .onTrue(superstructure.setWantedSuperStateCommand(Superstructure.WantedSuperState.STOPPED)); //test of elevator heights - controller.a().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(0.5),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(0.5))); - controller.b().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(0.6),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(0.6))); - controller.x().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(0.7),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(0.7))); - controller.y().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(0.47),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(0.47))); + controller.a().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L1_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L1_EXTENSION_METERS.get()))); + controller.b().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L2_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L2_EXTENSION_METERS.get()))); + controller.x().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L3_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L3_EXTENSION_METERS.get()))); + controller.y().onTrue(Commands.runOnce(() -> elevatorSubsystem.setPosition(L4_EXTENSION_METERS.get()),elevatorSubsystem).until(() ->elevatorSubsystem.isAtSetpoint(L4_EXTENSION_METERS.get()))); controller.povDown().onTrue(new ElevatorZeroingCommand(elevatorSubsystem)); } diff --git a/src/main/java/frc/robot/commands/ElevatorZeroingCommand.java b/src/main/java/frc/robot/commands/ElevatorZeroingCommand.java index 99a67fc..2342c5e 100644 --- a/src/main/java/frc/robot/commands/ElevatorZeroingCommand.java +++ b/src/main/java/frc/robot/commands/ElevatorZeroingCommand.java @@ -21,17 +21,16 @@ public void initialize() { @Override public void execute(){ - elevator.setVoltage(-0.5); + elevator.setVoltage(-1); } @Override public boolean isFinished() { - return elevator.getLeaderCurrent() > RobotConstants.ElevatorConstants.ELEVATOR_ZEROING_CURRENT.get(); + return Math.abs(elevator.getLeaderCurrent()) > RobotConstants.ElevatorConstants.ELEVATOR_ZEROING_CURRENT.get(); } @Override public void end(boolean interrupted) { - elevator.resetPosition(); elevator.setVoltage(0); } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index c07a478..7fa2b12 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -12,12 +12,14 @@ public class Superstructure extends SubsystemBase { public enum WantedSuperState { STOPPED, INTAKE_CORAL_FUNNEL, + INTAKE_CORAL_GROUND, SHOOT_CORAL } public enum CurrentSuperState { STOPPED, INTAKE_CORAL_FUNNEL, + INTAKE_CORAL_GROUND, SHOOT_CORAL } @@ -45,6 +47,9 @@ private CurrentSuperState handleStateTransition() { case INTAKE_CORAL_FUNNEL: currentSuperState = CurrentSuperState.INTAKE_CORAL_FUNNEL; break; + case INTAKE_CORAL_GROUND: + currentSuperState = CurrentSuperState.INTAKE_CORAL_GROUND; + break; case SHOOT_CORAL: currentSuperState = CurrentSuperState.SHOOT_CORAL; break; @@ -61,6 +66,9 @@ private void applyStates() { case INTAKE_CORAL_FUNNEL: intakeCoralFunnel(); break; + case INTAKE_CORAL_GROUND: + intakeCoralGround(); + break; case SHOOT_CORAL: shootCoral(); break; @@ -80,15 +88,23 @@ public Command setWantedSuperStateCommand(WantedSuperState wantedSuperState) { } private void intakeCoralFunnel() { - if (endEffector.isFunnelIntakeFinished()) { - endEffector.setWantedState(EndEffectorSubsystem.WantedState.FUNNEL_TRANSFER); + if (endEffector.isIntakeFinished()) { + endEffector.setWantedState(EndEffectorSubsystem.WantedState.TRANSFER); } else { endEffector.setWantedState(EndEffectorSubsystem.WantedState.FUNNEL_INTAKE); } } + private void intakeCoralGround() { + if (endEffector.isIntakeFinished()) { + endEffector.setWantedState(EndEffectorSubsystem.WantedState.TRANSFER); + } else { + endEffector.setWantedState(EndEffectorSubsystem.WantedState.GROUND_INTAKE); + } + } + private void shootCoral() { - endEffector.setWantedState(EndEffectorSubsystem.WantedState.SHOOT); + if (endEffector.isCoralReady()) {endEffector.setWantedState(EndEffectorSubsystem.WantedState.SHOOT);} } private void handleStopped() { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 2ff801b..f9d1e3d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -27,10 +27,6 @@ public default void stop() { setVoltage(0.0); } - public default double getLeaderCurrent(){ - return getLeaderCurrent(); - } - public default void resetEncoder(double position) {} public default void resetEncoder() { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 2c540ee..c238d3c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -47,7 +47,6 @@ public class ElevatorIOReal implements ElevatorIO { private final StatusSignal supplyRight; private final StatusSignal tempLeft; private final StatusSignal tempRight; - private final StatusSignal closedLoopReferenceSlope; public ElevatorIOReal() { this.leader = new TalonFX(LEFT_ELEVATOR_MOTOR_ID, CANIVORE_CAN_BUS_NAME); @@ -98,17 +97,21 @@ public ElevatorIOReal() { statorLeft = leader.getStatorCurrent(); statorRight = follower.getStatorCurrent(); supplyLeft = leader.getSupplyCurrent(); - supplyRight = leader.getSupplyCurrent(); + supplyRight = follower.getSupplyCurrent(); tempLeft = leader.getDeviceTemp(); tempRight = follower.getDeviceTemp(); - closedLoopReferenceSlope = leader.getClosedLoopReferenceSlope(); follower.setControl(new Follower(leader.getDeviceID(), true)); } @Override public void updateInputs(ElevatorIOInputs inputs) { - BaseStatusSignal.refreshAll(); + BaseStatusSignal.refreshAll( + voltageLeft, voltageRight, + statorLeft, statorRight, + supplyLeft, supplyRight, + tempLeft, tempRight + ); inputs.positionMeters = getHeight(); inputs.velocityMetersPerSec = getVelocity(); @@ -126,6 +129,7 @@ public void updateInputs(ElevatorIOInputs inputs) { slot0Configs.kP = ELEVATOR_KP.get(); slot0Configs.kI = ELEVATOR_KI.get(); slot0Configs.kD = ELEVATOR_KD.get(); + slot0Configs.kG = ELEVATOR_KG.get(); motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); @@ -154,9 +158,7 @@ public void resetEncoder(final double position) { follower.setPosition(position); } - public double getLeaderCurrent(){ - return leader.getStatorCurrent().getValueAsDouble(); - } + @Override public void resetPosition(){ diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 51e828e..847f93e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.elevator; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; @@ -10,6 +11,9 @@ public class ElevatorSubsystem extends SubsystemBase { private final ElevatorIO io; private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + private final LinearFilter currentFilter = LinearFilter.movingAverage(5); + public double currentFilterValue = 0.0; + public ElevatorSubsystem(ElevatorIO io) { this.io = io; } @@ -18,6 +22,9 @@ public ElevatorSubsystem(ElevatorIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Elevator", inputs); + + currentFilterValue = currentFilter.calculate(inputs.statorCurrentAmps[0]); + Logger.recordOutput("Elevator/CurrentFilter", currentFilterValue); } public void setPosition(double heightMeters) { @@ -38,7 +45,7 @@ public boolean isAtSetpoint(double setpoint) { } public double getLeaderCurrent(){ - return io.getLeaderCurrent(); + return currentFilterValue; } public void resetPosition(){ diff --git a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java index a5861f9..2c3266e 100644 --- a/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/endeffector/EndEffectorSubsystem.java @@ -25,6 +25,7 @@ public class EndEffectorSubsystem extends RollerSubsystem { private SystemState systemState = SystemState.IDLING; private boolean hasTransitionedToTransfer = false; + private boolean hasTransitionedToHold = false; public double kp = ENDEFFECTOR_KP.get(); public double ki = ENDEFFECTOR_KI.get(); @@ -42,7 +43,8 @@ public class EndEffectorSubsystem extends RollerSubsystem { public enum WantedState { IDLE, FUNNEL_INTAKE, - FUNNEL_TRANSFER, + GROUND_INTAKE, + TRANSFER, HOLD, SHOOT, OFF @@ -51,7 +53,8 @@ public enum WantedState { public enum SystemState { IDLING, FUNNEL_INTAKING, - FUNNEL_TRANSFERRING, + GROUND_INTAKING, + TRANSFERRING, HOLDING, SHOOTING, OFF @@ -93,7 +96,10 @@ public void periodic() { case FUNNEL_INTAKING: io.setVelocity(intakeRPS); break; - case FUNNEL_TRANSFERRING: + case GROUND_INTAKING: + io.setVelocity(-intakeRPS); + break; + case TRANSFERRING: io.setVelocity(transferRPS); break; case HOLDING: @@ -129,19 +135,31 @@ private SystemState handleStateTransition() { case FUNNEL_INTAKE -> { if (middleBBInputs.isBeambreakOn) { hasTransitionedToTransfer = true; - yield SystemState.FUNNEL_TRANSFERRING; + yield SystemState.TRANSFERRING; } yield SystemState.FUNNEL_INTAKING; } - case FUNNEL_TRANSFER -> { + case GROUND_INTAKE -> { + if (middleBBInputs.isBeambreakOn) { + hasTransitionedToTransfer = true; + yield SystemState.TRANSFERRING; + } + yield SystemState.GROUND_INTAKING; + } + case TRANSFER -> { if (edgeBBInputs.isBeambreakOn && !middleBBInputs.isBeambreakOn) { yield SystemState.HOLDING; } - yield SystemState.FUNNEL_TRANSFERRING; + yield SystemState.TRANSFERRING; + } + case HOLD -> { + hasTransitionedToHold = true; + yield SystemState.HOLDING; } - case HOLD -> SystemState.HOLDING; case SHOOT -> { if (isShootFinished()) { + hasTransitionedToTransfer = false; + hasTransitionedToHold = false; yield SystemState.IDLING; } yield SystemState.SHOOTING; @@ -151,11 +169,15 @@ private SystemState handleStateTransition() { }; } + public boolean isCoralReady () { + return hasTransitionedToHold; + } + public boolean isShootFinished () { - return hasTransitionedToTransfer && !edgeBBInputs.isBeambreakOn; + return hasTransitionedToHold && !edgeBBInputs.isBeambreakOn; } - public boolean isFunnelIntakeFinished () { + public boolean isIntakeFinished () { return hasTransitionedToTransfer; }