From 1655e5df4299b21824caed7773ef8ceb1a689289 Mon Sep 17 00:00:00 2001 From: manthan-acharya Date: Sat, 10 Feb 2024 13:03:33 -0500 Subject: [PATCH] Update state machine --- .../littletonrobotics/frc2024/Constants.java | 2 +- .../frc2024/RobotContainer.java | 29 ++++---- ...uperstructure.java => Superstructure.java} | 46 ++++++------- .../SuperstructureConstants.java | 4 +- .../subsystems/superstructure/arm/Arm.java | 21 ++---- .../superstructure/arm/ArmIOSim.java | 7 +- .../superstructure/feeder/Feeder.java | 29 ++++++++ .../superstructure/intake/Intake.java | 2 +- .../intake/IntakeIOKrakenFOC.java | 66 ++++++++++++++++++- 9 files changed, 141 insertions(+), 65 deletions(-) rename src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/{DevBotSuperstructure.java => Superstructure.java} (70%) diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java index 0949b15a..61e4f76c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Constants.java +++ b/src/main/java/org/littletonrobotics/frc2024/Constants.java @@ -27,7 +27,7 @@ */ public final class Constants { public static final int loopPeriodMs = 20; - private static RobotType robotType = RobotType.SIMBOT; + private static RobotType robotType = RobotType.DEVBOT; public static final boolean tuningMode = true; private static boolean invalidRobotAlertSent = false; diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 58889576..a6cecccd 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -33,7 +33,7 @@ import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIONorthstar; import org.littletonrobotics.frc2024.subsystems.drive.*; -import org.littletonrobotics.frc2024.subsystems.superstructure.DevBotSuperstructure; +import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC; @@ -72,7 +72,7 @@ public class RobotContainer { private Feeder feeder; - private DevBotSuperstructure superstructure; + private Superstructure superstructure; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -103,8 +103,8 @@ public RobotContainer() { new AprilTagVisionIONorthstar( AprilTagVisionConstants.instanceNames[1], AprilTagVisionConstants.cameraIds[1])); - // intake = new Intake(new IntakeIOSparkMax()); - superstructure = new DevBotSuperstructure(arm, flywheels, feeder); + intake = new Intake(new IntakeIOKrakenFOC()); + superstructure = new Superstructure(arm, flywheels, feeder); } case SIMBOT -> { drive = @@ -118,7 +118,7 @@ public RobotContainer() { flywheels = new Flywheels(new FlywheelsIOSim()); intake = new Intake(new IntakeIOSim()); feeder = new Feeder(new FeederIOSim()); - superstructure = new DevBotSuperstructure(arm, flywheels, feeder); + superstructure = new Superstructure(arm, flywheels, feeder); } case COMPBOT -> { // No impl yet @@ -162,7 +162,7 @@ public RobotContainer() { } if (superstructure == null) { - superstructure = new DevBotSuperstructure(arm, flywheels, feeder); + superstructure = new Superstructure(arm, flywheels, feeder); } autoChooser = new LoggedDashboardChooser<>("Auto Choices"); @@ -234,11 +234,9 @@ private void configureButtonBindings() { .leftTrigger() .onTrue( Commands.runOnce( - () -> - superstructure.setGoalState(DevBotSuperstructure.SystemState.PREPARE_SHOOT))) + () -> superstructure.setGoalState(Superstructure.SystemState.PREPARE_SHOOT))) .onFalse( - Commands.runOnce( - () -> superstructure.setGoalState(DevBotSuperstructure.SystemState.IDLE))); + Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE))); Trigger readyToShootTrigger = new Trigger(() -> drive.isAutoAimGoalCompleted() && superstructure.atShootingSetpoint()); @@ -253,22 +251,19 @@ private void configureButtonBindings() { .rightTrigger() .and(readyToShootTrigger) .onTrue( - Commands.runOnce( - () -> superstructure.setGoalState(DevBotSuperstructure.SystemState.SHOOT)) + Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.SHOOT)) .andThen(Commands.waitSeconds(0.5)) .andThen( Commands.runOnce( - () -> - superstructure.setGoalState(DevBotSuperstructure.SystemState.IDLE)))); + () -> superstructure.setGoalState(Superstructure.SystemState.IDLE)))); controller .leftBumper() .onTrue( Commands.runOnce( - () -> superstructure.setGoalState(DevBotSuperstructure.SystemState.INTAKE))) + () -> superstructure.setGoalState(Superstructure.SystemState.INTAKE))) .onFalse( - Commands.runOnce( - () -> superstructure.setGoalState(DevBotSuperstructure.SystemState.IDLE))); + Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE))); } controller diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java similarity index 70% rename from src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java rename to src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index 79a788f9..e0fe1faa 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -15,29 +15,24 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class DevBotSuperstructure extends SubsystemBase { +public class Superstructure extends SubsystemBase { private static LoggedTunableNumber armIdleSetpointDegrees = - new LoggedTunableNumber("DevBotSuperstructure/ArmIdleSetpointDegrees", 10.0); + new LoggedTunableNumber("Superstructure/ArmIdleSetpointDegrees", 20.0); + private static LoggedTunableNumber armStationIntakeSetpointDegrees = + new LoggedTunableNumber("Superstructure/ArmStationIntakeSetpointDegrees", 45.0); private static LoggedTunableNumber armIntakeSetpointDegrees = - new LoggedTunableNumber("DevBotSuperstructure/ArmIntakeSetpointDegrees", 50.0); - private static LoggedTunableNumber shootingLeftRPM = - new LoggedTunableNumber("DevBotSuperstructure/ShootingLeftRPM", 6000.0); - private static LoggedTunableNumber shootingRightRPM = - new LoggedTunableNumber("DevBotSuperstructure/ShootingRightRPM", 4000.0); - private static LoggedTunableNumber idleLeftRPM = - new LoggedTunableNumber("DevBotSuperstructure/IdleLeftRPM", 2000.0); - private static LoggedTunableNumber idleRightRPM = - new LoggedTunableNumber("DevBotSuperstructure/IdleRightRPM", 2000.0); + new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 20.0); private static LoggedTunableNumber yCompensation = - new LoggedTunableNumber("DevBotSuperstructure/CompensationInches", 6.0); + new LoggedTunableNumber("Superstructure/CompensationInches", 6.0); private static LoggedTunableNumber followThroughTime = - new LoggedTunableNumber("DevBotSuperstructure/FollowthroughTimeSecs", 0.5); + new LoggedTunableNumber("Superstructure/FollowthroughTimeSecs", 0.5); public enum SystemState { PREPARE_SHOOT, SHOOT, INTAKE, - IDLE + IDLE, + STATION_INTAKE } @Getter private SystemState currentState = SystemState.IDLE; @@ -48,7 +43,7 @@ public enum SystemState { private final Timer followThroughTimer = new Timer(); - public DevBotSuperstructure(Arm arm, Flywheels flywheels, Feeder feeder) { + public Superstructure(Arm arm, Flywheels flywheels, Feeder feeder) { this.arm = arm; this.flywheels = flywheels; this.feeder = feeder; @@ -91,8 +86,13 @@ public void periodic() { } case INTAKE -> { arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get())); - flywheels.runVolts(-2.0, -2.0); - feeder.runVolts(-0.5); + flywheels.setGoal(Flywheels.Goal.IDLE); + feeder.setGoal(Feeder.Goal.INTAKING); + } + case STATION_INTAKE -> { + arm.setSetpoint(Rotation2d.fromDegrees(armStationIntakeSetpointDegrees.get())); + flywheels.setGoal(Flywheels.Goal.INTAKING); + feeder.setGoal(Feeder.Goal.REVERSE_INTAKING); } case PREPARE_SHOOT -> { var aimingParams = RobotState.getInstance().getAimingParameters(); @@ -101,19 +101,19 @@ public void periodic() { aimingParams.effectiveDistance(), FieldConstants.Speaker.centerSpeakerOpening.getZ() + Units.inchesToMeters(yCompensation.get()))); - flywheels.setSetpointRpm(shootingLeftRPM.get(), shootingRightRPM.get()); - feeder.runVolts(0.0); + flywheels.setGoal(Flywheels.Goal.SHOOTING); + feeder.setGoal(Feeder.Goal.IDLE); } case SHOOT -> { - feeder.runVolts(2.0); + feeder.setGoal(Feeder.Goal.FEEDING); } } - Logger.recordOutput("DevBotSuperstructure/GoalState", goalState); - Logger.recordOutput("DevBotSuperstructure/currentState", currentState); + Logger.recordOutput("Superstructure/GoalState", goalState); + Logger.recordOutput("Superstructure/CurrentState", currentState); } - @AutoLogOutput(key = "DevBotSuperstructure/ReadyToShoot") + @AutoLogOutput(key = "Superstructure/ReadyToShoot") public boolean atShootingSetpoint() { return flywheels.atSetpoint(); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java index 5232e224..115dda05 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java @@ -38,14 +38,14 @@ public record Gains(double kP, double kI, double kD, double kS, double kV, doubl } public static class IntakeConstants { - public static double reduction = (1.0 / 1.0); + public static double reduction = (18.0 / 12.0); public static int id = switch (Constants.getRobot()) { default -> 45; }; public static boolean inverted = switch (Constants.getRobot()) { - default -> true; + default -> false; }; } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index 42525179..75960f07 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -19,20 +19,13 @@ import org.littletonrobotics.junction.Logger; public class Arm extends SubsystemBase { - private static final LoggedTunableNumber kP = - new LoggedTunableNumber("Arm/kP", gains.kP()); - private static final LoggedTunableNumber kI = - new LoggedTunableNumber("Arm/kI", gains.kI()); - private static final LoggedTunableNumber kD = - new LoggedTunableNumber("Arm/kD", gains.kD()); - private static final LoggedTunableNumber kS = - new LoggedTunableNumber("Arm/kS", gains.ffkS()); - private static final LoggedTunableNumber kV = - new LoggedTunableNumber("Arm/kV", gains.ffkV()); - private static final LoggedTunableNumber kA = - new LoggedTunableNumber("Arm/kA", gains.ffkA()); - private static final LoggedTunableNumber kG = - new LoggedTunableNumber("Arm/kG", gains.ffkG()); + private static final LoggedTunableNumber kP = new LoggedTunableNumber("Arm/kP", gains.kP()); + private static final LoggedTunableNumber kI = new LoggedTunableNumber("Arm/kI", gains.kI()); + private static final LoggedTunableNumber kD = new LoggedTunableNumber("Arm/kD", gains.kD()); + private static final LoggedTunableNumber kS = new LoggedTunableNumber("Arm/kS", gains.ffkS()); + private static final LoggedTunableNumber kV = new LoggedTunableNumber("Arm/kV", gains.ffkV()); + private static final LoggedTunableNumber kA = new LoggedTunableNumber("Arm/kA", gains.ffkA()); + private static final LoggedTunableNumber kG = new LoggedTunableNumber("Arm/kG", gains.ffkG()); private static final LoggedTunableNumber armVelocity = new LoggedTunableNumber("Arm/Velocity", profileConstraints.cruiseVelocityRadPerSec()); private static final LoggedTunableNumber armAcceleration = diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java index 8940ecc9..9f8d2c4f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java @@ -32,12 +32,7 @@ public class ArmIOSim implements ArmIO { private boolean closedLoop = false; public ArmIOSim() { - ff = - new ArmFeedforward( - gains.ffkS(), - gains.ffkG(), - gains.ffkV(), - gains.ffkA()); + ff = new ArmFeedforward(gains.ffkS(), gains.ffkG(), gains.ffkV(), gains.ffkA()); profiledController = new ProfiledPIDController( gains.kP(), 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 37669cff..7797321b 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 @@ -1,20 +1,49 @@ package org.littletonrobotics.frc2024.subsystems.superstructure.feeder; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import lombok.Getter; +import lombok.Setter; import org.littletonrobotics.junction.Logger; public class Feeder extends SubsystemBase { private final FeederIO io; private FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged(); + private double leftSetpointRpm; + private double rightSetpointRPM; + public Feeder(FeederIO io) { this.io = io; } + public enum Goal { + IDLE, + FEEDING, + INTAKING, + REVERSE_INTAKING + } + + @Getter @Setter private Goal goal = Goal.IDLE; + @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Feeder", inputs); + + if (DriverStation.isDisabled()) { + stop(); + setGoal(Goal.IDLE); + } else { + switch (goal) { + case IDLE -> runVolts(0.0); + case FEEDING -> runVolts(12.0); + case INTAKING -> runVolts(6.0); + case REVERSE_INTAKING -> runVolts(-6.0); + } + } + + Logger.recordOutput("Feeder/Goal", goal); } public void runVolts(double volts) { 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 8dfb58e0..d73967aa 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 @@ -9,7 +9,7 @@ public class Intake extends SubsystemBase { private final LoggedDashboardNumber intakeVoltage = - new LoggedDashboardNumber("Intake/intakeVoltage", 12.0); + new LoggedDashboardNumber("Intake/intakeVoltage", 8.0); private final IntakeIO io; private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); 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 67bb239f..82630be6 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 @@ -1,5 +1,69 @@ package org.littletonrobotics.frc2024.subsystems.superstructure.intake; +import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.IntakeConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.util.Units; + public class IntakeIOKrakenFOC implements IntakeIO { - public IntakeIOKrakenFOC() {} + private final TalonFX motor; + + private final TalonFX otherMotor; + private StatusSignal velocityRadsPerSec; + private StatusSignal positionRads; + private StatusSignal appliedVoltage; + private StatusSignal currentAmps; + + public IntakeIOKrakenFOC() { + motor = new TalonFX(2, "canivore"); + otherMotor = new TalonFX(3); + + TalonFXConfiguration config = new TalonFXConfiguration(); + config.CurrentLimits.SupplyCurrentLimit = 30.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = + inverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + motor.getConfigurator().apply(config); + + velocityRadsPerSec = motor.getVelocity(); + positionRads = motor.getPosition(); + appliedVoltage = motor.getMotorVoltage(); + currentAmps = motor.getTorqueCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, velocityRadsPerSec, positionRads, appliedVoltage, currentAmps); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + BaseStatusSignal.refreshAll(velocityRadsPerSec, positionRads, appliedVoltage, currentAmps); + + inputs.velocityRadsPerSec = + Units.rotationsToRadians(velocityRadsPerSec.getValueAsDouble()) / reduction; + inputs.positionRads = Units.rotationsToRadians(positionRads.getValueAsDouble()) / reduction; + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setVoltage(double volts) { + motor.setControl(new VoltageOut(volts).withEnableFOC(true)); + otherMotor.setControl(new VoltageOut(volts).withEnableFOC(true)); + } + + @Override + public void stop() { + motor.setControl(new NeutralOut()); + otherMotor.setControl(new NeutralOut()); + } }