From ca94b074ad925ace36b2b42bb0b251d1bf5e4fb8 Mon Sep 17 00:00:00 2001 From: suryatho Date: Thu, 15 Feb 2024 00:24:03 -0500 Subject: [PATCH 1/6] Added LinearProfile.java Use LinearProfile for flywheels --- .../flywheels/FlywheelConstants.java | 10 +- .../subsystems/flywheels/Flywheels.java | 94 ++++++++++++++----- .../subsystems/flywheels/FlywheelsIOSim.java | 4 +- .../flywheels/FlywheelsIOSparkFlex.java | 16 ++-- .../frc2024/util/LinearProfile.java | 72 ++++++++++++++ 5 files changed, 155 insertions(+), 41 deletions(-) create mode 100644 src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java index 913c3ed3..5197fcfb 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java @@ -10,21 +10,21 @@ import org.littletonrobotics.frc2024.Constants; public class FlywheelConstants { - public static Config config = + public static FlywheelConfig flywheelConfig = switch (Constants.getRobot()) { case COMPBOT -> null; - case DEVBOT -> new Config(5, 4, (1.0 / 2.0), 100.0); - case SIMBOT -> new Config(0, 0, (1.0 / 2.0), 100.0); + case DEVBOT -> new FlywheelConfig(5, 4, (1.0 / 2.0), 6000.0, 100.0); + case SIMBOT -> new FlywheelConfig(0, 0, (1.0 / 2.0), 6000.0, 50.0); }; public static Gains gains = switch (Constants.getRobot()) { case COMPBOT -> null; case DEVBOT -> new Gains(0.0006, 0.0, 0.05, 0.33329, 0.00083, 0.0); - case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0); + case SIMBOT -> new Gains(0.05, 0.0, 0.0, 0.01, 0.00103, 0.0); }; - public record Config(int leftID, int rightID, double reduction, double toleranceRPM) {} + public record FlywheelConfig(int leftID, int rightID, double reduction, double maxAcclerationRpmPerSec, double toleranceRPM) {} public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {} } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java index 622cca30..55a20ad0 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -15,6 +15,7 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; +import org.littletonrobotics.frc2024.util.LinearProfile; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -34,17 +35,23 @@ public class Flywheels extends SubsystemBase { new LoggedTunableNumber("Superstructure/IdleLeftRPM", 200.0); private static LoggedTunableNumber idleRightRPM = new LoggedTunableNumber("Superstructure/IdleRightRPM", 200.0); - private static LoggedTunableNumber intakingLeftRPM = new LoggedTunableNumber("Superstructure/IntakingLeftRPM", -2000.0); private static LoggedTunableNumber intakingRightRPM = new LoggedTunableNumber("Superstructure/IntakingRightRPM", -2000.0); private static final LoggedTunableNumber shooterTolerance = - new LoggedTunableNumber("Flywheels/ToleranceRPM", config.toleranceRPM()); + new LoggedTunableNumber("Flywheels/ToleranceRPM", flywheelConfig.toleranceRPM()); + private static final LoggedTunableNumber maxAcceleration = + new LoggedTunableNumber("Flywheels/MaxAccelerationRpmPerSec", flywheelConfig.maxAcclerationRpmPerSec()); private final FlywheelsIO io; private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged(); + private final LinearProfile leftProfile; + private final LinearProfile rightProfile; + private boolean wasClosedLoop = false; + private boolean closedLoop = false; + @RequiredArgsConstructor public enum Goal { STOP(() -> 0.0, () -> 0.0), @@ -53,15 +60,15 @@ public enum Goal { INTAKING(intakingLeftRPM, intakingRightRPM), CHARACTERIZING(() -> 0.0, () -> 0.0); - private final DoubleSupplier leftSetpoint; - private final DoubleSupplier rightSetpoint; + private final DoubleSupplier leftGoal; + private final DoubleSupplier rightGoal; - private double getLeftSetpoint() { - return leftSetpoint.getAsDouble(); + private double getLeftGoal() { + return leftGoal.getAsDouble(); } - private double getRightSetpoint() { - return rightSetpoint.getAsDouble(); + private double getRightGoal() { + return rightGoal.getAsDouble(); } } @@ -69,6 +76,10 @@ private double getRightSetpoint() { public Flywheels(FlywheelsIO io) { this.io = io; + + leftProfile = new LinearProfile(maxAcceleration.get(), 0.02); + rightProfile = new LinearProfile(maxAcceleration.get(), 0.02); + setDefaultCommand(runOnce(() -> goal = Goal.IDLE).withName("FlywheelsIdle")); } @@ -81,26 +92,56 @@ public void periodic() { LoggedTunableNumber.ifChanged(hashCode(), pid -> io.setPID(pid[0], pid[1], pid[2]), kP, kI, kD); LoggedTunableNumber.ifChanged( hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA); - + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + leftProfile.setMaxAcceleration(maxAcceleration.get()); + rightProfile.setMaxAcceleration(maxAcceleration.get()); + }, + maxAcceleration); + + // Stop when disabled if (DriverStation.isDisabled()) { - goal = Goal.STOP; + setGoal(Goal.STOP); } - switch (goal) { - case STOP -> io.stop(); - case CHARACTERIZING -> {} // Handled by runCharacterizationVolts - default -> io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint()); + // Check if profile needs to be reset + if (!closedLoop && wasClosedLoop) { + leftProfile.reset(); + rightProfile.reset(); + wasClosedLoop = false; + } + + // Run to setpoint + if (closedLoop) { + double leftSetpoint = leftProfile.calculateSetpoint(); + double rightSetpoint = rightProfile.calculateSetpoint(); + io.runVelocity(leftSetpoint, rightSetpoint); + + Logger.recordOutput("Flywheels/SetpointLeftRpm", leftSetpoint); + Logger.recordOutput("Flywheels/SetpointRightRpm", rightSetpoint); } Logger.recordOutput("Flywheels/Goal", goal); - Logger.recordOutput("Flywheels/LeftSetpointRPM", goal.getLeftSetpoint()); - Logger.recordOutput("Flywheels/RightSetpointRPM", goal.getRightSetpoint()); - Logger.recordOutput("Flywheels/LeftRPM", inputs.leftVelocityRpm); - Logger.recordOutput("Flywheels/RightRPM", inputs.rightVelocityRpm); + Logger.recordOutput("Flywheels/GoalLeftRpm", goal.getLeftGoal()); + Logger.recordOutput("Flywheels/GoalRightRpm", goal.getRightGoal()); + } + + private void setGoal(Goal goal) { + if (goal == Goal.CHARACTERIZING || goal == Goal.STOP) { + wasClosedLoop = closedLoop; + closedLoop = false; + return; // Don't set a goal + } + // Enable close loop + leftProfile.setGoal(goal.getLeftGoal(), inputs.leftVelocityRpm); + rightProfile.setGoal(goal.getRightGoal(), inputs.rightVelocityRpm); + closedLoop = true; + this.goal = goal; } public void runCharacterizationVolts(double volts) { - goal = Goal.CHARACTERIZING; + setGoal(Goal.CHARACTERIZING); io.runCharacterizationLeftVolts(volts); io.runCharacterizationRightVolts(volts); } @@ -109,19 +150,20 @@ public double getCharacterizationVelocity() { return (inputs.leftVelocityRpm + inputs.rightVelocityRpm) / 2.0; } - @AutoLogOutput(key = "Shooter/AtSetpoint") - public boolean atSetpoint() { - return Math.abs(inputs.leftVelocityRpm - goal.leftSetpoint.getAsDouble()) - <= shooterTolerance.get() - && Math.abs(inputs.rightVelocityRpm - goal.rightSetpoint.getAsDouble()) + @AutoLogOutput(key = "Shooter/AtGoal") + public boolean atGoal() { + return Math.abs(leftProfile.getCurrentSetpoint() - goal.getLeftGoal()) <= shooterTolerance.get() + && Math.abs(rightProfile.getCurrentSetpoint() - goal.getRightGoal()) <= shooterTolerance.get(); } public Command shootCommand() { - return startEnd(() -> goal = Goal.SHOOTING, () -> goal = Goal.IDLE).withName("FlywheelsShoot"); + return startEnd(() -> setGoal(Goal.SHOOTING), () -> setGoal(Goal.IDLE)) + .withName("FlywheelsShoot"); } public Command intakeCommand() { - return startEnd(() -> goal = Goal.INTAKING, () -> goal = Goal.IDLE).withName("FlywheelsIntake"); + return startEnd(() -> setGoal(Goal.INTAKING), () -> setGoal(Goal.IDLE)) + .withName("FlywheelsIntake"); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSim.java index a8379415..61566c12 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSim.java @@ -18,9 +18,9 @@ public class FlywheelsIOSim implements FlywheelsIO { private final FlywheelSim leftSim = - new FlywheelSim(DCMotor.getKrakenX60Foc(1), config.reduction(), 0.00363458292); + new FlywheelSim(DCMotor.getKrakenX60Foc(1), flywheelConfig.reduction(), 0.00363458292); private final FlywheelSim rightSim = - new FlywheelSim(DCMotor.getKrakenX60Foc(1), config.reduction(), 0.00363458292); + new FlywheelSim(DCMotor.getKrakenX60Foc(1), flywheelConfig.reduction(), 0.00363458292); private final PIDController leftController = new PIDController(gains.kP(), gains.kI(), gains.kD()); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java index 76222901..d41c96a5 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java @@ -31,8 +31,8 @@ public class FlywheelsIOSparkFlex implements FlywheelsIO { public FlywheelsIOSparkFlex() { // Init Hardware - leftMotor = new CANSparkFlex(config.leftID(), CANSparkFlex.MotorType.kBrushless); - rightMotor = new CANSparkFlex(config.rightID(), CANSparkFlex.MotorType.kBrushless); + leftMotor = new CANSparkFlex(flywheelConfig.leftID(), CANSparkFlex.MotorType.kBrushless); + rightMotor = new CANSparkFlex(flywheelConfig.rightID(), CANSparkFlex.MotorType.kBrushless); leftEncoder = leftMotor.getEncoder(); rightEncoder = rightMotor.getEncoder(); @@ -72,15 +72,15 @@ public FlywheelsIOSparkFlex() { @Override public void updateInputs(FlywheelsIOInputs inputs) { inputs.leftPositionRads = - Units.rotationsToRadians(leftEncoder.getPosition()) / config.reduction(); - inputs.leftVelocityRpm = leftEncoder.getVelocity() / config.reduction(); + Units.rotationsToRadians(leftEncoder.getPosition()) / flywheelConfig.reduction(); + inputs.leftVelocityRpm = leftEncoder.getVelocity() / flywheelConfig.reduction(); inputs.leftAppliedVolts = leftMotor.getAppliedOutput(); inputs.leftOutputCurrent = leftMotor.getOutputCurrent(); inputs.leftTempCelsius = leftMotor.getMotorTemperature(); inputs.rightPositionRads = - Units.rotationsToRadians(rightEncoder.getPosition()) / config.reduction(); - inputs.rightVelocityRpm = rightEncoder.getVelocity() / config.reduction(); + Units.rotationsToRadians(rightEncoder.getPosition()) / flywheelConfig.reduction(); + inputs.rightVelocityRpm = rightEncoder.getVelocity() / flywheelConfig.reduction(); inputs.rightAppliedVolts = rightMotor.getAppliedOutput(); inputs.rightOutputCurrent = rightMotor.getOutputCurrent(); inputs.rightTempCelsius = rightMotor.getMotorTemperature(); @@ -95,13 +95,13 @@ public void runVolts(double leftVolts, double rightVolts) { @Override public void runVelocity(double leftRpm, double rightRpm) { leftController.setReference( - leftRpm * config.reduction(), + leftRpm * flywheelConfig.reduction(), CANSparkBase.ControlType.kVelocity, 0, ff.calculate(leftRpm), SparkPIDController.ArbFFUnits.kVoltage); rightController.setReference( - rightRpm * config.reduction(), + rightRpm * flywheelConfig.reduction(), CANSparkBase.ControlType.kVelocity, 0, ff.calculate(rightRpm), diff --git a/src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java b/src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java new file mode 100644 index 00000000..368c03e5 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java @@ -0,0 +1,72 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2024.util; + +import lombok.Getter; +import lombok.Setter; + +/** Ramps up and down to setpoint for velocity closed loop control */ +public class LinearProfile { + private double dv; + @Getter private final double period; + @Getter private double currentSetpoint = 0; + @Getter @Setter private double goal = 0; + + /** + * Creates a new LinearProfiler + * + * @param maxAcceleration The max ramp rate in velocity in rpm/sec + * @param period Period of control loop (0.02) + */ + public LinearProfile(double maxAcceleration, double period) { + this.period = period; + setMaxAcceleration(maxAcceleration); + } + + /** Set the max acceleration constraint in rpm/sec */ + public void setMaxAcceleration(double maxAcceleration) { + dv = maxAcceleration * period; + } + + /** + * Sets the target setpoint, starting from the current speed + * + * @param goal Target setpoint + * @param currentSpeed Current speed, to be used as the starting setpoint + */ + public void setGoal(double goal, double currentSpeed) { + this.goal = goal; + currentSetpoint = currentSpeed; + } + + /** Resets target setpoint and current setpoint */ + public void reset() { + currentSetpoint = 0; + goal = 0; + } + + /** + * Returns the current setpoint to send to motors + * + * @return Setpoint to send to motors + */ + public double calculateSetpoint() { + if (goal > currentSetpoint) { + currentSetpoint += dv; + if (currentSetpoint > goal) { + currentSetpoint = goal; + } + } else if (goal < currentSetpoint) { + currentSetpoint -= dv; + if (currentSetpoint < goal) { + currentSetpoint = goal; + } + } + return currentSetpoint; + } +} From c5082a9ae0222e04d7adc254c46b3ca8ead3b5f8 Mon Sep 17 00:00:00 2001 From: suryatho Date: Thu, 15 Feb 2024 00:24:03 -0500 Subject: [PATCH 2/6] Added LinearProfile.java Use LinearProfile for flywheels --- src/main/java/org/littletonrobotics/frc2024/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 0cbb9363..95715d10 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -211,7 +211,7 @@ private void configureButtonBindings() { new Trigger( () -> drive.isAutoAimGoalCompleted() - && flywheels.atSetpoint() + && flywheels.atGoal() && superstructure.atArmSetpoint()); readyToShootTrigger .whileTrue( From 03b5bbc0424ec8a40c281fdafafb27f14bfed090 Mon Sep 17 00:00:00 2001 From: suryatho Date: Thu, 15 Feb 2024 20:42:19 -0500 Subject: [PATCH 3/6] Use loopPeriodSecs --- .../littletonrobotics/frc2024/Constants.java | 2 +- .../flywheels/FlywheelConstants.java | 7 +++- .../subsystems/flywheels/Flywheels.java | 35 +++++++++---------- .../frc2024/util/LinearProfile.java | 2 +- 4 files changed, 25 insertions(+), 21 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java index 6bebedc2..e6f05ebb 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Constants.java +++ b/src/main/java/org/littletonrobotics/frc2024/Constants.java @@ -20,7 +20,7 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final int loopPeriodMs = 20; + public static final double loopPeriodSecs = 0.02; private static RobotType robotType = RobotType.DEVBOT; public static final boolean tuningMode = true; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java index 5197fcfb..f08f990e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java @@ -24,7 +24,12 @@ public class FlywheelConstants { case SIMBOT -> new Gains(0.05, 0.0, 0.0, 0.01, 0.00103, 0.0); }; - public record FlywheelConfig(int leftID, int rightID, double reduction, double maxAcclerationRpmPerSec, double toleranceRPM) {} + public record FlywheelConfig( + int leftID, + int rightID, + double reduction, + double maxAcclerationRpmPerSec, + double toleranceRPM) {} public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {} } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java index 55a20ad0..d3861de6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -15,6 +15,7 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; +import org.littletonrobotics.frc2024.Constants; import org.littletonrobotics.frc2024.util.LinearProfile; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; @@ -27,22 +28,23 @@ public class Flywheels extends SubsystemBase { private static final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheels/kS", gains.kS()); private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV()); private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA()); - private static LoggedTunableNumber shootingLeftRPM = + private static final LoggedTunableNumber shootingLeftRPM = new LoggedTunableNumber("Superstructure/ShootingLeftRPM", 6000.0); - private static LoggedTunableNumber shootingRightRPM = + private static final LoggedTunableNumber shootingRightRPM = new LoggedTunableNumber("Superstructure/ShootingRightRPM", 4000.0); - private static LoggedTunableNumber idleLeftRPM = - new LoggedTunableNumber("Superstructure/IdleLeftRPM", 200.0); - private static LoggedTunableNumber idleRightRPM = - new LoggedTunableNumber("Superstructure/IdleRightRPM", 200.0); - private static LoggedTunableNumber intakingLeftRPM = + private static final LoggedTunableNumber idleLeftRPM = + new LoggedTunableNumber("Superstructure/IdleLeftRPM", 1500.0); + private static final LoggedTunableNumber idleRightRPM = + new LoggedTunableNumber("Superstructure/IdleRightRPM", 1000.0); + private static final LoggedTunableNumber intakingLeftRPM = new LoggedTunableNumber("Superstructure/IntakingLeftRPM", -2000.0); - private static LoggedTunableNumber intakingRightRPM = + private static final LoggedTunableNumber intakingRightRPM = new LoggedTunableNumber("Superstructure/IntakingRightRPM", -2000.0); private static final LoggedTunableNumber shooterTolerance = new LoggedTunableNumber("Flywheels/ToleranceRPM", flywheelConfig.toleranceRPM()); private static final LoggedTunableNumber maxAcceleration = - new LoggedTunableNumber("Flywheels/MaxAccelerationRpmPerSec", flywheelConfig.maxAcclerationRpmPerSec()); + new LoggedTunableNumber( + "Flywheels/MaxAccelerationRpmPerSec", flywheelConfig.maxAcclerationRpmPerSec()); private final FlywheelsIO io; private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged(); @@ -77,10 +79,10 @@ private double getRightGoal() { public Flywheels(FlywheelsIO io) { this.io = io; - leftProfile = new LinearProfile(maxAcceleration.get(), 0.02); - rightProfile = new LinearProfile(maxAcceleration.get(), 0.02); + leftProfile = new LinearProfile(maxAcceleration.get(), Constants.loopPeriodSecs); + rightProfile = new LinearProfile(maxAcceleration.get(), Constants.loopPeriodSecs); - setDefaultCommand(runOnce(() -> goal = Goal.IDLE).withName("FlywheelsIdle")); + setDefaultCommand(runOnce(() -> setGoal(Goal.IDLE)).withName("FlywheelsIdle")); } @Override @@ -114,15 +116,12 @@ public void periodic() { // Run to setpoint if (closedLoop) { - double leftSetpoint = leftProfile.calculateSetpoint(); - double rightSetpoint = rightProfile.calculateSetpoint(); - io.runVelocity(leftSetpoint, rightSetpoint); - - Logger.recordOutput("Flywheels/SetpointLeftRpm", leftSetpoint); - Logger.recordOutput("Flywheels/SetpointRightRpm", rightSetpoint); + io.runVelocity(leftProfile.calculateSetpoint(), rightProfile.calculateSetpoint()); } Logger.recordOutput("Flywheels/Goal", goal); + Logger.recordOutput("Flywheels/SetpointLeftRpm", leftProfile.getCurrentSetpoint()); + Logger.recordOutput("Flywheels/SetpointRightRpm", rightProfile.getCurrentSetpoint()); Logger.recordOutput("Flywheels/GoalLeftRpm", goal.getLeftGoal()); Logger.recordOutput("Flywheels/GoalRightRpm", goal.getRightGoal()); } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java b/src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java index 368c03e5..a33271f6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java @@ -18,7 +18,7 @@ public class LinearProfile { @Getter @Setter private double goal = 0; /** - * Creates a new LinearProfiler + * Creates a new LinearProfile * * @param maxAcceleration The max ramp rate in velocity in rpm/sec * @param period Period of control loop (0.02) From 1a3cd19cffafe58bcaff529b8c8630009b8f0e99 Mon Sep 17 00:00:00 2001 From: suryatho Date: Thu, 15 Feb 2024 21:23:03 -0500 Subject: [PATCH 4/6] Reset goal periodically Check if at setpoint, not working when checking if equal within 1e-3, but works when checking withing 5 rpm --- .../subsystems/flywheels/Flywheels.java | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java index d3861de6..91ea4604 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -16,6 +16,7 @@ import lombok.Getter; import lombok.RequiredArgsConstructor; import org.littletonrobotics.frc2024.Constants; +import org.littletonrobotics.frc2024.util.EqualsUtil; import org.littletonrobotics.frc2024.util.LinearProfile; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; @@ -29,19 +30,17 @@ public class Flywheels extends SubsystemBase { private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV()); private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA()); private static final LoggedTunableNumber shootingLeftRPM = - new LoggedTunableNumber("Superstructure/ShootingLeftRPM", 6000.0); + new LoggedTunableNumber("Flywheels/ShootingLeftRPM", 6000.0); private static final LoggedTunableNumber shootingRightRPM = - new LoggedTunableNumber("Superstructure/ShootingRightRPM", 4000.0); + new LoggedTunableNumber("Flywheels/ShootingRightRPM", 4000.0); private static final LoggedTunableNumber idleLeftRPM = - new LoggedTunableNumber("Superstructure/IdleLeftRPM", 1500.0); + new LoggedTunableNumber("Flywheels/IdleLeftRPM", 1500.0); private static final LoggedTunableNumber idleRightRPM = - new LoggedTunableNumber("Superstructure/IdleRightRPM", 1000.0); + new LoggedTunableNumber("Flywheels/IdleRightRPM", 1000.0); private static final LoggedTunableNumber intakingLeftRPM = - new LoggedTunableNumber("Superstructure/IntakingLeftRPM", -2000.0); + new LoggedTunableNumber("Flywheels/IntakingLeftRPM", -2000.0); private static final LoggedTunableNumber intakingRightRPM = - new LoggedTunableNumber("Superstructure/IntakingRightRPM", -2000.0); - private static final LoggedTunableNumber shooterTolerance = - new LoggedTunableNumber("Flywheels/ToleranceRPM", flywheelConfig.toleranceRPM()); + new LoggedTunableNumber("Flywheels/IntakingRightRPM", -2000.0); private static final LoggedTunableNumber maxAcceleration = new LoggedTunableNumber( "Flywheels/MaxAccelerationRpmPerSec", flywheelConfig.maxAcclerationRpmPerSec()); @@ -116,6 +115,9 @@ public void periodic() { // Run to setpoint if (closedLoop) { + // Update goals + leftProfile.setGoal(goal.getLeftGoal()); + rightProfile.setGoal(goal.getRightGoal()); io.runVelocity(leftProfile.calculateSetpoint(), rightProfile.calculateSetpoint()); } @@ -149,11 +151,10 @@ public double getCharacterizationVelocity() { return (inputs.leftVelocityRpm + inputs.rightVelocityRpm) / 2.0; } - @AutoLogOutput(key = "Shooter/AtGoal") + @AutoLogOutput(key = "Flywheels/AtGoal") public boolean atGoal() { - return Math.abs(leftProfile.getCurrentSetpoint() - goal.getLeftGoal()) <= shooterTolerance.get() - && Math.abs(rightProfile.getCurrentSetpoint() - goal.getRightGoal()) - <= shooterTolerance.get(); + return EqualsUtil.epsilonEquals(leftProfile.getCurrentSetpoint(), goal.getLeftGoal(), 5) + && EqualsUtil.epsilonEquals(rightProfile.getCurrentSetpoint(), goal.getRightGoal(), 5); } public Command shootCommand() { From dcdd6bd928ae1eb90c7a14e402fce79b442cab0a Mon Sep 17 00:00:00 2001 From: suryatho Date: Thu, 15 Feb 2024 23:10:23 -0500 Subject: [PATCH 5/6] Only set goal if different from current goal atGoal checks for setpoint equal to goal --- .../subsystems/flywheels/Flywheels.java | 18 +++++++++++------- .../frc2024/util/LinearProfile.java | 3 +++ 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java index 91ea4604..b378155a 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -16,7 +16,6 @@ import lombok.Getter; import lombok.RequiredArgsConstructor; import org.littletonrobotics.frc2024.Constants; -import org.littletonrobotics.frc2024.util.EqualsUtil; import org.littletonrobotics.frc2024.util.LinearProfile; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; @@ -81,7 +80,7 @@ public Flywheels(FlywheelsIO io) { leftProfile = new LinearProfile(maxAcceleration.get(), Constants.loopPeriodSecs); rightProfile = new LinearProfile(maxAcceleration.get(), Constants.loopPeriodSecs); - setDefaultCommand(runOnce(() -> setGoal(Goal.IDLE)).withName("FlywheelsIdle")); + setDefaultCommand(runOnce(() -> setGoal(Goal.IDLE)).withName("Flywheels Idle")); } @Override @@ -134,10 +133,15 @@ private void setGoal(Goal goal) { closedLoop = false; return; // Don't set a goal } + // If not already controlling to requested goal + // set closed loop false + closedLoop = this.goal == goal; // Enable close loop - leftProfile.setGoal(goal.getLeftGoal(), inputs.leftVelocityRpm); - rightProfile.setGoal(goal.getRightGoal(), inputs.rightVelocityRpm); - closedLoop = true; + if (!closedLoop) { + leftProfile.setGoal(goal.getLeftGoal(), inputs.leftVelocityRpm); + rightProfile.setGoal(goal.getRightGoal(), inputs.rightVelocityRpm); + closedLoop = true; + } this.goal = goal; } @@ -153,8 +157,8 @@ public double getCharacterizationVelocity() { @AutoLogOutput(key = "Flywheels/AtGoal") public boolean atGoal() { - return EqualsUtil.epsilonEquals(leftProfile.getCurrentSetpoint(), goal.getLeftGoal(), 5) - && EqualsUtil.epsilonEquals(rightProfile.getCurrentSetpoint(), goal.getRightGoal(), 5); + return leftProfile.getCurrentSetpoint() == goal.getLeftGoal() + && rightProfile.getCurrentSetpoint() == goal.getRightGoal(); } public Command shootCommand() { diff --git a/src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java b/src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java index a33271f6..427dfc5a 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/LinearProfile.java @@ -56,6 +56,9 @@ public void reset() { * @return Setpoint to send to motors */ public double calculateSetpoint() { + if (EqualsUtil.epsilonEquals(goal, currentSetpoint)) { + return currentSetpoint; + } if (goal > currentSetpoint) { currentSetpoint += dv; if (currentSetpoint > goal) { From d253093c5bd2679679a18a75d4499b000df93166 Mon Sep 17 00:00:00 2001 From: suryatho Date: Fri, 16 Feb 2024 19:53:50 -0500 Subject: [PATCH 6/6] Merge into main --- .../frc2024/RobotContainer.java | 12 +++---- .../subsystems/flywheels/Flywheels.java | 32 +++++++++---------- 2 files changed, 21 insertions(+), 23 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index f2708f97..309648db 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -217,16 +217,13 @@ private void configureButtonBindings() { .a() .whileTrue( Commands.startEnd(drive::setAutoAimGoal, drive::clearAutoAimGoal) - .alongWith(superstructure.aim(), flywheels.shoot()) + .alongWith(superstructure.aim(), flywheels.shootCommand()) .withName("Aim")); // Shoot Trigger readyToShoot = new Trigger( - () -> - drive.isAutoAimGoalCompleted() - && flywheels.atGoal() - && superstructure.atArmSetpoint()); - readyToShootTrigger + () -> drive.isAutoAimGoalCompleted() && flywheels.atGoal() && superstructure.atGoal()); + readyToShoot .whileTrue( Commands.run( () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0))) @@ -240,7 +237,8 @@ private void configureButtonBindings() { Commands.parallel( Commands.waitSeconds(0.5), Commands.waitUntil(controller.rightTrigger().negate())) - .deadlineWith(rollers.feedShooter(), superstructure.aim(), flywheels.shoot())); + .deadlineWith( + rollers.feedShooter(), superstructure.aim(), flywheels.shootCommand())); // Intake Floor controller .leftTrigger() diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java index da4f04f6..c5fe53ac 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -28,22 +28,22 @@ public class Flywheels extends SubsystemBase { private static final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheels/kS", gains.kS()); private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV()); private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA()); - private static final LoggedTunableNumber shootingLeftRPM = - new LoggedTunableNumber("Flywheels/ShootingLeftRPM", 6000.0); - private static final LoggedTunableNumber shootingRightRPM = - new LoggedTunableNumber("Flywheels/ShootingRightRPM", 4000.0); - private static final LoggedTunableNumber idleLeftRPM = - new LoggedTunableNumber("Flywheels/IdleLeftRPM", 1500.0); - private static final LoggedTunableNumber idleRightRPM = - new LoggedTunableNumber("Flywheels/IdleRightRPM", 1000.0); - private static final LoggedTunableNumber intakingLeftRPM = - new LoggedTunableNumber("Flywheels/IntakingLeftRPM", -2000.0); - private static final LoggedTunableNumber intakingRightRPM = - new LoggedTunableNumber("Flywheels/IntakingRightRPM", -2000.0); + private static final LoggedTunableNumber shootingLeftRpm = + new LoggedTunableNumber("Flywheels/ShootingLeftRpm", 6000.0); + private static final LoggedTunableNumber shootingRightRpm = + new LoggedTunableNumber("Flywheels/ShootingRightRpm", 4000.0); + private static final LoggedTunableNumber idleLeftRpm = + new LoggedTunableNumber("Flywheels/IdleLeftRpm", 1500.0); + private static final LoggedTunableNumber idleRightRpm = + new LoggedTunableNumber("Flywheels/IdleRightRpm", 1000.0); + private static final LoggedTunableNumber intakingRpm = + new LoggedTunableNumber("Flywheels/IntakingRpm", -2000.0); + private static final LoggedTunableNumber ejectingRpm = + new LoggedTunableNumber("Flywheels/EjectingRpm", 1000.0); private static final LoggedTunableNumber maxAcceleration = new LoggedTunableNumber( "Flywheels/MaxAccelerationRpmPerSec", flywheelConfig.maxAcclerationRpmPerSec()); - + private final FlywheelsIO io; private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged(); @@ -54,6 +54,7 @@ public class Flywheels extends SubsystemBase { @RequiredArgsConstructor public enum Goal { + STOP(() -> 0, () -> 0), IDLE(idleLeftRpm, idleRightRpm), SHOOT(shootingLeftRpm, shootingRightRpm), INTAKE(intakingRpm, intakingRpm), @@ -162,12 +163,11 @@ public boolean atGoal() { } public Command shootCommand() { - return startEnd(() -> setGoal(Goal.SHOOTING), () -> setGoal(Goal.IDLE)) - .withName("FlywheelsShoot"); + return startEnd(() -> setGoal(Goal.SHOOT), () -> setGoal(Goal.IDLE)).withName("FlywheelsShoot"); } public Command intakeCommand() { - return startEnd(() -> setGoal(Goal.INTAKING), () -> setGoal(Goal.IDLE)) + return startEnd(() -> setGoal(Goal.INTAKE), () -> setGoal(Goal.IDLE)) .withName("FlywheelsIntake"); } }