added code for new drum shooter prototype

src/main/java/com/team2813/Constants.java

@@ -45,16 +45,18 @@ public final class Constants {
   // NOTE: The below motors are with placeholder CANIDs and are subject to change.
   // TODO: Discuss with electrical for permanent IDs.
   // Shooter Motors. Aliases: Flywheel motors.
-  public static final int MAIN_SHOOTER_MOTOR_ID = 19; // Right shooter motor.
-  public static final int FOLLOWER_SHOOTER_MOTOR_ID = 20; // Left shooter motor.
+  public static final int SHOOTER_MOTOR_1_ID = 19;
+  public static final int SHOOTER_MOTOR_2_ID = 20;
+  public static final int SHOOTER_MOTOR_3_ID = 32; // 2 and 3 are motors on the other side
+  public static final int SHOOTER_MOTOR_4_ID = 33;
 
   // Kicker Motor
-  public static final int KICKER_MOTOR_ID = 21;
+  public static final int KICKER_MOTOR_1_ID = 21;
+  public static final int KICKER_MOTOR_2_ID = 34;
 
   // Climb motors
-  // TODO(Stefan): ID these motors!
-  public static final int INNER_CLIMB_MOTOR_ID = 30;
-  public static final int OUTER_CLIMB_MOTOR_ID = 31;
+  //  public static final int INNER_CLIMB_MOTOR_ID = 30;
+  //  public static final int OUTER_CLIMB_MOTOR_ID = 31;
 
   /**
    * Returns true if the robot is on the red alliance.

src/main/java/com/team2813/RobotContainer.java

@@ -161,6 +161,7 @@ public RobotContainer(
         intakeExtension = new IntakeExtension(new IntakeExtensionIOSim());
         intakeRoller = new IntakeRoller(new IntakeRollerIOSim());
 
+        // todo add sim code for shooter and kicker if we go with the drum shooter
         shooter = new Shooter(new ShooterIOSim());
         kicker = new Kicker(new KickerIOSim());
         break;
@@ -265,6 +266,7 @@ private void configureButtonBindings() {
     operatorController.rightTrigger().whileTrue(shooter.spoolShooterTrenchSpeedCommand());
     operatorController.x().whileTrue(shooter.spoolShooterHubSpeedCommand());
     operatorController.y().whileTrue(shooter.spoolShooterHerdSpeedCommand());
+    operatorController.a().whileTrue(kicker.shootCommand());
 
     // Driver controls
     // Default command, normal field-relative drive

src/main/java/com/team2813/subsystems/kicker/KickerConstants.java

@@ -15,6 +15,12 @@ class KickerConstants {
   static final String RESIST_FUEL_PREFERENCE_NT = "Kicker/RESIST_FUEL_VOLTAGE";
 
   static final TalonFXConfiguration KICKER_MOTOR_CONFIG =
+      new TalonFXConfiguration()
+          .withMotorOutput(
+              new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive))
+          .withCurrentLimits(new CurrentLimitsConfigs().withStatorCurrentLimit(Amps.of(35)));
+
+  static final TalonFXConfiguration KICKER_MOTOR_2_CONFIG =
       new TalonFXConfiguration()
           .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))
           .withCurrentLimits(new CurrentLimitsConfigs().withStatorCurrentLimit(Amps.of(35)));

src/main/java/com/team2813/subsystems/kicker/KickerIO.java

@@ -10,10 +10,15 @@
 public interface KickerIO extends AutoCloseable {
   @AutoLog
   class KickerIOInputs {
-    public Voltage motorVoltage = Volts.of(0);
-    public AngularVelocity motorRotationalVelocity = RotationsPerSecond.of(0);
-    public Current motorStatorCurrent = Amps.of(0);
-    public Current motorSupplyCurrent = Amps.of(0);
+    public Voltage motor1Voltage = Volts.of(0);
+    public AngularVelocity motor1RotationalVelocity = RotationsPerSecond.of(0);
+    public Current motor1StatorCurrent = Amps.of(0);
+    public Current motor1SupplyCurrent = Amps.of(0);
+
+    public Voltage motor2Voltage = Volts.of(0);
+    public AngularVelocity motor2RotationalVelocity = RotationsPerSecond.of(0);
+    public Current motor2StatorCurrent = Amps.of(0);
+    public Current motor2SupplyCurrent = Amps.of(0);
   }
 
   @Override

src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java

@@ -7,28 +7,39 @@
 import edu.wpi.first.units.measure.Voltage;
 
 public class KickerIOReal implements KickerIO {
-  private final TalonFX motor;
+  private final TalonFX motor1;
+  private final TalonFX motor2;
 
   public KickerIOReal() {
-    motor = new TalonFX(Constants.KICKER_MOTOR_ID);
-    motor.getConfigurator().apply(KickerConstants.KICKER_MOTOR_CONFIG);
+    motor1 = new TalonFX(Constants.KICKER_MOTOR_1_ID);
+    motor1.getConfigurator().apply(KickerConstants.KICKER_MOTOR_CONFIG);
+
+    motor2 = new TalonFX(Constants.KICKER_MOTOR_2_ID);
+    motor2.getConfigurator().apply(KickerConstants.KICKER_MOTOR_2_CONFIG);
   }
 
   @Override
   public void updateState(KickerIOInputs inputs) {
-    inputs.motorVoltage = motor.getMotorVoltage().getValue();
-    inputs.motorRotationalVelocity = motor.getVelocity().getValue();
-    inputs.motorStatorCurrent = motor.getStatorCurrent().getValue();
-    inputs.motorSupplyCurrent = motor.getSupplyCurrent().getValue();
+    inputs.motor1Voltage = motor1.getMotorVoltage().getValue();
+    inputs.motor1RotationalVelocity = motor1.getVelocity().getValue();
+    inputs.motor1StatorCurrent = motor1.getStatorCurrent().getValue();
+    inputs.motor1SupplyCurrent = motor1.getSupplyCurrent().getValue();
+
+    inputs.motor2Voltage = motor2.getMotorVoltage().getValue();
+    inputs.motor2RotationalVelocity = motor2.getVelocity().getValue();
+    inputs.motor2StatorCurrent = motor2.getStatorCurrent().getValue();
+    inputs.motor2SupplyCurrent = motor2.getSupplyCurrent().getValue();
   }
 
   @Override
   public void setMotorVoltage(Voltage kickerMotorVoltage) {
-    motor.setVoltage(kickerMotorVoltage.in(Volts));
+    motor1.setVoltage(kickerMotorVoltage.in(Volts));
+    motor2.setVoltage(kickerMotorVoltage.in(Volts));
   }
 
   @Override
   public void close() {
-    motor.close();
+    motor1.close();
+    motor2.close();
   }
 }

src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java

@@ -1,60 +1,7 @@
-package com.team2813.subsystems.kicker;
-
-import static edu.wpi.first.units.Units.KilogramSquareMeters;
-import static edu.wpi.first.units.Units.Volts;
+// todo re add sim code
 
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.sim.TalonFXSimState;
-import com.team2813.Constants;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.units.measure.Voltage;
-import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+package com.team2813.subsystems.kicker;
 
 public class KickerIOSim implements KickerIO {
-  private final FlywheelSim flywheelSim;
-  private final TalonFX motor;
-
-  public KickerIOSim() {
-    motor = new TalonFX(Constants.KICKER_MOTOR_ID);
-    flywheelSim =
-        new FlywheelSim(
-            LinearSystemId.createFlywheelSystem(
-                DCMotor.getKrakenX60(1),
-                KickerConstants.KICKER_SIM_MOI.in(
-                    KilogramSquareMeters), // "Moment of Inertia" taken from OnShape.
-                KickerConstants.KICKER_MOTOR_TO_FLYWHEEL_GEARING),
-            DCMotor.getKrakenX60(1));
-  }
-
-  @Override
-  public void setMotorVoltage(Voltage kickerMotorVoltage) {
-    double volts = kickerMotorVoltage.in(Volts);
-    motor.setVoltage(volts);
-    flywheelSim.setInputVoltage(volts);
-  }
-
-  @Override
-  public void updateState(KickerIOInputs inputs) {
-    updateSimulation();
-
-    inputs.motorVoltage = motor.getMotorVoltage().getValue();
-    inputs.motorRotationalVelocity = motor.getVelocity().getValue();
-    inputs.motorStatorCurrent = motor.getStatorCurrent().getValue();
-    inputs.motorSupplyCurrent = motor.getSupplyCurrent().getValue();
-  }
-
-  private void updateSimulation() {
-    flywheelSim.update(Constants.SIM_TIME_PERIOD);
-
-    TalonFXSimState simState = motor.getSimState();
-    simState.setRotorAcceleration(flywheelSim.getAngularAcceleration());
-    simState.setRotorVelocity(flywheelSim.getAngularVelocity());
-    simState.setSupplyVoltage(Volts.of(12));
-  }
-
-  @Override
-  public void close() {
-    motor.close();
-  }
+  // todo implement sim code if drum shooter is used
 }

src/main/java/com/team2813/subsystems/shooter/Shooter.java

@@ -86,9 +86,9 @@ public void setShooterMotorVelocity(AngularVelocity velocity) {
    *     ShooterConstants#SHOOTER_SPOOL_SPEED_TOLERANCE} of the current motor setpoint
    */
   public boolean isMotorVelocityWithinTolerance() {
-    return RotationsPerSecond.of(replayedInputs.mainShooterMotorRotPerSec)
+    return RotationsPerSecond.of(replayedInputs.shooterMotor1RotPerSec)
         .isNear(
-            RotationsPerSecond.of(replayedInputs.mainShooterSetpointRotsPerSec),
+            RotationsPerSecond.of(replayedInputs.shooter1SetpointRotsPerSec),
             ShooterConstants.SHOOTER_SPOOL_SPEED_TOLERANCE);
   }
 }

