diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 0b274f6..a0e706a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -152,19 +152,15 @@ public class Shooter extends SubsystemBase { switch (mode) { case Shooting: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); - if(state.motor1Velocity.in(RotationsPerSecond) > 30){ - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_FORWARD_VELOCITY.get())); - }else{ - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get())); - } + io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); break; case Ready: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get())); + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); break; case NotReady: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get())); + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); break; } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 1a20a91..55e533c 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -25,8 +25,8 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -35); public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", -10); - public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", -10); - public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10); + public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD Velocity", -10); + public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse Velocity", 10); // Tolerances diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index efd97b0..463cb4b 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -21,11 +21,12 @@ public interface ShooterIO { AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0); AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0); - AngularVelocity indexerTargetVelocity = RotationsPerSecond.of(0); + double indexerTargetOutput = 0; + AngularVelocity motor1Velocity = RotationsPerSecond.of(0); AngularVelocity motor2Velocity = RotationsPerSecond.of(0); - AngularVelocity indexerVelocity = RotationsPerSecond.of(0); + double indexerOutput = 0; Current motor1Current = Amps.of(0); Current motor2Current = Amps.of(0); @@ -38,7 +39,7 @@ public interface ShooterIO { // public default void setShooterPitch(ShooterState state, Angle angle) {} public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {} // public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {} - public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {} + public default void setIndexerOutput(ShooterState state, double percentOutput) {} public default void updateInputs(ShooterState state) {} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index a8e9479..4379de6 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -56,16 +56,18 @@ public class ShooterReal implements ShooterIO { } @Override - public void setIndexerVelocity(ShooterState state, AngularVelocity target) { - state.indexerTargetVelocity = target; + public void setIndexerOutput(ShooterState state, double percentOutput) { + state.indexerTargetOutput = percentOutput; - if(target.baseUnitMagnitude() == 0) { + if(percentOutput == 0) { m_indexerMotor.set(0); return; } - AngularVelocity motorRps = target.times(ShooterConstants.INDEXER_GEAR_RATIO); - m_indexerMotor.setControl(m_indexerVelocity.withVelocity(motorRps)); + + + // AngularVelocity motorRps = target.times(ShooterConstants.INDEXER_GEAR_RATIO); + m_indexerMotor.set(percentOutput); } @@ -74,7 +76,8 @@ public class ShooterReal implements ShooterIO { state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); - state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); + state.indexerOutput = m_indexerMotor.get(); + // state.indexerOutput = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); // state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);