diff --git a/simgui-ds.json b/simgui-ds.json index 4a63cc1..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ @@ -93,5 +88,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/src/main/java/frc/lib/Rebuilt2026/HubShiftUtil.java b/src/main/java/frc/lib/Rebuilt2026/HubShiftUtil.java new file mode 100644 index 0000000..765a471 --- /dev/null +++ b/src/main/java/frc/lib/Rebuilt2026/HubShiftUtil.java @@ -0,0 +1,206 @@ +package frc.lib.Rebuilt2026; + +// Copyright (c) 2025-2026 Littleton Robotics +// 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. + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import java.util.Optional; +import java.util.function.Supplier; +import lombok.Setter; + +public class HubShiftUtil { + public enum ShiftEnum { + TRANSITION, + SHIFT1, + SHIFT2, + SHIFT3, + SHIFT4, + ENDGAME, + AUTO, + DISABLED; + } + + public record ShiftInfo( + ShiftEnum currentShift, double elapsedTime, double remainingTime, boolean active) {} + + private static Timer shiftTimer = new Timer(); + private static final ShiftEnum[] shiftsEnums = ShiftEnum.values(); + + private static final double[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; + private static final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; + + private static final double minFuelCountDelay = 1.0; + private static final double maxFuelCountDelay = 2.0; + private static final double shiftEndFuelCountExtension = 3.0; + private static final double minTimeOfFlight = 0; + private static final double maxTimeOfFlight = 0; + private static final double approachingActiveFudge = -1 * (minTimeOfFlight + minFuelCountDelay); + private static final double endingActiveFudge = + shiftEndFuelCountExtension + -1 * (maxTimeOfFlight + maxFuelCountDelay); + + public static final double autoEndTime = 20.0; + public static final double teleopDuration = 140.0; + private static final boolean[] activeSchedule = {true, true, false, true, false, true}; + private static final boolean[] inactiveSchedule = {true, false, true, false, true, true}; + private static final double timeResetThreshold = 3.0; + private static double shiftTimerOffset = 0.0; + @Setter private static Supplier> allianceWinOverride = () -> Optional.empty(); + + public static Optional getAllianceWinOverride() { + return allianceWinOverride.get(); + } + + public static Alliance getFirstActiveAlliance() { + var alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + + // Return override value + var winOverride = getAllianceWinOverride(); + if (!winOverride.isEmpty()) { + return winOverride.get() + ? (alliance == Alliance.Blue ? Alliance.Red : Alliance.Blue) + : (alliance == Alliance.Blue ? Alliance.Blue : Alliance.Red); + } + + // Return FMS value + String message = DriverStation.getGameSpecificMessage(); + if (message.length() > 0) { + char character = message.charAt(0); + if (character == 'R') { + return Alliance.Blue; + } else if (character == 'B') { + return Alliance.Red; + } + } + + // Return default value + return alliance == Alliance.Blue ? Alliance.Red : Alliance.Blue; + } + + /** Starts the timer at the begining of teleop. */ + public static void initialize() { + shiftTimerOffset = 0; + shiftTimer.restart(); + } + + private static boolean[] getSchedule() { + boolean[] currentSchedule; + Alliance startAlliance = getFirstActiveAlliance(); + currentSchedule = + startAlliance == DriverStation.getAlliance().orElse(Alliance.Blue) + ? activeSchedule + : inactiveSchedule; + return currentSchedule; + } + + private static ShiftInfo getShiftInfo( + boolean[] currentSchedule, double[] shiftStartTimes, double[] shiftEndTimes) { + double timerValue = shiftTimer.get(); + double currentTime = timerValue - shiftTimerOffset; + double stateTimeElapsed = currentTime; + double stateTimeRemaining = 0.0; + boolean active = false; + ShiftEnum currentShift = ShiftEnum.DISABLED; + double fieldTeleopTime = 140.0 - DriverStation.getMatchTime(); + + if (DriverStation.isAutonomousEnabled()) { + stateTimeElapsed = currentTime; + stateTimeRemaining = autoEndTime - currentTime; + active = true; + currentShift = ShiftEnum.AUTO; + } else if (DriverStation.isEnabled()) { + // Adjust the current offset if the time difference above the theshold + if (Math.abs(fieldTeleopTime - currentTime) >= timeResetThreshold + && fieldTeleopTime <= 135 + && DriverStation.isFMSAttached()) { + shiftTimerOffset += currentTime - fieldTeleopTime; + currentTime = timerValue + shiftTimerOffset; + } + int currentShiftIndex = -1; + for (int i = 0; i < shiftStartTimes.length; i++) { + if (currentTime >= shiftStartTimes[i] && currentTime < shiftEndTimes[i]) { + currentShiftIndex = i; + break; + } + } + if (currentShiftIndex < 0) { + // After last shift, so assume endgame + currentShiftIndex = shiftStartTimes.length - 1; + } + + // Calculate elapsed and remaining time in the current shift, ignoring combined shifts + stateTimeElapsed = currentTime - shiftStartTimes[currentShiftIndex]; + stateTimeRemaining = shiftEndTimes[currentShiftIndex] - currentTime; + + // If the state is the same as the last shift, combine the elapsed time + if (currentShiftIndex > 0) { + if (currentSchedule[currentShiftIndex] == currentSchedule[currentShiftIndex - 1]) { + stateTimeElapsed = currentTime - shiftStartTimes[currentShiftIndex - 1]; + } + } + + // If the state is the same as the next shift, combine the remaining time + if (currentShiftIndex < shiftEndTimes.length - 1) { + if (currentSchedule[currentShiftIndex] == currentSchedule[currentShiftIndex + 1]) { + stateTimeRemaining = shiftEndTimes[currentShiftIndex + 1] - currentTime; + } + } + + active = currentSchedule[currentShiftIndex]; + currentShift = shiftsEnums[currentShiftIndex]; + } + ShiftInfo shiftInfo = new ShiftInfo(currentShift, stateTimeElapsed, stateTimeRemaining, active); + return shiftInfo; + } + + public static ShiftInfo getOfficialShiftInfo() { + return getShiftInfo(getSchedule(), shiftStartTimes, shiftEndTimes); + } + + public static ShiftInfo getShiftedShiftInfo() { + boolean[] shiftSchedule = getSchedule(); + // Starting active + if (shiftSchedule[1] == true) { + double[] shiftedShiftStartTimes = { + 0.0, + 10.0, + 35.0 + endingActiveFudge, + 60.0 + approachingActiveFudge, + 85.0 + endingActiveFudge, + 110.0 + approachingActiveFudge + }; + double[] shiftedShiftEndTimes = { + 10.0, + 35.0 + endingActiveFudge, + 60.0 + approachingActiveFudge, + 85.0 + endingActiveFudge, + 110.0 + approachingActiveFudge, + 140.0 + }; + return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); + } + double[] shiftedShiftStartTimes = { + 0.0, + 10.0 + endingActiveFudge, + 35.0 + approachingActiveFudge, + 60.0 + endingActiveFudge, + 85.0 + approachingActiveFudge, + 110.0 + }; + double[] shiftedShiftEndTimes = { + 10.0 + endingActiveFudge, + 35.0 + approachingActiveFudge, + 60.0 + endingActiveFudge, + 85.0 + approachingActiveFudge, + 110.0, + 140.0 + }; + return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); + // } + } +} diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFXSim.java b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFXSim.java index 2c5a75c..c1bc526 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFXSim.java +++ b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFXSim.java @@ -17,26 +17,51 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.units.AngularAccelerationUnit; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.RobotController; import frc.lib.W8.util.Device.CAN; /** * Abstraction for a simulated CTRE TalonFX motor implementing the {@link MotorIOSim} interface. * Wraps motor setup, control modes, telemetry polling, and error handling. + * + *