src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java

@@ -48,7 +48,49 @@ public class ShooterConstants {
   }
 
   // Reminder: this is the right shooter motor when robot is viewed from behind.
-  public static final TalonFXConfiguration MAIN_SHOOTER_MOTOR_CONFIG =
+  public static final TalonFXConfiguration SHOOTER_MOTOR_1_CONFIG =
+      new TalonFXConfiguration()
+          .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))
+          .withSlot0(
+              new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743))
+          .withCurrentLimits(
+              new CurrentLimitsConfigs()
+                  .withStatorCurrentLimit(Amps.of(70))
+                  .withSupplyCurrentLimit(50));
+
+  public static final TalonFXConfiguration SHOOTER_MOTOR_2_CONFIG =
+      new TalonFXConfiguration()
+          .withMotorOutput(
+              new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive))
+          .withSlot0(
+              new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743))
+          .withCurrentLimits(
+              new CurrentLimitsConfigs()
+                  .withStatorCurrentLimit(Amps.of(70))
+                  .withSupplyCurrentLimit(50));
+
+  public static final TalonFXConfiguration SHOOTER_MOTOR_3_CONFIG =
+      new TalonFXConfiguration()
+          .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))
+          .withSlot0(
+              new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743))
+          .withCurrentLimits(
+              new CurrentLimitsConfigs()
+                  .withStatorCurrentLimit(Amps.of(70))
+                  .withSupplyCurrentLimit(50));
+
+  public static final TalonFXConfiguration SHOOTER_MOTOR_4_CONFIG =
+      new TalonFXConfiguration()
+          .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))
+          .withSlot0(
+              new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743))
+          .withCurrentLimits(
+              new CurrentLimitsConfigs()
+                  .withStatorCurrentLimit(Amps.of(70))
+                  .withSupplyCurrentLimit(50));
+
+  // Reminder: this is the upper left shooter motor when the robot is viewed from behind.
+  public static final TalonFXConfiguration UPPER_LEFT_SHOOTER_MOTOR_CONFIG =
       new TalonFXConfiguration()
           .withMotorOutput(
               new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive))
