From ecf86d0960576c8b5babe6f3ff7ba2ee89d973c3 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sat, 10 Feb 2024 15:06:46 -0500 Subject: [PATCH] WPI Testing Co-authored-by: suryatho --- .../org/littletonrobotics/frc2024/Robot.java | 2 +- .../frc2024/RobotContainer.java | 18 +++-- .../frc2024/commands/auto/AutoCommands.java | 9 --- .../subsystems/drive/ModuleIOSparkMax.java | 7 +- .../trajectory/GenerateTrajectories.java | 2 +- .../superstructure/Superstructure.java | 41 +++++++--- .../superstructure/arm/ArmIOKrakenFOC.java | 2 +- .../superstructure/feeder/Feeder.java | 16 +++- .../feeder/FeederIOSparkFlex.java | 1 + .../superstructure/flywheels/Flywheels.java | 4 +- .../superstructure/intake/Intake.java | 75 +++++-------------- .../superstructure/intake/IntakeIO.java | 5 +- .../intake/IntakeIOKrakenFOC.java | 2 +- .../superstructure/intake/IntakeIOSim.java | 2 +- .../intake/IntakeIOSparkMax.java | 7 +- 15 files changed, 88 insertions(+), 105 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index b10285b1..3b26c2da 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -87,7 +87,7 @@ public void robotInit() { setUseTiming(false); // Run as fast as possible String logPath = LogFileUtil.findReplayLog(); Logger.setReplaySource(new WPILOGReader(logPath)); - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"), 0.01)); break; } diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index bfcbbc75..adb2bb3b 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -106,7 +106,7 @@ public RobotContainer() { AprilTagVisionConstants.instanceNames[1], AprilTagVisionConstants.cameraIds[1])); intake = new Intake(new IntakeIOKrakenFOC()); - superstructure = new Superstructure(arm, flywheels, feeder); + superstructure = new Superstructure(arm, flywheels, feeder, intake); } case SIMBOT -> { drive = @@ -120,7 +120,7 @@ public RobotContainer() { flywheels = new Flywheels(new FlywheelsIOSim()); intake = new Intake(new IntakeIOSim()); feeder = new Feeder(new FeederIOSim()); - superstructure = new Superstructure(arm, flywheels, feeder); + superstructure = new Superstructure(arm, flywheels, feeder, intake); } case COMPBOT -> { // No impl yet @@ -164,7 +164,7 @@ public RobotContainer() { } if (superstructure == null) { - superstructure = new Superstructure(arm, flywheels, feeder); + superstructure = new Superstructure(arm, flywheels, feeder, intake); } autoChooser = new LoggedDashboardChooser<>("Auto Choices"); @@ -224,8 +224,6 @@ private void configureButtonBindings() { drive.setTeleopDriveGoal( -controller.getLeftY(), -controller.getLeftX(), -controller.getRightX()))); - controller.rightBumper().onTrue(intake.intakeCommand()).onFalse(intake.stopCommand()); - controller .leftTrigger() .onTrue(Commands.runOnce(drive::setAutoAimGoal)) @@ -263,7 +261,15 @@ private void configureButtonBindings() { .leftBumper() .onTrue( Commands.runOnce( - () -> superstructure.setGoalState(Superstructure.SystemState.INTAKE))) + () -> superstructure.setGoalState(Superstructure.SystemState.STATION_INTAKE))) + .onFalse( + Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE))); + + controller + .a() + .onTrue( + Commands.runOnce( + () -> superstructure.setGoalState(Superstructure.SystemState.REVERSE_INTAKE))) .onFalse( Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE))); } diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java index 28a1591a..2de63fcc 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import java.util.function.Supplier; import org.littletonrobotics.frc2024.RobotState; -import org.littletonrobotics.frc2024.subsystems.superstructure.intake.Intake; public class AutoCommands { @@ -17,14 +16,6 @@ public static Command waitForRegion(Supplier region) { return Commands.waitUntil(() -> inRegion(region)); } - public static Command intakeWhileInRegion(Intake intake, Supplier region) { - return Commands.sequence( - Commands.waitUntil(() -> inRegion(region)), - intake.intakeCommand(), - Commands.waitUntil(() -> !inRegion(region)), - intake.stopCommand()); - } - public interface Region { boolean contains(Translation2d point); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java index 705934a3..00103456 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSparkMax.java @@ -78,10 +78,11 @@ public ModuleIOSparkMax(ModuleConfig config) { driveMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); turnMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - - if (driveMotor.burnFlash().equals(REVLibError.kOk) - && turnMotor.burnFlash().equals(REVLibError.kOk)) break; } + + driveMotor.burnFlash(); + turnMotor.burnFlash(); + driveMotor.setCANTimeout(0); turnMotor.setCANTimeout(0); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java index 11354e71..f6e8215d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java @@ -12,7 +12,7 @@ public class GenerateTrajectories { public static void main(String[] args) { Constants.disableHAL(); var channel = - Grpc.newChannelBuilder("10.63.28.184:56328", InsecureChannelCredentials.create()).build(); + Grpc.newChannelBuilder("127.0.0.1:56328", InsecureChannelCredentials.create()).build(); var service = VehicleTrajectoryServiceGrpc.newBlockingStub(channel); VehicleModel model = diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index f78a4069..a4992142 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -5,23 +5,26 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; +import lombok.RequiredArgsConstructor; import lombok.Setter; import org.littletonrobotics.frc2024.FieldConstants; import org.littletonrobotics.frc2024.RobotState; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.Feeder; import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels; +import org.littletonrobotics.frc2024.subsystems.superstructure.intake.Intake; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +@RequiredArgsConstructor public class Superstructure extends SubsystemBase { private static LoggedTunableNumber armIdleSetpointDegrees = new LoggedTunableNumber("Superstructure/ArmIdleSetpointDegrees", 20.0); private static LoggedTunableNumber armStationIntakeSetpointDegrees = new LoggedTunableNumber("Superstructure/ArmStationIntakeSetpointDegrees", 45.0); private static LoggedTunableNumber armIntakeSetpointDegrees = - new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 20.0); + new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0); private static LoggedTunableNumber yCompensation = new LoggedTunableNumber("Superstructure/CompensationInches", 6.0); private static LoggedTunableNumber followThroughTime = @@ -30,25 +33,34 @@ public class Superstructure extends SubsystemBase { public enum SystemState { PREPARE_SHOOT, SHOOT, + + PREPARE_INTAKE, INTAKE, - IDLE, - STATION_INTAKE + STATION_INTAKE, + REVERSE_INTAKE, + IDLE + } + + public enum GamepieceState { + NO_GAMEPIECE, + + HOLDING_SHOOTER, + + HOLDING_BACKPACK } @Getter private SystemState currentState = SystemState.IDLE; @Getter @Setter private SystemState goalState = SystemState.IDLE; + + @Getter @Setter private GamepieceState gamepieceState = GamepieceState.NO_GAMEPIECE; + private final Arm arm; private final Flywheels flywheels; private final Feeder feeder; + private final Intake intake; private final Timer followThroughTimer = new Timer(); - public Superstructure(Arm arm, Flywheels flywheels, Feeder feeder) { - this.arm = arm; - this.flywheels = flywheels; - this.feeder = feeder; - } - @Override public void periodic() { switch (goalState) { @@ -65,6 +77,7 @@ public void periodic() { currentState = SystemState.IDLE; } } + case STATION_INTAKE -> currentState = SystemState.STATION_INTAKE; case INTAKE -> currentState = SystemState.INTAKE; case PREPARE_SHOOT -> currentState = SystemState.PREPARE_SHOOT; case SHOOT -> { @@ -83,16 +96,24 @@ public void periodic() { arm.setSetpoint(Rotation2d.fromDegrees(armIdleSetpointDegrees.get())); flywheels.setGoal(Flywheels.Goal.IDLE); feeder.setGoal(Feeder.Goal.IDLE); + intake.setGoal(Intake.Goal.IDLE); } case INTAKE -> { arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get())); flywheels.setGoal(Flywheels.Goal.IDLE); feeder.setGoal(Feeder.Goal.INTAKING); + intake.setGoal(Intake.Goal.INTAKE); } case STATION_INTAKE -> { arm.setSetpoint(Rotation2d.fromDegrees(armStationIntakeSetpointDegrees.get())); flywheels.setGoal(Flywheels.Goal.INTAKING); feeder.setGoal(Feeder.Goal.REVERSE_INTAKING); + intake.setGoal(Intake.Goal.IDLE); + } + case REVERSE_INTAKE -> { + arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get())); + feeder.setGoal(Feeder.Goal.REVERSE_INTAKING); + intake.setGoal(Intake.Goal.REVERSE_INTAKE); } case PREPARE_SHOOT -> { var aimingParams = RobotState.getInstance().getAimingParameters(); @@ -103,9 +124,11 @@ public void periodic() { + Units.inchesToMeters(yCompensation.get()))); flywheels.setGoal(Flywheels.Goal.SHOOTING); feeder.setGoal(Feeder.Goal.IDLE); + intake.setGoal(Intake.Goal.IDLE); } case SHOOT -> { feeder.setGoal(Feeder.Goal.FEEDING); + gamepieceState = GamepieceState.NO_GAMEPIECE; } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java index a44dea5a..591e8c74 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java @@ -55,7 +55,7 @@ public ArmIOKrakenFOC() { leaderInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; leaderConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; leaderConfig.Feedback.FeedbackRemoteSensorID = armEncoderID; - leaderConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + leaderConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.SyncCANcoder; leaderConfig.Feedback.SensorToMechanismRatio = 1.0; leaderConfig.Feedback.RotorToSensorRatio = reduction; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java index 7797321b..d8e1a1c4 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java @@ -4,9 +4,15 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; import lombok.Setter; +import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.Logger; public class Feeder extends SubsystemBase { + private static final LoggedTunableNumber intakeVoltage = + new LoggedTunableNumber("Feeder/IntakeVoltage", 4.0); + private static final LoggedTunableNumber feedVoltage = + new LoggedTunableNumber("Feeder/FeedVoltage", 12.0); + private final FeederIO io; private FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged(); @@ -37,15 +43,19 @@ public void periodic() { } else { switch (goal) { case IDLE -> runVolts(0.0); - case FEEDING -> runVolts(12.0); - case INTAKING -> runVolts(6.0); - case REVERSE_INTAKING -> runVolts(-6.0); + case FEEDING -> runVolts(feedVoltage.get()); + case INTAKING -> runVolts(intakeVoltage.get()); + case REVERSE_INTAKING -> runVolts(-intakeVoltage.get()); } } Logger.recordOutput("Feeder/Goal", goal); } + public boolean hasGamepiece() { + return inputs.outputCurrent >= 40.0; + } + public void runVolts(double volts) { io.runVolts(volts); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java index 46f945eb..bd4d8a27 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java @@ -14,6 +14,7 @@ public class FeederIOSparkFlex implements FeederIO { public FeederIOSparkFlex() { motor = new CANSparkFlex(id, CANSparkBase.MotorType.kBrushless); + motor.setSmartCurrentLimit(80); motor.setInverted(inverted); motor.setIdleMode(CANSparkBase.IdleMode.kBrake); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java index e17e7627..b559dffe 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java @@ -28,9 +28,9 @@ public class Flywheels extends SubsystemBase { new LoggedTunableNumber("Superstructure/IdleRightRPM", 200.0); private static LoggedTunableNumber intakingLeftRPM = - new LoggedTunableNumber("Superstructure/IdleLeftRPM", 2000.0); + new LoggedTunableNumber("Superstructure/IntakingLeftRPM", -2000.0); private static LoggedTunableNumber intakingRightRPM = - new LoggedTunableNumber("Superstructure/IdleRightRPM", -2000.0); + new LoggedTunableNumber("Superstructure/IntakingRightRPM", -2000.0); private static final LoggedTunableNumber shooterTolerance = new LoggedTunableNumber("Flywheels/ToleranceRPM", shooterToleranceRPM); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/Intake.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/Intake.java index d73967aa..7755b8a6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/Intake.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/Intake.java @@ -1,26 +1,29 @@ package org.littletonrobotics.frc2024.subsystems.superstructure.intake; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import lombok.Getter; +import lombok.Setter; +import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; public class Intake extends SubsystemBase { - private final LoggedDashboardNumber intakeVoltage = - new LoggedDashboardNumber("Intake/intakeVoltage", 8.0); + private final LoggedTunableNumber intakeVolts = + new LoggedTunableNumber("Intake/intakeVoltage", 8.0); + private final LoggedTunableNumber ejectVolts = new LoggedTunableNumber("Intake/ejectVolts", -2.0); private final IntakeIO io; private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - private boolean intake = false; - private boolean eject = false; + public enum Goal { + IDLE, + INTAKE, + REVERSE_INTAKE + } + + @Setter @Getter Goal goal = Goal.IDLE; public Intake(IntakeIO io) { System.out.println("[Init] Creating Intake"); this.io = io; - // TODO: test if this is needed - io.setBrakeMode(false); } @Override @@ -28,54 +31,10 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); - if (DriverStation.isDisabled()) { - stop(); - } else { - if (intake) { - io.setVoltage(intakeVoltage.get()); - } else if (eject) { - io.setVoltage(-intakeVoltage.get()); - } + switch (goal) { + case IDLE -> io.stop(); + case INTAKE -> io.runVolts(intakeVolts.get()); + case REVERSE_INTAKE -> io.runVolts(ejectVolts.get()); } } - - public boolean intaking() { - return intake; - } - - public boolean ejecting() { - return eject; - } - - public boolean running() { - return eject || intake; - } - - private void intake() { - intake = true; - eject = false; - } - - private void eject() { - eject = true; - intake = false; - } - - private void stop() { - intake = false; - eject = false; - io.stop(); - } - - public Command intakeCommand() { - return Commands.runOnce(this::intake); - } - - public Command ejectCommand() { - return Commands.runOnce(this::eject); - } - - public Command stopCommand() { - return Commands.runOnce(this::stop); - } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIO.java index 37f59428..047c3fe7 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIO.java @@ -15,10 +15,7 @@ class IntakeIOInputs { default void updateInputs(IntakeIOInputs inputs) {} /** Set voltage of intake */ - default void setVoltage(double volts) {} - - /** Set brake mode of intake */ - default void setBrakeMode(boolean enabled) {} + default void runVolts(double volts) {} /** Stop intake */ default void stop() {} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOKrakenFOC.java index 82630be6..573f0e19 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOKrakenFOC.java @@ -56,7 +56,7 @@ public void updateInputs(IntakeIOInputs inputs) { } @Override - public void setVoltage(double volts) { + public void runVolts(double volts) { motor.setControl(new VoltageOut(volts).withEnableFOC(true)); otherMotor.setControl(new VoltageOut(volts).withEnableFOC(true)); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSim.java index 55c833b6..ef73c587 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSim.java @@ -18,7 +18,7 @@ public void updateInputs(IntakeIOInputs inputs) { } @Override - public void setVoltage(double volts) { + public void runVolts(double volts) { appliedVoltage = volts; sim.setInputVoltage(volts); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSparkMax.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSparkMax.java index bef8fcee..324a8358 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSparkMax.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/intake/IntakeIOSparkMax.java @@ -30,15 +30,10 @@ public void updateInputs(IntakeIOInputs inputs) { } @Override - public void setVoltage(double volts) { + public void runVolts(double volts) { motor.setVoltage(volts); } - @Override - public void setBrakeMode(boolean enabled) { - motor.setIdleMode(enabled ? CANSparkMax.IdleMode.kBrake : CANSparkMax.IdleMode.kCoast); - } - @Override public void stop() { motor.stopMotor();