Merge pull request #9 from strykeforce/superStructure

added left and right shooter control

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -4,7 +4,8 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 
 public class ShooterConstants {
-  public static final int kShooterTalonID = -1;
+  public static final int kLeftShooterTalonID = -1;
+  public static final int kRightShooterTalonID = -1;
   public static final double kCloseEnough = 100;
 
   public static final TalonFXConfiguration getShooterConfig() {

src/main/java/frc/robot/subsystems/shooter/ShooterIO.java

@@ -8,8 +8,10 @@ public interface ShooterIO {
 
   @AutoLog
   public static class ShooterIOInputs {
-    public double velocity;
-    public double position;
+    public double velocityLeft;
+    public double positionLeft;
+    public double velcoityRight;
+    public double positionRight;
     public boolean isFwdLimitSwitchClosed = false;
     public boolean isRevLimitSwitchClosed = false;
   }
@@ -24,4 +26,8 @@ public default void setSupplyCurrentLimit(
   public default void registerWith(TelemetryService telemetryService) {}
 
   public default void setSpeed(double speed) {}
+
+  public default void setLeftSpeed(double speed) {}
+
+  public default void setRightSpeed(double speed) {}
 }

src/main/java/frc/robot/subsystems/shooter/ShooterIOFX.java

@@ -14,56 +14,83 @@
 import org.strykeforce.telemetry.TelemetryService;
 
 public class ShooterIOFX implements ShooterIO {
-  private TalonFX shooter;
+  private TalonFX shooterLeft;
+  private TalonFX shooterRight;
   private Logger logger;
 
   private double setpoint;
   private final double absSensorIntial;
 
   TalonFXConfigurator configurator;
-  private VelocityDutyCycle velocityRequest =
+  private VelocityDutyCycle velocityLeftRequest =
       new VelocityDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
-  StatusSignal<Double> currPosition;
-  StatusSignal<Double> currVelocity;
+  private VelocityDutyCycle velocityRightRequest =
+      new VelocityDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
+  StatusSignal<Double> curLeftPosition;
+  StatusSignal<Double> curLeftVelocity;
+  StatusSignal<Double> curRightPosition;
+  StatusSignal<Double> curRightVelocity;
   StatusSignal<ForwardLimitValue> fwdLimitSwitch;
   StatusSignal<ReverseLimitValue> revLimitSwitch;
 
   public ShooterIOFX() {
     logger = LoggerFactory.getLogger(this.getClass());
-    shooter = new TalonFX(ShooterConstants.kShooterTalonID);
-    absSensorIntial = shooter.getPosition().getValue();
+    shooterLeft = new TalonFX(ShooterConstants.kLeftShooterTalonID);
+    shooterRight = new TalonFX(ShooterConstants.kRightShooterTalonID);
+    absSensorIntial = shooterLeft.getPosition().getValue();
+
+    configurator = shooterLeft.getConfigurator();
+    configurator.apply(new TalonFXConfiguration());
+    configurator.apply(ShooterConstants.getShooterConfig());
 
-    configurator = shooter.getConfigurator();
+    configurator = shooterRight.getConfigurator();
     configurator.apply(new TalonFXConfiguration());
     configurator.apply(ShooterConstants.getShooterConfig());
 
-    currPosition = shooter.getPosition();
-    currVelocity = shooter.getVelocity();
-    fwdLimitSwitch = shooter.getForwardLimit();
-    revLimitSwitch = shooter.getReverseLimit();
+    curLeftPosition = shooterLeft.getPosition();
+    curLeftVelocity = shooterLeft.getVelocity();
+    fwdLimitSwitch = shooterLeft.getForwardLimit();
+    revLimitSwitch = shooterLeft.getReverseLimit();
+
+    curRightPosition = shooterRight.getPosition();
+    curRightVelocity = shooterRight.getVelocity();
   }
 
   @Override
   public void updateInputs(ShooterIOInputs inputs) {
-    inputs.velocity = currVelocity.refresh().getValue();
-    inputs.position = currPosition.refresh().getValue();
+    inputs.velocityLeft = curLeftVelocity.refresh().getValue();
+    inputs.positionLeft = curLeftPosition.refresh().getValue();
+    inputs.velcoityRight = curRightVelocity.refresh().getValue();
+    inputs.positionRight = curRightPosition.refresh().getValue();
     inputs.isFwdLimitSwitchClosed = fwdLimitSwitch.refresh().getValue().value == 1;
     inputs.isRevLimitSwitchClosed = revLimitSwitch.refresh().getValue().value == 1;
   }
 
   @Override
   public void setPct(double percentOutput) {
-    shooter.set(percentOutput);
+    shooterLeft.set(percentOutput);
+    shooterRight.set(percentOutput);
   }
 
   @Override
   public void setSpeed(double speed) {
-    shooter.setControl(velocityRequest.withVelocity(speed));
+    shooterLeft.setControl(velocityLeftRequest.withVelocity(speed));
+    shooterRight.setControl(velocityRightRequest.withVelocity(speed));
+  }
+
+  @Override
+  public void setLeftSpeed(double speed) {
+    shooterLeft.setControl(velocityLeftRequest.withVelocity(speed));
+  }
+
+  @Override
+  public void setRightSpeed(double speed) {
+    shooterRight.setControl(velocityRightRequest.withVelocity(speed));
   }
 
   @Override
   public void registerWith(TelemetryService telemetryService) {
-    telemetryService.register(shooter, true);
+    telemetryService.register(shooterLeft, true);
   }
 
   @Override

src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java

@@ -13,7 +13,8 @@ public class ShooterSubsystem extends MeasurableSubsystem implements ClosedLoopS
   // Private Variables
   ShooterIO io;
   ShooterStates curState = ShooterStates.IDLE;
-  private double setpoint = 0.0;
+  private double leftSetpoint = 0.0;
+  double rightSetpoint = 0.0;
 
   ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
   private Logger logger = LoggerFactory.getLogger(ShooterSubsystem.class);
@@ -27,20 +28,32 @@ public ShooterSubsystem(ShooterIO io) {
 
   @Override
   public double getSpeed() {
-    return inputs.velocity;
+    return inputs.velocityLeft;
   }
 
   @Override
   public boolean atSpeed() {
-    return Math.abs(inputs.velocity - setpoint) < ShooterConstants.kCloseEnough;
+    return Math.abs(inputs.velocityLeft - leftSetpoint) < ShooterConstants.kCloseEnough
+        && Math.abs(inputs.velcoityRight - rightSetpoint) < ShooterConstants.kCloseEnough;
   }
 
   @Override
   public void setSpeed(double speed) {
-    setpoint = speed;
+    leftSetpoint = speed;
+    rightSetpoint = speed;
     io.setSpeed(speed);
   }
 
+  public void setLeftSpeed(double speed) {
+    leftSetpoint = speed;
+    io.setLeftSpeed(speed);
+  }
+
+  public void setRightSpeed(double speed) {
+    rightSetpoint = speed;
+    io.setRightSpeed(speed);
+  }
+
   public ShooterStates getState() {
     return curState;
   }

src/main/java/frc/robot/subsystems/superStructure/SuperStructure.java

@@ -18,7 +18,8 @@ public class SuperStructure extends MeasurableSubsystem {
   Logger logger;
   SuperStructureStates curState = SuperStructureStates.IDLE;
   SuperStructureStates nextState = SuperStructureStates.IDLE;
-  private double shooterSpeed = 0.0;
+  private double leftShooterSpeed = 0.0;
+  private double rightShooterSpeed = 0.0;
   private double elbowSetpoint = 0.0;
   private double wristSetpoint = 0.0;
   private boolean flipMagazineOut = false;
@@ -57,12 +58,14 @@ public SuperStructureStates getState() {
   // Basic methods to go to each position
   //    Works by setting each axes' setpoint and starting intial movement
   //      Then determines if elbow or wrist go first by flipMagazineOut boolean
-  public void shoot(double shooterSpeed, double elbowSetpoint) {
+  public void shoot(double leftShooterSpeed, double rightShooterSpeed, double elbowSetpoint) {
     this.elbowSetpoint = elbowSetpoint;
-    this.shooterSpeed = shooterSpeed;
+    this.leftShooterSpeed = leftShooterSpeed;
+    this.rightShooterSpeed = rightShooterSpeed;
     wristSetpoint = SuperStructureConstants.kWristShootSetPoint;
 
-    shooterSubsystem.setSpeed(shooterSpeed);
+    shooterSubsystem.setLeftSpeed(leftShooterSpeed);
+    shooterSubsystem.setRightSpeed(rightShooterSpeed);
     wristSubsystem.setPosition(wristSetpoint);
 
     logger.info("{} -> TRANSFER(SHOOTING)");
@@ -74,9 +77,10 @@ public void shoot(double shooterSpeed, double elbowSetpoint) {
   public void preClimb() {
     elbowSetpoint = SuperStructureConstants.kElbowPreClimbSetPoint;
     wristSetpoint = SuperStructureConstants.kWristPreClimbSetPoint;
-    shooterSpeed = SuperStructureConstants.kShooterPreClimbSetPoint;
+    leftShooterSpeed = SuperStructureConstants.kShooterPreClimbSetPoint;
+    rightShooterSpeed = SuperStructureConstants.kShooterPreClimbSetPoint;
 
-    shooterSubsystem.setSpeed(shooterSpeed);
+    shooterSubsystem.setSpeed(leftShooterSpeed);
     wristSubsystem.setPosition(wristSetpoint);
 
     logger.info("{} -> TRANSFER(PRE_CLIMB)");
@@ -88,9 +92,10 @@ public void preClimb() {
   public void postClimb() {
     elbowSetpoint = SuperStructureConstants.kElbowPostClimbSetPoint;
     wristSetpoint = SuperStructureConstants.kWristPostClimbSetPoint;
-    shooterSpeed = SuperStructureConstants.kShooterPreClimbSetPoint;
+    leftShooterSpeed = SuperStructureConstants.kShooterPreClimbSetPoint;
+    rightShooterSpeed = SuperStructureConstants.kShooterPreClimbSetPoint;
 
-    shooterSubsystem.setSpeed(shooterSpeed);
+    shooterSubsystem.setSpeed(rightShooterSpeed);
     wristSubsystem.setPosition(wristSetpoint);
 
     logger.info("{} -> TRANSFER(POST_CLIMB)");
@@ -102,9 +107,10 @@ public void postClimb() {
   public void amp() {
     elbowSetpoint = SuperStructureConstants.kElbowAmpSetPoint;
     wristSetpoint = SuperStructureConstants.kWristAmpSetPoint;
-    shooterSpeed = SuperStructureConstants.kShooterAmpSetPoint;
+    leftShooterSpeed = SuperStructureConstants.kShooterAmpSetPoint;
+    rightShooterSpeed = SuperStructureConstants.kShooterAmpSetPoint;
 
-    shooterSubsystem.setSpeed(shooterSpeed);
+    shooterSubsystem.setSpeed(leftShooterSpeed);
     elbowSubsystem.setPosition(elbowSetpoint);
 
     logger.info("{} -> TRANSFER(AMP)");
@@ -116,9 +122,10 @@ public void amp() {
   public void trap() {
     elbowSetpoint = SuperStructureConstants.kElbowTrapSetPoint;
     wristSetpoint = SuperStructureConstants.kWristTrapSetPoint;
-    shooterSpeed = SuperStructureConstants.kShooterTrapSetPoint;
+    leftShooterSpeed = SuperStructureConstants.kShooterTrapSetPoint;
+    rightShooterSpeed = SuperStructureConstants.kShooterTrapSetPoint;
 
-    shooterSubsystem.setSpeed(shooterSpeed);
+    shooterSubsystem.setSpeed(leftShooterSpeed);
     elbowSubsystem.setPosition(elbowSetpoint);
 
     logger.info("{} -> TRANSFER(TRAP)");
@@ -130,9 +137,10 @@ public void trap() {
   public void intake() {
     elbowSetpoint = SuperStructureConstants.kElbowIntakeSetPoint;
     wristSetpoint = SuperStructureConstants.kWristIntakeSetPoint;
-    shooterSpeed = SuperStructureConstants.kShooterIntakeSetPoint;
+    leftShooterSpeed = SuperStructureConstants.kShooterIntakeSetPoint;
+    rightShooterSpeed = SuperStructureConstants.kShooterIntakeSetPoint;
 
-    shooterSubsystem.setSpeed(shooterSpeed);
+    shooterSubsystem.setSpeed(rightShooterSpeed);
     wristSubsystem.setPosition(wristSetpoint);
 
     logger.info("{} -> TRANSFER(INTAKE)");
@@ -144,9 +152,10 @@ public void intake() {
   public void defense() {
     elbowSetpoint = SuperStructureConstants.kElbowDefenseSetPoint;
     wristSetpoint = SuperStructureConstants.kWristDefenseSetPoint;
-    shooterSpeed = SuperStructureConstants.kShooterDefenseSetPoint;
+    rightShooterSpeed = SuperStructureConstants.kShooterDefenseSetPoint;
+    leftShooterSpeed = SuperStructureConstants.kShooterDefenseSetPoint;
 
-    shooterSubsystem.setSpeed(shooterSpeed);
+    shooterSubsystem.setSpeed(leftShooterSpeed);
     elbowSubsystem.setPosition(elbowSetpoint);
 
     logger.info("{} -> TRANSFER(DEFENSE)");