@@ -59,9 +101,11 @@ public class ShooterConstants {
                   .withStatorCurrentLimit(Amps.of(80))
                   .withSupplyCurrentLimit(60));
 
-  public static final TalonFXConfiguration FOLLOWER_SHOOTER_MOTOR_CONFIG =
+  // Reminder: this is the lower left shooter motor.
+  public static final TalonFXConfiguration LOWER_LEFT_SHOOTER_MOTOR_CONFIG =
       new TalonFXConfiguration()
-          .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))
+          .withMotorOutput(
+              new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive))
           .withSlot0(
               new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743))
           .withCurrentLimits(

src/main/java/com/team2813/subsystems/shooter/ShooterIO.java

@@ -8,17 +8,33 @@ public interface ShooterIO {
 
   @AutoLog
   class ShooterIOInputs {
-    public double mainShooterMotorVoltageVolts = 0;
-    public double mainShooterMotorAngleRotations = 0;
-    public double mainShooterMotorRotPerSec = 0;
-    public double mainShooterMotorStatorCurrentAmps = 0;
-    public double mainShooterMotorSupplyCurrentAmps = 0;
-    public double mainShooterSetpointRotsPerSec = 0;
-
-    public double followerShooterMotorVoltageVolts = 0;
-    public double followerShooterMotorRotPerSec = 0;
-    public double followerShooterMotorStatorCurrentAmps = 0;
-    public double followerShooterMotorSupplyCurrentAmps = 0;
+    public double shooterMotor1VoltageVolts = 0;
+    public double shooterMotor1AngleRotations = 0;
+    public double shooterMotor1RotPerSec = 0;
+    public double shooterMotor1SupplyCurrentAmps = 0;
+    public double shooterMotor1StatorCurrentAmps = 0;
+    public double shooter1SetpointRotsPerSec = 0;
+
+    public double shooterMotor2VoltageVolts = 0;
+    public double shooterMotor2AngleRotations = 0;
+    public double shooterMotor2RotPerSec = 0;
+    public double shooterMotor2SupplyCurrentAmps = 0;
+    public double shooterMotor2StatorCurrentAmps = 0;
+    public double shooter2SetpointRotsPerSec = 0;
+
+    public double shooterMotor3VoltageVolts = 0;
+    public double shooterMotor3AngleRotations = 0;
+    public double shooterMotor3RotPerSec = 0;
+    public double shooterMotor3SupplyCurrentAmps = 0;
+    public double shooterMotor3StatorCurrentAmps = 0;
+    public double shooter3SetpointRotsPerSec = 0;
+
+    public double shooterMotor4VoltageVolts = 0;
+    public double shooterMotor4AngleRotations = 0;
+    public double shooterMotor4RotPerSec = 0;
+    public double shooterMotor4SupplyCurrentAmps = 0;
+    public double shooterMotor4StatorCurrentAmps = 0;
+    public double shooter4SetpointRotsPerSec = 0;
   }
 
   /**