diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 2d6fe9ff..67d642d5 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -56,6 +56,7 @@ public static class Drivetrain { public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; } public static class Indexer { + // TODO: fix these constants public static final int INTAKE_ID = 0; public static final int FEEDER_ID = 0; public static final double FEEDER_KP = 0.0; @@ -63,8 +64,8 @@ public static class Indexer { public static final double FEEDER_KD = 0.0; public static final double FEEDER_KS = 0.0; public static final double FEEDER_KV = 0.0; - public static final double FEEDER_GEAR_RATIO = 0.0; - public static final double INTAKE_GEAR_RATIO = 0.0; + public static final double FEEDER_GEAR_RATIO = 1.0; + public static final double INTAKE_GEAR_RATIO = 1.0; } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java index 5ed351bc..93060ac9 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java @@ -12,7 +12,7 @@ class IndexerIOInputs { public double feederVoltage; public double feederCurrentAmps; public double feederVelocityRPM; - public boolean noteInIntake; + public boolean noteInIntake = false; } /** diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java index 347e371c..1252d170 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java @@ -14,7 +14,7 @@ public class IndexerIOSim implements IndexerIO { private final DCMotorSim intakeSim = new DCMotorSim(DCMotor.getNEO(1), INTAKE_GEAR_RATIO,0.025); private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO,0.025); - private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break"); +// private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break"); private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD); @Override @@ -27,10 +27,10 @@ public void updateInputs(IndexerIOInputs inputs) { inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps(); // inputs.feederVoltage = feederSim.getBusVoltage() * feederSim.getAppliedOutput(); inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM(); - inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get(); +// inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get(); // this is a very funny line of code, and absolutely does not belong here, but I don't know how to do this otherwise - feederSim.setState(feederSim.getAngularPositionRad(), feederSimPID.calculate(feederSim.getAngularVelocityRadPerSec())); + feederSim.setState(feederSim.getAngularPositionRad(), feederSimPID.calculate(feederSim.getAngularVelocityRadPerSec())); }