Skip to content

Commit

Permalink
.
Browse files Browse the repository at this point in the history
  • Loading branch information
Strflightmight09 committed Oct 6, 2024
1 parent 3013589 commit a49d43c
Showing 1 changed file with 8 additions and 7 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -10,14 +10,14 @@
import java.util.function.DoubleSupplier;

public class GearRatioCalculationCommand extends Command {
private final LoggedDashboardNumber MOVEMENT_VOLTAGE;
private final LoggedDashboardBoolean SHOULD_MOVE_CLOCKWISE;

private final DoubleSupplier rotorPositionSupplier;
private final DoubleSupplier encoderPositionSupplier;
private final DoubleConsumer runGearRatioCalculation;
private final String subsystemName;

private final LoggedDashboardNumber movementVoltage;
private final LoggedDashboardBoolean shouldMoveClockwise;

private double startingRotorPosition;
private double startingEncoderPosition;
private double gearRatio;
Expand All @@ -28,9 +28,10 @@ public GearRatioCalculationCommand(DoubleSupplier rotorPositionSupplier, DoubleS
this.runGearRatioCalculation = runGearRatioCalculation;
this.subsystemName = requirement.getName();

this.movementVoltage = new LoggedDashboardNumber("GearRatioCalculation/" + this.subsystemName + "/Voltage", 1);
this.shouldMoveClockwise = new LoggedDashboardBoolean("GearRatioCalculation/" + this.subsystemName + "/ShouldMoveClockwise", false);

addRequirements(requirement);
MOVEMENT_VOLTAGE = new LoggedDashboardNumber("GearRatioCalculation/" + subsystemName + "/Voltage", 1);
SHOULD_MOVE_CLOCKWISE = new LoggedDashboardBoolean("GearRatioCalculation/" + subsystemName + "/ShouldMoveClockwise", false);
}

@Override
Expand All @@ -41,7 +42,7 @@ public void initialize() {

@Override
public void execute() {
runGearRatioCalculation.accept(MOVEMENT_VOLTAGE.get() * getRotationDirection());
runGearRatioCalculation.accept(movementVoltage.get() * getRotationDirection());
gearRatio = calculateGearRatio();

Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/RotorDistance", getRotorDistance());
Expand Down Expand Up @@ -69,7 +70,7 @@ private double getEncoderDistance() {
}

private int getRotationDirection() {
return SHOULD_MOVE_CLOCKWISE.get() ? -1 : 1;
return shouldMoveClockwise.get() ? -1 : 1;
}

private void printResult() {
Expand Down

0 comments on commit a49d43c

Please sign in to comment.