Because Phoenix TalonFX simulation does not simulate closed-loop PID control, this class + * implements manual PID controllers for position and velocity modes. When runPosition() or + * runVelocity() are called, the appropriate PID controller is enabled and its output voltage is + * applied to the simulated motor via setInputVoltage(). */ public class MotorIOTalonFXSim extends MotorIOTalonFX implements MotorIOSim { + // Simulation configuration + private static final double POSITION_KP = 50.0; // Position control proportional gain + private static final double POSITION_KD = 5.0; // Position control derivative gain + private static final double VELOCITY_KP = 0.1; // Velocity control proportional gain + private static final double VELOCITY_KD = 0.0; // Velocity control derivative gain + private double rotorToSensorRatio; private double sensorToMechanismRatio; private TalonFXSimState simState; + // Manual PID control for simulation + private final PIDController positionController = new PIDController(POSITION_KP, 0, POSITION_KD); + private final PIDController velocityController = new PIDController(VELOCITY_KP, 0, VELOCITY_KD); + + // Track which control mode is active for manual PID + private boolean positionClosedLoop = false; + private boolean velocityClosedLoop = false; + private double appliedVoltage = 0.0; + /** * Constructs and initializes a TalonFX motor simulation. * @@ -84,8 +109,68 @@ public void setEncoderPosition(Angle position) { super.setEncoderPosition(position.times(rotorToSensorRatio * sensorToMechanismRatio)); } + /** + * Runs the motor to a target position using manual PID control (since Phoenix sim doesn't + * simulate closed-loop control). + */ + @Override + public void runPosition( + Angle position, + AngularVelocity cruiseVelocity, + AngularAcceleration acceleration, + Velocity maxJerk, + PIDSlot slot) { + this.goalPosition = position; + double newSetpoint = position.in(Rotations); + + // Always reset PID controller when a new position command is issued + // This ensures fresh control for each new setpoint + positionController.reset(); + + positionClosedLoop = true; + velocityClosedLoop = false; + positionController.setSetpoint(newSetpoint); + } + + /** + * Runs the motor at a target velocity using manual PID control (since Phoenix sim doesn't + * simulate closed-loop control). + */ + @Override + public void runVelocity( + AngularVelocity velocity, AngularAcceleration acceleration, PIDSlot slot) { + double newSetpoint = velocity.in(RotationsPerSecond); + + // Always reset PID controller when a new velocity command is issued + // This ensures fresh control for each new setpoint + velocityController.reset(); + + velocityClosedLoop = true; + positionClosedLoop = false; + velocityController.setSetpoint(newSetpoint); + } + @Override public void updateInputs(MotorInputs inputs) { + // Run manual PID control for closed-loop modes since Phoenix sim doesn't simulate firmware + if (positionClosedLoop) { + double currentPosition = super.position.getValue().in(Rotations); + double pidOutput = positionController.calculate(currentPosition); + appliedVoltage = MathUtil.clamp(pidOutput, -12.0, 12.0); + // Apply calculated voltage via VoltageOut control + super.motor.setControl(super.voltageControl.withOutput(appliedVoltage)); + } else if (velocityClosedLoop) { + double currentVelocity = super.velocity.getValue().in(RotationsPerSecond); + double pidOutput = velocityController.calculate(currentVelocity); + appliedVoltage = MathUtil.clamp(pidOutput, -12.0, 12.0); + // Apply calculated voltage via VoltageOut control + super.motor.setControl(super.voltageControl.withOutput(appliedVoltage)); + } else { + appliedVoltage = 0.0; + positionController.reset(); + velocityController.reset(); + } + inputs.connected = BaseStatusSignal.refreshAll( super.position, @@ -103,7 +188,8 @@ public void updateInputs(MotorInputs inputs) { inputs.position = super.position.getValue(); inputs.velocity = super.velocity.getValue(); - inputs.appliedVoltage = simState.getMotorVoltageMeasure(); + // Use the voltage we calculated, not what the simState reports (which may not have updated yet) + inputs.appliedVoltage = Volts.of(appliedVoltage); inputs.supplyCurrent = simState.getSupplyCurrentMeasure(); inputs.torqueCurrent = simState.getTorqueCurrentMeasure(); inputs.temperature = super.temperature.getValue(); diff --git a/src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismSim.java b/src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismSim.java index 64404d7..4c2fc34 100644 --- a/src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismSim.java +++ b/src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismSim.java @@ -76,6 +76,7 @@ public RotaryMechanismSim( @Override public void periodic() { super.periodic(); + io.updateInputs(inputs); Time currentTime = Seconds.of(Timer.getTimestamp()); double deltaTime = currentTime.minus(lastTime).in(Seconds); @@ -103,7 +104,6 @@ public void periodic() { Logger.processInputs(encoderSim.getName(), absoluteEncoderInputs); }); - io.updateInputs(inputs); Logger.processInputs(io.getName(), inputs); } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d58b34a..c3fd387 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -286,6 +286,8 @@ public final class ShooterConstants { public static final double DEFAULT_SPEED_RPM = (1.0); public static final double FLYWHEEL_VELOCITY_TOLERANCE = 1.0; + public static final double SIM_MULTIPLIER = (simMode == Mode.SIM) ? 1.0 / 57.0 : 1.0; + // Hood Constants public static final double HEIGHT_DIFFERENCE = 1.295; // Meters between flywheel center and top of hub opening @@ -459,7 +461,7 @@ public static TalonFXConfiguration getFXConfig() { } else { config.Feedback.SensorToMechanismRatio = (50.0 / 1.0); - config.Slot0 = new Slot0Configs().withKP(30.0).withKI(0.0).withKD(5.0); + config.Slot0 = new Slot0Configs().withKP(30.0).withKI(0.0).withKD(2.0).withKS(8.0); } config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); @@ -755,7 +757,7 @@ public static TalonFXConfiguration getFXConfig() { } else { config.Slot0 = - new Slot0Configs().withKP(100.0).withKI(0.0).withKD(0).withKS(0.07).withKV(0.1); + new Slot0Configs().withKP(500.0).withKI(0.0).withKD(0).withKS(0.07).withKV(0.1); } config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 112672b..8f8108d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,11 +23,14 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.lib.Rebuilt2026.FuelSim; +import frc.lib.Rebuilt2026.HubShiftUtil; import frc.robot.generated.TunerConstants; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -107,6 +110,7 @@ public Robot() { throw new RuntimeException( "You are using an unsupported swerve configuration, which this template does not support without manual customization. The 2025 release of Phoenix supports some swerve configurations which were not available during 2025 beta testing, preventing any development and support from the AdvantageKit developers."); } + HubShiftUtil.initialize(); } // Instantiate our RobotContainer. This will perform all our button bindings, @@ -117,6 +121,19 @@ public Robot() { /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { + // Publish match time + SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); + + // Update from HubShiftUtil + SmartDashboard.putString( + "Shifts/Remaining Shift Time", + String.format("%.1f", Math.max(HubShiftUtil.getShiftedShiftInfo().remainingTime(), 0.0))); + SmartDashboard.putBoolean("Shifts/Shift Active", HubShiftUtil.getShiftedShiftInfo().active()); + SmartDashboard.putString( + "Shifts/Game State", HubShiftUtil.getShiftedShiftInfo().currentShift().toString()); + SmartDashboard.putBoolean( + "Shifts/Active First?", + DriverStation.getAlliance().orElse(Alliance.Blue) == HubShiftUtil.getFirstActiveAlliance()); // Optionally switch the thread to high priority to improve loop // timing (see the template project documentation for details) // Threads.setCurrentThreadPriority(true, 99); @@ -152,6 +169,7 @@ public void autonomousInit() { // if (autonomousCommand != null) { // autonomousCommand.schedule(); // } + HubShiftUtil.initialize(); } /** This function is called periodically during autonomous. */ @@ -168,6 +186,7 @@ public void teleopInit() { if (autonomousCommand != null) { autonomousCommand.cancel(); } + HubShiftUtil.initialize(); } /** This function is called periodically during operator control. */ @@ -179,6 +198,7 @@ public void teleopPeriodic() {} public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + HubShiftUtil.initialize(); } /** This function is called periodically during test mode. */ @@ -213,6 +233,7 @@ public void simulationInit() { fuelSim.enableAirResistance(); // an additional drag force will be applied to fuel in physics // update step + HubShiftUtil.initialize(); } /** This function is called periodically whilst in simulation. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9cc0c4d..cf9c4c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,10 +20,12 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.lib.Rebuilt2026.HubShiftUtil; import frc.lib.W8.io.motor.*; import frc.lib.W8.io.vision.VisionIOPhotonVision; import frc.lib.W8.io.vision.VisionIOPhotonVisionSim; @@ -57,6 +59,7 @@ import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; import java.util.Optional; +import java.util.function.Supplier; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -355,31 +358,16 @@ private void configureButtonBindings() { // // Switch to X pattern when X button is pressed // controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - // controller - // .x() - // .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); - - // // Commands for sim testing! - // controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); + // Shoot + controller.leftTrigger().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(55))); + controller.leftTrigger().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); - // controller - // .leftTrigger() - // .onTrue( - // Commands.runOnce( - // () -> { - // Robot.fuelSim.clearFuel(); - // Robot.fuelSim.spawnStartingFuel(); - // intake.simBalls = 0; - // })); - - // Feed and Shoot + // Feed controller .leftBumper() .whileTrue( Commands.parallel( - shooter.score(), - hopper.runSpindexer(RotationsPerSecond.of(15)), - intake.intake())); + shooter.score(), hopper.runSpindexer(RotationsPerSecond.of(15)), intake.intake())); controller .leftBumper() .onFalse( @@ -395,6 +383,8 @@ private void configureButtonBindings() { // Align // controller.a().onTrue(getAutonomousCommand()); // controller.a().onFalse(getAutonomousCommand()); + // controller.a().onTrue(getAutonomousCommand()); + // controller.a().onFalse(getAutonomousCommand()); // Jostle controller.b().onTrue(intake.jostleIntake()); @@ -406,21 +396,31 @@ private void configureButtonBindings() { controller.x().whileTrue(intake.setPivotAngle(IntakePivotConstants.STOW_ANGLE)); // Climber Raise/Lower - // controller.povUp().whileTrue(climber.raiseClimber()); - // controller.povUp().onFalse(climber.stopClimber()); - // controller.povDown().whileTrue(climber.lowerClimber()); - // controller.povDown().onFalse(climber.stopClimber()); - - // controller.povLeft().onTrue(intake.zeroEncoder()); // Testing Commands controller.povUp().onTrue(shooter.setHoodAngle(2.3)); controller.povRight().onTrue(shooter.setHoodAngle(8)); controller.povDown().onTrue(shooter.setHoodAngle(9.8)); - // controller.povLeft().onTrue(shooter.setAngleForDistance(2.0)); + // controller.povDown().onTrue(shooter.setAngleForDistance(Meters.of(1.0))); + // controller.povRight().onTrue(shooter.setAngleForDistance(Meters.of(2.0))); + // controller.povUp().onTrue(shooter.setAngleForDistance(Meters.of(5.0))); + + HubShiftUtil.setAllianceWinOverride( + () -> { + if (loseauto.get()) { + return Optional.of(false); + } + if (winauto.get()) { + return Optional.of(true); + } + return Optional.empty(); + }); } + Supplier loseauto = () -> SmartDashboard.getBoolean("Auto Lost", false); + Supplier winauto = () -> SmartDashboard.getBoolean("Auto Won", false); + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 31926ec..2c39035 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; @@ -133,7 +132,7 @@ public Command setHoodAngle(double angleDegrees) { () -> { System.out.println("Command"); _hood.runPosition( - Angle.ofBaseUnits(angleDegrees, Degrees), + Angle.ofBaseUnits(angleDegrees * ShooterConstants.SIM_MULTIPLIER, Degrees), ShooterRotaryConstants.CRUISE_VELOCITY, ShooterRotaryConstants.ACCELERATION, ShooterRotaryConstants.JERK, @@ -188,11 +187,19 @@ public Command runTower(AngularVelocity velocity) { } public Command score() { - return Commands.run(() -> {runFeeder(RotationsPerSecond.of(30)); setFlywheelVelocity(RotationsPerSecond.of(67));}); + return Commands.run( + () -> { + runFeeder(RotationsPerSecond.of(30)); + setFlywheelVelocity(RotationsPerSecond.of(67)); + }); } public Command stopScore() { - return Commands.run(() -> {runFeeder(RotationsPerSecond.of(0)); setFlywheelVelocity(RotationsPerSecond.of(0));}); + return Commands.run( + () -> { + runFeeder(RotationsPerSecond.of(0)); + setFlywheelVelocity(RotationsPerSecond.of(0)); + }); } public void simShoot() { @@ -277,7 +284,7 @@ public Command setAngleForDistance(Distance distance) { () -> { System.out.println("Command"); _hood.runPosition( - Angle.ofBaseUnits(angle, Degrees), + Angle.ofBaseUnits(angle * ShooterConstants.SIM_MULTIPLIER, Degrees), ShooterRotaryConstants.CRUISE_VELOCITY, ShooterRotaryConstants.ACCELERATION, ShooterRotaryConstants.JERK,