From f6069a73ee4fbe31e42a80d646e610fd60030b91 Mon Sep 17 00:00:00 2001 From: Piyou Date: Fri, 20 Mar 2026 19:15:25 -0700 Subject: [PATCH 01/16] added code for new drum shooter prototype --- src/main/java/com/team2813/Constants.java | 9 ++- .../java/com/team2813/RobotContainer.java | 1 + .../subsystems/kicker/KickerConstants.java | 5 ++ .../team2813/subsystems/kicker/KickerIO.java | 10 ++- .../subsystems/kicker/KickerIOReal.java | 26 ++++--- .../subsystems/kicker/KickerIOSim.java | 15 ++-- .../team2813/subsystems/shooter/Shooter.java | 4 +- .../subsystems/shooter/ShooterConstants.java | 26 ++++++- .../subsystems/shooter/ShooterIO.java | 32 ++++++--- .../subsystems/shooter/ShooterIOReal.java | 69 ++++++++++++------- .../subsystems/shooter/ShooterIOSim.java | 13 +--- 11 files changed, 139 insertions(+), 71 deletions(-) diff --git a/src/main/java/com/team2813/Constants.java b/src/main/java/com/team2813/Constants.java index e753fb43..3dcdc647 100644 --- a/src/main/java/com/team2813/Constants.java +++ b/src/main/java/com/team2813/Constants.java @@ -45,11 +45,14 @@ 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; // Right shooter motor. + public static final int SHOOTER_MOTOR_2_ID = 20; // Left shooter motor. + public static final int SHOOTER_MOTOR_3_ID = 0; // 2 and 3 are motors on the other side + public static final int SHOOTER_MOTOR_4_ID = 0; // 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 = 21; // Climb motors // TODO(Stefan): ID these motors! diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index c0ddaf7e..8c962030 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -150,6 +150,7 @@ public RobotContainer(AllTunerConstants tunerConstants, Mode mode) { intakeExtension = new IntakeExtension(new IntakeExtensionIOSim()); intakeRoller = new IntakeRoller(new IntakeRollerIOSim()); + // todo fix sim code shooter = new Shooter(new ShooterIOSim()); kicker = new Kicker(new KickerIOSim()); break; diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java b/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java index 083bbb49..fd183f1e 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java @@ -19,6 +19,11 @@ class KickerConstants { .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_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))); + static final double KICKER_MOTOR_TO_FLYWHEEL_GEARING = 2.0 / 5.0; static final double SHOOT_VOLTAGE = 7; diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerIO.java b/src/main/java/com/team2813/subsystems/kicker/KickerIO.java index c6e685dc..9ccba273 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerIO.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerIO.java @@ -10,9 +10,13 @@ public interface KickerIO extends AutoCloseable { @AutoLog class KickerIOInputs { - public Voltage motorVoltage = Volts.of(0); - public AngularVelocity motorRotationalVelocity = RotationsPerSecond.of(0); - public Current motorCurrent = Amps.of(0); + public Voltage motor1Voltage = Volts.of(0); + public AngularVelocity motor1RotationalVelocity = RotationsPerSecond.of(0); + public Current motor1Current = Amps.of(0); + + public Voltage motor2Voltage = Volts.of(0); + public AngularVelocity motor2RotationalVelocity = RotationsPerSecond.of(0); + public Current motor2Current = Amps.of(0); } @Override diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java b/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java index 780b1ab0..b0891ab4 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java @@ -7,27 +7,37 @@ 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_CONFIG); } @Override public void updateState(KickerIOInputs inputs) { - inputs.motorVoltage = motor.getMotorVoltage().getValue(); - inputs.motorRotationalVelocity = motor.getVelocity().getValue(); - inputs.motorCurrent = motor.getStatorCurrent().getValue(); + inputs.motor1Voltage = motor1.getMotorVoltage().getValue(); + inputs.motor1RotationalVelocity = motor1.getVelocity().getValue(); + inputs.motor1Current = motor1.getStatorCurrent().getValue(); + + inputs.motor2Voltage = motor2.getMotorVoltage().getValue(); + inputs.motor2RotationalVelocity = motor2.getVelocity().getValue(); + inputs.motor2Current = motor2.getStatorCurrent().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(); } } diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java b/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java index 65a7afcb..1ea8f177 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java @@ -1,17 +1,9 @@ -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; @@ -56,4 +48,5 @@ private void updateSimulation() { public void close() { motor.close(); } + */ } diff --git a/src/main/java/com/team2813/subsystems/shooter/Shooter.java b/src/main/java/com/team2813/subsystems/shooter/Shooter.java index 875ad87d..c9acad73 100644 --- a/src/main/java/com/team2813/subsystems/shooter/Shooter.java +++ b/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); } } diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java index a89bc2ca..fed9dafd 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java @@ -48,7 +48,7 @@ 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.CounterClockwise_Positive)) @@ -59,7 +59,7 @@ public class ShooterConstants { .withStatorCurrentLimit(Amps.of(70)) .withSupplyCurrentLimit(50)); - public static final TalonFXConfiguration FOLLOWER_SHOOTER_MOTOR_CONFIG = + public static final TalonFXConfiguration SHOOTER_MOTOR_2_CONFIG = new TalonFXConfiguration() .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) .withSlot0( @@ -69,6 +69,28 @@ public class ShooterConstants { .withStatorCurrentLimit(Amps.of(70)) .withSupplyCurrentLimit(50)); + public static final TalonFXConfiguration SHOOTER_MOTOR_3_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_4_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 double SHOOTER_MOTOR_TO_FLYWHEEL_GEARING = 1.0; public static AngularVelocity getShooterTrenchShootVelocity() { diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java index afd973ca..883836a4 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java @@ -8,15 +8,29 @@ public interface ShooterIO { @AutoLog class ShooterIOInputs { - public double mainShooterMotorVoltageVolts = 0; - public double mainShooterMotorAngleRotations = 0; - public double mainShooterMotorRotPerSec = 0; - public double mainShooterMotorCurrentAmps = 0; - public double mainShooterSetpointRotsPerSec = 0; - - public double followerShooterMotorVoltageVolts = 0; - public double followerShooterMotorRotPerSec = 0; - public double followerShooterMotorCurrentAmps = 0; + public double shooterMotor1VoltageVolts = 0; + public double shooterMotor1AngleRotations = 0; + public double shooterMotor1RotPerSec = 0; + public double shooterMotor1CurrentAmps = 0; + public double shooter1SetpointRotsPerSec = 0; + + public double shooterMotor2VoltageVolts = 0; + public double shooterMotor2AngleRotations = 0; + public double shooterMotor2RotPerSec = 0; + public double shooterMotor2CurrentAmps = 0; + public double shooter2SetpointRotsPerSec = 0; + + public double shooterMotor3VoltageVolts = 0; + public double shooterMotor3AngleRotations = 0; + public double shooterMotor3RotPerSec = 0; + public double shooterMotor3CurrentAmps = 0; + public double shooter3SetpointRotsPerSec = 0; + + public double shooterMotor4VoltageVolts = 0; + public double shooterMotor4AngleRotations = 0; + public double shooterMotor4RotPerSec = 0; + public double shooterMotor4CurrentAmps = 0; + public double shooter4SetpointRotsPerSec = 0; } /** diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java index d221ba95..0524406f 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java @@ -12,36 +12,54 @@ public class ShooterIOReal implements ShooterIO { // Declaring the control here saves on having to create a new object each time. private final VelocityVoltage shooterVelocityControl; - private TalonFX mainShooterMotor; - private TalonFX followerShooterMotor; + private TalonFX shooterMotor1; + private TalonFX shooterMotor2; + private TalonFX shooterMotor3; + private TalonFX shooterMotor4; private AngularVelocity mainShooterSetpoint = RotationsPerSecond.of(0); public ShooterIOReal() { - mainShooterMotor = new TalonFX(Constants.MAIN_SHOOTER_MOTOR_ID); - mainShooterMotor.getConfigurator().apply(ShooterConstants.MAIN_SHOOTER_MOTOR_CONFIG); + shooterMotor1 = new TalonFX(Constants.SHOOTER_MOTOR_1_ID); + shooterMotor1.getConfigurator().apply(ShooterConstants.SHOOTER_MOTOR_1_CONFIG); - followerShooterMotor = new TalonFX(Constants.FOLLOWER_SHOOTER_MOTOR_ID); - followerShooterMotor.getConfigurator().apply(ShooterConstants.FOLLOWER_SHOOTER_MOTOR_CONFIG); + shooterMotor2 = new TalonFX(Constants.SHOOTER_MOTOR_2_ID); + shooterMotor2.getConfigurator().apply(ShooterConstants.SHOOTER_MOTOR_2_CONFIG); + + shooterMotor3 = new TalonFX(Constants.SHOOTER_MOTOR_3_ID); + shooterMotor3.getConfigurator().apply(ShooterConstants.SHOOTER_MOTOR_3_CONFIG); + + shooterMotor4 = new TalonFX(Constants.SHOOTER_MOTOR_4_ID); + shooterMotor4.getConfigurator().apply(ShooterConstants.SHOOTER_MOTOR_4_CONFIG); shooterVelocityControl = new VelocityVoltage(RotationsPerSecond.of(0)); } @Override public void updateState(ShooterIOInputs inputs) { - inputs.mainShooterMotorVoltageVolts = mainShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.mainShooterMotorAngleRotations = mainShooterMotor.getPosition().getValue().in(Rotations); - inputs.mainShooterMotorRotPerSec = - mainShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.mainShooterMotorCurrentAmps = mainShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.mainShooterSetpointRotsPerSec = mainShooterSetpoint.in(RotationsPerSecond); - - inputs.followerShooterMotorVoltageVolts = - followerShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.followerShooterMotorRotPerSec = - followerShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.followerShooterMotorCurrentAmps = - followerShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.shooterMotor1VoltageVolts = shooterMotor1.getMotorVoltage().getValue().in(Volts); + inputs.shooterMotor1AngleRotations = shooterMotor1.getPosition().getValue().in(Rotations); + inputs.shooterMotor1RotPerSec = shooterMotor1.getVelocity().getValue().in(RotationsPerSecond); + inputs.shooterMotor1CurrentAmps = shooterMotor1.getStatorCurrent().getValue().in(Amps); + inputs.shooter1SetpointRotsPerSec = mainShooterSetpoint.in(RotationsPerSecond); + + inputs.shooterMotor2VoltageVolts = shooterMotor2.getMotorVoltage().getValue().in(Volts); + inputs.shooterMotor2AngleRotations = shooterMotor2.getPosition().getValue().in(Rotations); + inputs.shooterMotor2RotPerSec = shooterMotor2.getVelocity().getValue().in(RotationsPerSecond); + inputs.shooterMotor2CurrentAmps = shooterMotor2.getStatorCurrent().getValue().in(Amps); + inputs.shooter2SetpointRotsPerSec = mainShooterSetpoint.in(RotationsPerSecond); + + inputs.shooterMotor3VoltageVolts = shooterMotor3.getMotorVoltage().getValue().in(Volts); + inputs.shooterMotor3AngleRotations = shooterMotor3.getPosition().getValue().in(Rotations); + inputs.shooterMotor3RotPerSec = shooterMotor3.getVelocity().getValue().in(RotationsPerSecond); + inputs.shooterMotor3CurrentAmps = shooterMotor3.getStatorCurrent().getValue().in(Amps); + inputs.shooter3SetpointRotsPerSec = mainShooterSetpoint.in(RotationsPerSecond); + + inputs.shooterMotor4VoltageVolts = shooterMotor4.getMotorVoltage().getValue().in(Volts); + inputs.shooterMotor4AngleRotations = shooterMotor4.getPosition().getValue().in(Rotations); + inputs.shooterMotor4RotPerSec = shooterMotor4.getVelocity().getValue().in(RotationsPerSecond); + inputs.shooterMotor4CurrentAmps = shooterMotor4.getStatorCurrent().getValue().in(Amps); + inputs.shooter4SetpointRotsPerSec = mainShooterSetpoint.in(RotationsPerSecond); } @Override @@ -49,15 +67,20 @@ public void setShooterMotorVelocity(AngularVelocity shooterMotorVelocity) { // Uses Rot/s rather than passing AngularVelocity because there seems to be some issue with // AngularVelocity converting its value (i.e. Rot/s) to the base unit (rad/s) mainShooterSetpoint = shooterMotorVelocity; - mainShooterMotor.setControl(shooterVelocityControl.withVelocity(shooterMotorVelocity)); + shooterMotor1.setControl(shooterVelocityControl.withVelocity(shooterMotorVelocity)); // same velocity - followerShooterMotor.setControl(shooterVelocityControl); + shooterMotor2.setControl(shooterVelocityControl); + + shooterMotor3.setControl(shooterVelocityControl); + shooterMotor4.setControl(shooterVelocityControl); } @Override public void setShooterMotorVoltage(Voltage shooterVoltage) { mainShooterSetpoint = RotationsPerSecond.of(0); - mainShooterMotor.setVoltage(shooterVoltage.in(Volts)); - followerShooterMotor.setVoltage(shooterVoltage.in(Volts)); + shooterMotor1.setVoltage(shooterVoltage.in(Volts)); + shooterMotor2.setVoltage(shooterVoltage.in(Volts)); + shooterMotor3.setVoltage(shooterVoltage.in(Volts)); + shooterMotor4.setVoltage(shooterVoltage.in(Volts)); } } diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java index 7bf08eb2..aa739c1c 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java @@ -1,18 +1,10 @@ +// todo fix sim code package com.team2813.subsystems.shooter; import static edu.wpi.first.units.Units.*; -import com.ctre.phoenix6.controls.VelocityVoltage; -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.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; - public class ShooterIOSim implements ShooterIO { + /* private final TalonFX mainShooterMotor; private final TalonFXSimState mainShooterSimState; @@ -97,4 +89,5 @@ public void setShooterMotorVoltage(Voltage shooterMotorVoltage) { followerShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); shooterSim.setInputVoltage(shooterMotorVoltage.in(Volts)); } + */ } From facfb290523329ad7426e28f6c9025bb74e8518d Mon Sep 17 00:00:00 2001 From: Piyou Date: Sat, 21 Mar 2026 12:45:49 -0700 Subject: [PATCH 02/16] updated based on Mr.Cooney's comments --- .../java/com/team2813/RobotContainer.java | 2 +- .../subsystems/kicker/KickerIOSim.java | 47 +--------- .../subsystems/shooter/ShooterIOSim.java | 87 +------------------ 3 files changed, 3 insertions(+), 133 deletions(-) diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index 8c962030..a92cb447 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -150,7 +150,7 @@ public RobotContainer(AllTunerConstants tunerConstants, Mode mode) { intakeExtension = new IntakeExtension(new IntakeExtensionIOSim()); intakeRoller = new IntakeRoller(new IntakeRollerIOSim()); - // todo fix sim code + // 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; diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java b/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java index 1ea8f177..f7c6cb8c 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java @@ -3,50 +3,5 @@ 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.motorCurrent = motor.getStatorCurrent().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 } diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java index aa739c1c..89eaa3a6 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java @@ -4,90 +4,5 @@ import static edu.wpi.first.units.Units.*; public class ShooterIOSim implements ShooterIO { - /* - private final TalonFX mainShooterMotor; - private final TalonFXSimState mainShooterSimState; - - private final TalonFX followerShooterMotor; - private final TalonFXSimState followerShooterSimState; - - private final VelocityVoltage shooterVelocityControl; - - private final FlywheelSim shooterSim; - - private AngularVelocity mainShooterSetpoint = RotationsPerSecond.of(0); - - public ShooterIOSim() { - mainShooterMotor = new TalonFX(Constants.MAIN_SHOOTER_MOTOR_ID); - mainShooterMotor.getConfigurator().apply(ShooterConstants.MAIN_SHOOTER_MOTOR_CONFIG); - mainShooterSimState = mainShooterMotor.getSimState(); - - followerShooterMotor = new TalonFX(Constants.FOLLOWER_SHOOTER_MOTOR_ID); - followerShooterMotor.getConfigurator().apply(ShooterConstants.FOLLOWER_SHOOTER_MOTOR_CONFIG); - followerShooterSimState = followerShooterMotor.getSimState(); - - shooterVelocityControl = new VelocityVoltage(RotationsPerSecond.of(0)); - - shooterSim = - new FlywheelSim( - LinearSystemId.createFlywheelSystem( - DCMotor.getKrakenX60(2), - ShooterConstants.SHOOTER_SIM_MOI, // "Moment of Inertia" taken from OnShape. - ShooterConstants.SHOOTER_MOTOR_TO_FLYWHEEL_GEARING), - DCMotor.getKrakenX60(2)); - } - - @Override - public void updateState(ShooterIOInputs inputs) { - updateSimulation(); - - mainShooterSimState.setSupplyVoltage(Volts.of(12)); - followerShooterSimState.setSupplyVoltage(Volts.of(12)); - - inputs.mainShooterMotorVoltageVolts = mainShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.mainShooterMotorAngleRotations = mainShooterMotor.getPosition().getValue().in(Rotations); - inputs.mainShooterMotorRotPerSec = - mainShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.mainShooterMotorCurrentAmps = mainShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.mainShooterSetpointRotsPerSec = mainShooterSetpoint.in(RotationsPerSecond); - - inputs.followerShooterMotorVoltageVolts = - followerShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.followerShooterMotorRotPerSec = - followerShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.followerShooterMotorCurrentAmps = - followerShooterMotor.getStatorCurrent().getValue().in(Amps); - } - - public void updateSimulation() { - // Update physics simulations every 20ms (like the actual bot). - shooterSim.update(Constants.SIM_TIME_PERIOD); - - // Feed the velocity and acceleration of the roller simulation into the simulation motors to - // accurately model them. - mainShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration()); - mainShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity()); - // The follower roller motor is opposed with the main motor, so it gets negated values. - followerShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration().unaryMinus()); - followerShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity().unaryMinus()); - } - - @Override - public void setShooterMotorVelocity(AngularVelocity shooterMotorVelocity) { - mainShooterSetpoint = shooterMotorVelocity; - mainShooterMotor.setControl( - shooterVelocityControl.withVelocity(shooterMotorVelocity.in(RotationsPerSecond))); - followerShooterMotor.setControl(shooterVelocityControl); - - shooterSim.setAngularVelocity(shooterMotorVelocity.in(RadiansPerSecond)); - } - - @Override - public void setShooterMotorVoltage(Voltage shooterMotorVoltage) { - mainShooterSetpoint = RotationsPerSecond.of(0); - mainShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); - followerShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); - shooterSim.setInputVoltage(shooterMotorVoltage.in(Volts)); - } - */ + // todo implement sim code if we are going with the drum shooter } From c88e420bbd582e3ee6be2440ab0cf2a6fd520dba Mon Sep 17 00:00:00 2001 From: Piyou Date: Sat, 21 Mar 2026 13:19:53 -0700 Subject: [PATCH 03/16] added can ids --- src/main/java/com/team2813/Constants.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/team2813/Constants.java b/src/main/java/com/team2813/Constants.java index 3dcdc647..565f0cd4 100644 --- a/src/main/java/com/team2813/Constants.java +++ b/src/main/java/com/team2813/Constants.java @@ -45,14 +45,14 @@ 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 SHOOTER_MOTOR_1_ID = 19; // Right shooter motor. - public static final int SHOOTER_MOTOR_2_ID = 20; // Left shooter motor. - public static final int SHOOTER_MOTOR_3_ID = 0; // 2 and 3 are motors on the other side - public static final int SHOOTER_MOTOR_4_ID = 0; + 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 4 are motors on the other side + public static final int SHOOTER_MOTOR_4_ID = 33; // Kicker Motor public static final int KICKER_MOTOR_1_ID = 21; - public static final int KICKER_MOTOR_2_ID = 21; + public static final int KICKER_MOTOR_2_ID = 34; // Climb motors // TODO(Stefan): ID these motors! From dc0a0ab850614b8b1c318ed82c79c748397c4c5c Mon Sep 17 00:00:00 2001 From: Piyou Date: Sat, 21 Mar 2026 17:19:19 -0700 Subject: [PATCH 04/16] added kicker shoot to operator controller for testing --- src/main/java/com/team2813/RobotContainer.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index a92cb447..5fbd5108 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -253,6 +253,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 From 8d2f272e66a9a98befd60e30cdaa82d1988bd8fd Mon Sep 17 00:00:00 2001 From: Piyou Date: Sat, 21 Mar 2026 17:32:49 -0700 Subject: [PATCH 05/16] reversed invert type of all motor --- .../team2813/subsystems/kicker/KickerConstants.java | 6 ++++-- .../subsystems/shooter/ShooterConstants.java | 12 +++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java b/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java index fd183f1e..f43b85ab 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java @@ -16,12 +16,14 @@ class KickerConstants { static final TalonFXConfiguration KICKER_MOTOR_CONFIG = new TalonFXConfiguration() - .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) + .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)) + .withMotorOutput( + new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) .withCurrentLimits(new CurrentLimitsConfigs().withStatorCurrentLimit(Amps.of(35))); static final double KICKER_MOTOR_TO_FLYWHEEL_GEARING = 2.0 / 5.0; diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java index fed9dafd..a1a676dd 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java @@ -50,8 +50,7 @@ public class ShooterConstants { // Reminder: this is the right shooter motor when robot is viewed from behind. public static final TalonFXConfiguration SHOOTER_MOTOR_1_CONFIG = new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) .withSlot0( new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) .withCurrentLimits( @@ -61,7 +60,8 @@ public class ShooterConstants { public static final TalonFXConfiguration SHOOTER_MOTOR_2_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( @@ -71,8 +71,7 @@ public class ShooterConstants { public static final TalonFXConfiguration SHOOTER_MOTOR_3_CONFIG = new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) .withSlot0( new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) .withCurrentLimits( @@ -82,8 +81,7 @@ public class ShooterConstants { public static final TalonFXConfiguration SHOOTER_MOTOR_4_CONFIG = new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) .withSlot0( new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) .withCurrentLimits( From 4f075945122a29012c8d9258134e117514e2bd88 Mon Sep 17 00:00:00 2001 From: Piyou Date: Sat, 21 Mar 2026 18:08:56 -0700 Subject: [PATCH 06/16] changes invert type of kicker motor 2 based on protype behavior --- .../java/com/team2813/subsystems/kicker/KickerConstants.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java b/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java index f43b85ab..66d9b3e7 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java @@ -22,8 +22,7 @@ class KickerConstants { static final TalonFXConfiguration KICKER_MOTOR_2_CONFIG = new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) .withCurrentLimits(new CurrentLimitsConfigs().withStatorCurrentLimit(Amps.of(35))); static final double KICKER_MOTOR_TO_FLYWHEEL_GEARING = 2.0 / 5.0; From 7352f9a20220df53d78e55ecf0cf748985de06d5 Mon Sep 17 00:00:00 2001 From: Piyou Date: Sat, 21 Mar 2026 18:33:50 -0700 Subject: [PATCH 07/16] fixed kicker motor 2 config --- src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java b/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java index b0891ab4..02199e58 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java @@ -15,7 +15,7 @@ public KickerIOReal() { motor1.getConfigurator().apply(KickerConstants.KICKER_MOTOR_CONFIG); motor2 = new TalonFX(Constants.KICKER_MOTOR_2_ID); - motor2.getConfigurator().apply(KickerConstants.KICKER_MOTOR_CONFIG); + motor2.getConfigurator().apply(KickerConstants.KICKER_MOTOR_2_CONFIG); } @Override From 2e92bdc3b82dcebf3a50675cdf8466d1c0fa38d2 Mon Sep 17 00:00:00 2001 From: Piyou Date: Sat, 21 Mar 2026 19:20:07 -0700 Subject: [PATCH 08/16] updatrs from prototyping --- src/main/java/com/team2813/Constants.java | 2 +- .../java/com/team2813/subsystems/shooter/ShooterConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team2813/Constants.java b/src/main/java/com/team2813/Constants.java index 565f0cd4..ade749d7 100644 --- a/src/main/java/com/team2813/Constants.java +++ b/src/main/java/com/team2813/Constants.java @@ -47,7 +47,7 @@ public final class Constants { // Shooter Motors. Aliases: Flywheel motors. 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 4 are motors on the other side + 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 diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java index a1a676dd..a8bbfa27 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java @@ -71,7 +71,7 @@ public class ShooterConstants { public static final TalonFXConfiguration SHOOTER_MOTOR_3_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( From 39911a6ff81c87b4a634cb3de526775a1d4792ea Mon Sep 17 00:00:00 2001 From: Spderman3333 <118777573+spderman3333@users.noreply.github.com> Date: Mon, 23 Mar 2026 14:00:58 -0700 Subject: [PATCH 09/16] Renamed shooter motors so they are clearer --- src/main/java/com/team2813/Constants.java | 4 +- .../team2813/subsystems/shooter/Shooter.java | 4 +- .../subsystems/shooter/ShooterIO.java | 20 ++--- .../subsystems/shooter/ShooterIOReal.java | 64 +++++++------- .../subsystems/shooter/ShooterIOSim.java | 87 ++++++++++--------- 5 files changed, 94 insertions(+), 85 deletions(-) diff --git a/src/main/java/com/team2813/Constants.java b/src/main/java/com/team2813/Constants.java index 4b8a5aa8..80bf0e08 100644 --- a/src/main/java/com/team2813/Constants.java +++ b/src/main/java/com/team2813/Constants.java @@ -45,8 +45,8 @@ 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 RIGHT_MAIN_SHOOTER_MOTOR_ID = 19; // Right shooter motor. + public static final int LEFT_FOLLOWER_SHOOTER_MOTOR_ID = 20; // Left shooter motor. // Kicker Motor public static final int KICKER_MOTOR_ID = 21; diff --git a/src/main/java/com/team2813/subsystems/shooter/Shooter.java b/src/main/java/com/team2813/subsystems/shooter/Shooter.java index 875ad87d..bf16b440 100644 --- a/src/main/java/com/team2813/subsystems/shooter/Shooter.java +++ b/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.rightMainShooterMotorRotPerSec) .isNear( - RotationsPerSecond.of(replayedInputs.mainShooterSetpointRotsPerSec), + RotationsPerSecond.of(replayedInputs.rightMainShooterSetpointRotsPerSec), ShooterConstants.SHOOTER_SPOOL_SPEED_TOLERANCE); } } diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java index a0a6be55..bc153172 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java @@ -8,17 +8,17 @@ 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 rightMainShooterMotorVoltageVolts = 0; + public double rightMainShooterMotorAngleRotations = 0; + public double rightMainShooterMotorRotPerSec = 0; + public double rightMainShooterMotorStatorCurrentAmps = 0; + public double rightMainShooterMotorSupplyCurrentAmps = 0; + public double rightMainShooterSetpointRotsPerSec = 0; - public double followerShooterMotorVoltageVolts = 0; - public double followerShooterMotorRotPerSec = 0; - public double followerShooterMotorStatorCurrentAmps = 0; - public double followerShooterMotorSupplyCurrentAmps = 0; + public double leftFollowerShooterMotorVoltageVolts = 0; + public double leftFollowerShooterMotorRotPerSec = 0; + public double leftFollowerShooterMotorStatorCurrentAmps = 0; + public double leftFollowerShooterMotorSupplyCurrentAmps = 0; } /** diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java index e0fac0a0..d1f2c1ee 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java @@ -12,57 +12,61 @@ public class ShooterIOReal implements ShooterIO { // Declaring the control here saves on having to create a new object each time. private final VelocityVoltage shooterVelocityControl; - private TalonFX mainShooterMotor; - private TalonFX followerShooterMotor; + private TalonFX rightMainShooterMotor; + private TalonFX leftFollowerShooterMotor; - private AngularVelocity mainShooterSetpoint = RotationsPerSecond.of(0); + private AngularVelocity rightMainShooterSetpoint = RotationsPerSecond.of(0); public ShooterIOReal() { - mainShooterMotor = new TalonFX(Constants.MAIN_SHOOTER_MOTOR_ID); - mainShooterMotor.getConfigurator().apply(ShooterConstants.MAIN_SHOOTER_MOTOR_CONFIG); + rightMainShooterMotor = new TalonFX(Constants.RIGHT_MAIN_SHOOTER_MOTOR_ID); + rightMainShooterMotor.getConfigurator().apply(ShooterConstants.MAIN_SHOOTER_MOTOR_CONFIG); - followerShooterMotor = new TalonFX(Constants.FOLLOWER_SHOOTER_MOTOR_ID); - followerShooterMotor.getConfigurator().apply(ShooterConstants.FOLLOWER_SHOOTER_MOTOR_CONFIG); + leftFollowerShooterMotor = new TalonFX(Constants.LEFT_FOLLOWER_SHOOTER_MOTOR_ID); + leftFollowerShooterMotor + .getConfigurator() + .apply(ShooterConstants.FOLLOWER_SHOOTER_MOTOR_CONFIG); shooterVelocityControl = new VelocityVoltage(RotationsPerSecond.of(0)); } @Override public void updateState(ShooterIOInputs inputs) { - inputs.mainShooterMotorVoltageVolts = mainShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.mainShooterMotorAngleRotations = mainShooterMotor.getPosition().getValue().in(Rotations); - inputs.mainShooterMotorRotPerSec = - mainShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.mainShooterMotorStatorCurrentAmps = - mainShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.mainShooterMotorSupplyCurrentAmps = - mainShooterMotor.getSupplyCurrent().getValue().in(Amps); - inputs.mainShooterSetpointRotsPerSec = mainShooterSetpoint.in(RotationsPerSecond); + inputs.rightMainShooterMotorVoltageVolts = + rightMainShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.rightMainShooterMotorAngleRotations = + rightMainShooterMotor.getPosition().getValue().in(Rotations); + inputs.rightMainShooterMotorRotPerSec = + rightMainShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.rightMainShooterMotorStatorCurrentAmps = + rightMainShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.rightMainShooterMotorSupplyCurrentAmps = + rightMainShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.rightMainShooterSetpointRotsPerSec = rightMainShooterSetpoint.in(RotationsPerSecond); - inputs.followerShooterMotorVoltageVolts = - followerShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.followerShooterMotorRotPerSec = - followerShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.followerShooterMotorStatorCurrentAmps = - followerShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.followerShooterMotorSupplyCurrentAmps = - followerShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.leftFollowerShooterMotorVoltageVolts = + leftFollowerShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.leftFollowerShooterMotorRotPerSec = + leftFollowerShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.leftFollowerShooterMotorStatorCurrentAmps = + leftFollowerShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.leftFollowerShooterMotorSupplyCurrentAmps = + leftFollowerShooterMotor.getSupplyCurrent().getValue().in(Amps); } @Override public void setShooterMotorVelocity(AngularVelocity shooterMotorVelocity) { // Uses Rot/s rather than passing AngularVelocity because there seems to be some issue with // AngularVelocity converting its value (i.e. Rot/s) to the base unit (rad/s) - mainShooterSetpoint = shooterMotorVelocity; - mainShooterMotor.setControl(shooterVelocityControl.withVelocity(shooterMotorVelocity)); + rightMainShooterSetpoint = shooterMotorVelocity; + rightMainShooterMotor.setControl(shooterVelocityControl.withVelocity(shooterMotorVelocity)); // same velocity - followerShooterMotor.setControl(shooterVelocityControl); + leftFollowerShooterMotor.setControl(shooterVelocityControl); } @Override public void setShooterMotorVoltage(Voltage shooterVoltage) { - mainShooterSetpoint = RotationsPerSecond.of(0); - mainShooterMotor.setVoltage(shooterVoltage.in(Volts)); - followerShooterMotor.setVoltage(shooterVoltage.in(Volts)); + rightMainShooterSetpoint = RotationsPerSecond.of(0); + rightMainShooterMotor.setVoltage(shooterVoltage.in(Volts)); + leftFollowerShooterMotor.setVoltage(shooterVoltage.in(Volts)); } } diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java index 4390d2a4..65ea7a21 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java @@ -13,26 +13,28 @@ import edu.wpi.first.wpilibj.simulation.FlywheelSim; public class ShooterIOSim implements ShooterIO { - private final TalonFX mainShooterMotor; - private final TalonFXSimState mainShooterSimState; + private final TalonFX rightMainShooterMotor; + private final TalonFXSimState rightMainShooterSimState; - private final TalonFX followerShooterMotor; - private final TalonFXSimState followerShooterSimState; + private final TalonFX leftFollowerShooterMotor; + private final TalonFXSimState leftFollowerShooterSimState; private final VelocityVoltage shooterVelocityControl; private final FlywheelSim shooterSim; - private AngularVelocity mainShooterSetpoint = RotationsPerSecond.of(0); + private AngularVelocity rightMainShooterSetpoint = RotationsPerSecond.of(0); public ShooterIOSim() { - mainShooterMotor = new TalonFX(Constants.MAIN_SHOOTER_MOTOR_ID); - mainShooterMotor.getConfigurator().apply(ShooterConstants.MAIN_SHOOTER_MOTOR_CONFIG); - mainShooterSimState = mainShooterMotor.getSimState(); + rightMainShooterMotor = new TalonFX(Constants.RIGHT_MAIN_SHOOTER_MOTOR_ID); + rightMainShooterMotor.getConfigurator().apply(ShooterConstants.MAIN_SHOOTER_MOTOR_CONFIG); + rightMainShooterSimState = rightMainShooterMotor.getSimState(); - followerShooterMotor = new TalonFX(Constants.FOLLOWER_SHOOTER_MOTOR_ID); - followerShooterMotor.getConfigurator().apply(ShooterConstants.FOLLOWER_SHOOTER_MOTOR_CONFIG); - followerShooterSimState = followerShooterMotor.getSimState(); + leftFollowerShooterMotor = new TalonFX(Constants.LEFT_FOLLOWER_SHOOTER_MOTOR_ID); + leftFollowerShooterMotor + .getConfigurator() + .apply(ShooterConstants.FOLLOWER_SHOOTER_MOTOR_CONFIG); + leftFollowerShooterSimState = leftFollowerShooterMotor.getSimState(); shooterVelocityControl = new VelocityVoltage(RotationsPerSecond.of(0)); @@ -49,57 +51,60 @@ public ShooterIOSim() { public void updateState(ShooterIOInputs inputs) { updateSimulation(); - inputs.mainShooterMotorVoltageVolts = mainShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.mainShooterMotorAngleRotations = mainShooterMotor.getPosition().getValue().in(Rotations); - inputs.mainShooterMotorRotPerSec = - mainShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.mainShooterMotorStatorCurrentAmps = - mainShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.mainShooterSetpointRotsPerSec = mainShooterSetpoint.in(RotationsPerSecond); - inputs.mainShooterMotorSupplyCurrentAmps = - mainShooterMotor.getSupplyCurrent().getValue().in(Amps); - - inputs.followerShooterMotorVoltageVolts = - followerShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.followerShooterMotorRotPerSec = - followerShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.followerShooterMotorStatorCurrentAmps = - followerShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.followerShooterMotorSupplyCurrentAmps = - followerShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.rightMainShooterMotorVoltageVolts = + rightMainShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.rightMainShooterMotorAngleRotations = + rightMainShooterMotor.getPosition().getValue().in(Rotations); + inputs.rightMainShooterMotorRotPerSec = + rightMainShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.rightMainShooterMotorStatorCurrentAmps = + rightMainShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.rightMainShooterSetpointRotsPerSec = rightMainShooterSetpoint.in(RotationsPerSecond); + inputs.rightMainShooterMotorSupplyCurrentAmps = + rightMainShooterMotor.getSupplyCurrent().getValue().in(Amps); + + inputs.leftFollowerShooterMotorVoltageVolts = + leftFollowerShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.leftFollowerShooterMotorRotPerSec = + leftFollowerShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.leftFollowerShooterMotorStatorCurrentAmps = + leftFollowerShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.leftFollowerShooterMotorSupplyCurrentAmps = + leftFollowerShooterMotor.getSupplyCurrent().getValue().in(Amps); } public void updateSimulation() { // Update physics simulations every 20ms (like the actual bot). shooterSim.update(Constants.SIM_TIME_PERIOD); - mainShooterSimState.setSupplyVoltage(Volts.of(12)); - followerShooterSimState.setSupplyVoltage(Volts.of(12)); + rightMainShooterSimState.setSupplyVoltage(Volts.of(12)); + leftFollowerShooterSimState.setSupplyVoltage(Volts.of(12)); // Feed the velocity and acceleration of the roller simulation into the simulation motors to // accurately model them. - mainShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration()); - mainShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity()); + rightMainShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration()); + rightMainShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity()); // The follower roller motor is opposed with the main motor, so it gets negated values. - followerShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration().unaryMinus()); - followerShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity().unaryMinus()); + leftFollowerShooterSimState.setRotorAcceleration( + shooterSim.getAngularAcceleration().unaryMinus()); + leftFollowerShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity().unaryMinus()); } @Override public void setShooterMotorVelocity(AngularVelocity shooterMotorVelocity) { - mainShooterSetpoint = shooterMotorVelocity; - mainShooterMotor.setControl( + rightMainShooterSetpoint = shooterMotorVelocity; + rightMainShooterMotor.setControl( shooterVelocityControl.withVelocity(shooterMotorVelocity.in(RotationsPerSecond))); - followerShooterMotor.setControl(shooterVelocityControl); + leftFollowerShooterMotor.setControl(shooterVelocityControl); shooterSim.setAngularVelocity(shooterMotorVelocity.in(RadiansPerSecond)); } @Override public void setShooterMotorVoltage(Voltage shooterMotorVoltage) { - mainShooterSetpoint = RotationsPerSecond.of(0); - mainShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); - followerShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); + rightMainShooterSetpoint = RotationsPerSecond.of(0); + rightMainShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); + leftFollowerShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); shooterSim.setInputVoltage(shooterMotorVoltage.in(Volts)); } } From da6dee2e38b122b28fe720af04a25cfe5806bba6 Mon Sep 17 00:00:00 2001 From: Spderman3333 <118777573+spderman3333@users.noreply.github.com> Date: Mon, 23 Mar 2026 16:42:55 -0700 Subject: [PATCH 10/16] More name refactoring --- .../team2813/subsystems/shooter/Shooter.java | 4 +- .../subsystems/shooter/ShooterConstants.java | 35 ++++++-- .../subsystems/shooter/ShooterIO.java | 20 ++--- .../subsystems/shooter/ShooterIOReal.java | 60 +++++++------- .../subsystems/shooter/ShooterIOSim.java | 80 +++++++++---------- 5 files changed, 112 insertions(+), 87 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/shooter/Shooter.java b/src/main/java/com/team2813/subsystems/shooter/Shooter.java index bf16b440..dbdba8fa 100644 --- a/src/main/java/com/team2813/subsystems/shooter/Shooter.java +++ b/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.rightMainShooterMotorRotPerSec) + return RotationsPerSecond.of(replayedInputs.upperRightShooterMotorRotPerSec) .isNear( - RotationsPerSecond.of(replayedInputs.rightMainShooterSetpointRotsPerSec), + RotationsPerSecond.of(replayedInputs.upperRightShooterSetpointRotsPerSec), ShooterConstants.SHOOTER_SPOOL_SPEED_TOLERANCE); } } diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java index a89bc2ca..eb5644b8 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java @@ -47,11 +47,11 @@ public class ShooterConstants { Preferences.initDouble(SHOOTER_OUTTAKE_PREFERENCE_NT, -5); } - // Reminder: this is the right shooter motor when robot is viewed from behind. - public static final TalonFXConfiguration MAIN_SHOOTER_MOTOR_CONFIG = + // Reminder: this is the upper right shooter motor when robot is viewed from behind. + public static final TalonFXConfiguration UPPER_RIGHT_SHOOTER_MOTOR_CONFIG = new TalonFXConfiguration() .withMotorOutput( - new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) .withSlot0( new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) .withCurrentLimits( @@ -59,9 +59,22 @@ public class ShooterConstants { .withStatorCurrentLimit(Amps.of(70)) .withSupplyCurrentLimit(50)); - public static final TalonFXConfiguration FOLLOWER_SHOOTER_MOTOR_CONFIG = + // Reminder: this is the lower right shooter motor. + public static final TalonFXConfiguration LOWER_RIGHT_SHOOTER_MOTOR_CONFIG = new TalonFXConfiguration() - .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) + .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)) .withSlot0( new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) .withCurrentLimits( @@ -69,6 +82,18 @@ public class ShooterConstants { .withStatorCurrentLimit(Amps.of(70)) .withSupplyCurrentLimit(50)); + // Reminder: this is the lower left shooter motor. + public static final TalonFXConfiguration LOWER_LEFT_SHOOTER_MOTOR_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 double SHOOTER_MOTOR_TO_FLYWHEEL_GEARING = 1.0; public static AngularVelocity getShooterTrenchShootVelocity() { diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java index bc153172..5f681f55 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java @@ -8,17 +8,17 @@ public interface ShooterIO { @AutoLog class ShooterIOInputs { - public double rightMainShooterMotorVoltageVolts = 0; - public double rightMainShooterMotorAngleRotations = 0; - public double rightMainShooterMotorRotPerSec = 0; - public double rightMainShooterMotorStatorCurrentAmps = 0; - public double rightMainShooterMotorSupplyCurrentAmps = 0; - public double rightMainShooterSetpointRotsPerSec = 0; + public double upperRightShooterMotorVoltageVolts = 0; + public double upperRightShooterMotorAngleRotations = 0; + public double upperRightShooterMotorRotPerSec = 0; + public double upperRightShooterMotorStatorCurrentAmps = 0; + public double upperRightShooterMotorSupplyCurrentAmps = 0; + public double upperRightShooterSetpointRotsPerSec = 0; - public double leftFollowerShooterMotorVoltageVolts = 0; - public double leftFollowerShooterMotorRotPerSec = 0; - public double leftFollowerShooterMotorStatorCurrentAmps = 0; - public double leftFollowerShooterMotorSupplyCurrentAmps = 0; + public double upperLeftShooterMotorVoltageVolts = 0; + public double upperLeftShooterMotorRotPerSec = 0; + public double upperLeftShooterMotorStatorCurrentAmps = 0; + public double upperLeftShooterMotorSupplyCurrentAmps = 0; } /** diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java index d1f2c1ee..047e3556 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java @@ -12,45 +12,45 @@ public class ShooterIOReal implements ShooterIO { // Declaring the control here saves on having to create a new object each time. private final VelocityVoltage shooterVelocityControl; - private TalonFX rightMainShooterMotor; - private TalonFX leftFollowerShooterMotor; + private TalonFX upperRightShooterMotor; + private TalonFX upperLeftShooterMotor; private AngularVelocity rightMainShooterSetpoint = RotationsPerSecond.of(0); public ShooterIOReal() { - rightMainShooterMotor = new TalonFX(Constants.RIGHT_MAIN_SHOOTER_MOTOR_ID); - rightMainShooterMotor.getConfigurator().apply(ShooterConstants.MAIN_SHOOTER_MOTOR_CONFIG); + upperRightShooterMotor = new TalonFX(Constants.RIGHT_MAIN_SHOOTER_MOTOR_ID); + upperRightShooterMotor.getConfigurator().apply(ShooterConstants.UPPER_RIGHT_SHOOTER_MOTOR_CONFIG); - leftFollowerShooterMotor = new TalonFX(Constants.LEFT_FOLLOWER_SHOOTER_MOTOR_ID); - leftFollowerShooterMotor + upperLeftShooterMotor = new TalonFX(Constants.LEFT_FOLLOWER_SHOOTER_MOTOR_ID); + upperLeftShooterMotor .getConfigurator() - .apply(ShooterConstants.FOLLOWER_SHOOTER_MOTOR_CONFIG); + .apply(ShooterConstants.UPPER_LEFT_SHOOTER_MOTOR_CONFIG); shooterVelocityControl = new VelocityVoltage(RotationsPerSecond.of(0)); } @Override public void updateState(ShooterIOInputs inputs) { - inputs.rightMainShooterMotorVoltageVolts = - rightMainShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.rightMainShooterMotorAngleRotations = - rightMainShooterMotor.getPosition().getValue().in(Rotations); - inputs.rightMainShooterMotorRotPerSec = - rightMainShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.rightMainShooterMotorStatorCurrentAmps = - rightMainShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.rightMainShooterMotorSupplyCurrentAmps = - rightMainShooterMotor.getSupplyCurrent().getValue().in(Amps); - inputs.rightMainShooterSetpointRotsPerSec = rightMainShooterSetpoint.in(RotationsPerSecond); + inputs.upperRightShooterMotorVoltageVolts = + upperRightShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.upperRightShooterMotorAngleRotations = + upperRightShooterMotor.getPosition().getValue().in(Rotations); + inputs.upperRightShooterMotorRotPerSec = + upperRightShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.upperRightShooterMotorStatorCurrentAmps = + upperRightShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.upperRightShooterMotorSupplyCurrentAmps = + upperRightShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.upperRightShooterSetpointRotsPerSec = rightMainShooterSetpoint.in(RotationsPerSecond); - inputs.leftFollowerShooterMotorVoltageVolts = - leftFollowerShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.leftFollowerShooterMotorRotPerSec = - leftFollowerShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.leftFollowerShooterMotorStatorCurrentAmps = - leftFollowerShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.leftFollowerShooterMotorSupplyCurrentAmps = - leftFollowerShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.upperLeftShooterMotorVoltageVolts = + upperLeftShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.upperLeftShooterMotorRotPerSec = + upperLeftShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.upperLeftShooterMotorStatorCurrentAmps = + upperLeftShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.upperLeftShooterMotorSupplyCurrentAmps = + upperLeftShooterMotor.getSupplyCurrent().getValue().in(Amps); } @Override @@ -58,15 +58,15 @@ public void setShooterMotorVelocity(AngularVelocity shooterMotorVelocity) { // Uses Rot/s rather than passing AngularVelocity because there seems to be some issue with // AngularVelocity converting its value (i.e. Rot/s) to the base unit (rad/s) rightMainShooterSetpoint = shooterMotorVelocity; - rightMainShooterMotor.setControl(shooterVelocityControl.withVelocity(shooterMotorVelocity)); + upperRightShooterMotor.setControl(shooterVelocityControl.withVelocity(shooterMotorVelocity)); // same velocity - leftFollowerShooterMotor.setControl(shooterVelocityControl); + upperLeftShooterMotor.setControl(shooterVelocityControl); } @Override public void setShooterMotorVoltage(Voltage shooterVoltage) { rightMainShooterSetpoint = RotationsPerSecond.of(0); - rightMainShooterMotor.setVoltage(shooterVoltage.in(Volts)); - leftFollowerShooterMotor.setVoltage(shooterVoltage.in(Volts)); + upperRightShooterMotor.setVoltage(shooterVoltage.in(Volts)); + upperLeftShooterMotor.setVoltage(shooterVoltage.in(Volts)); } } diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java index 65ea7a21..a137bb02 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java @@ -13,28 +13,28 @@ import edu.wpi.first.wpilibj.simulation.FlywheelSim; public class ShooterIOSim implements ShooterIO { - private final TalonFX rightMainShooterMotor; + private final TalonFX upperRightShooterMotor; private final TalonFXSimState rightMainShooterSimState; - private final TalonFX leftFollowerShooterMotor; - private final TalonFXSimState leftFollowerShooterSimState; + private final TalonFX upperLeftShooterMotor; + private final TalonFXSimState upperLeftShooterSimState; private final VelocityVoltage shooterVelocityControl; private final FlywheelSim shooterSim; - private AngularVelocity rightMainShooterSetpoint = RotationsPerSecond.of(0); + private AngularVelocity upperRightShooterSetpoint = RotationsPerSecond.of(0); public ShooterIOSim() { - rightMainShooterMotor = new TalonFX(Constants.RIGHT_MAIN_SHOOTER_MOTOR_ID); - rightMainShooterMotor.getConfigurator().apply(ShooterConstants.MAIN_SHOOTER_MOTOR_CONFIG); - rightMainShooterSimState = rightMainShooterMotor.getSimState(); + upperRightShooterMotor = new TalonFX(Constants.RIGHT_MAIN_SHOOTER_MOTOR_ID); + upperRightShooterMotor.getConfigurator().apply(ShooterConstants.UPPER_RIGHT_SHOOTER_MOTOR_CONFIG); + rightMainShooterSimState = upperRightShooterMotor.getSimState(); - leftFollowerShooterMotor = new TalonFX(Constants.LEFT_FOLLOWER_SHOOTER_MOTOR_ID); - leftFollowerShooterMotor + upperLeftShooterMotor = new TalonFX(Constants.LEFT_FOLLOWER_SHOOTER_MOTOR_ID); + upperLeftShooterMotor .getConfigurator() - .apply(ShooterConstants.FOLLOWER_SHOOTER_MOTOR_CONFIG); - leftFollowerShooterSimState = leftFollowerShooterMotor.getSimState(); + .apply(ShooterConstants.UPPER_LEFT_SHOOTER_MOTOR_CONFIG); + upperLeftShooterSimState = upperLeftShooterMotor.getSimState(); shooterVelocityControl = new VelocityVoltage(RotationsPerSecond.of(0)); @@ -51,26 +51,26 @@ public ShooterIOSim() { public void updateState(ShooterIOInputs inputs) { updateSimulation(); - inputs.rightMainShooterMotorVoltageVolts = - rightMainShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.rightMainShooterMotorAngleRotations = - rightMainShooterMotor.getPosition().getValue().in(Rotations); - inputs.rightMainShooterMotorRotPerSec = - rightMainShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.rightMainShooterMotorStatorCurrentAmps = - rightMainShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.rightMainShooterSetpointRotsPerSec = rightMainShooterSetpoint.in(RotationsPerSecond); - inputs.rightMainShooterMotorSupplyCurrentAmps = - rightMainShooterMotor.getSupplyCurrent().getValue().in(Amps); - - inputs.leftFollowerShooterMotorVoltageVolts = - leftFollowerShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.leftFollowerShooterMotorRotPerSec = - leftFollowerShooterMotor.getVelocity().getValue().in(RotationsPerSecond); - inputs.leftFollowerShooterMotorStatorCurrentAmps = - leftFollowerShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.leftFollowerShooterMotorSupplyCurrentAmps = - leftFollowerShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.upperRightShooterMotorVoltageVolts = + upperRightShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.upperRightShooterMotorAngleRotations = + upperRightShooterMotor.getPosition().getValue().in(Rotations); + inputs.upperRightShooterMotorRotPerSec = + upperRightShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.upperRightShooterMotorStatorCurrentAmps = + upperRightShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.upperRightShooterSetpointRotsPerSec = upperRightShooterSetpoint.in(RotationsPerSecond); + inputs.upperRightShooterMotorSupplyCurrentAmps = + upperRightShooterMotor.getSupplyCurrent().getValue().in(Amps); + + inputs.upperLeftShooterMotorVoltageVolts = + upperLeftShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.upperLeftShooterMotorRotPerSec = + upperLeftShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.upperLeftShooterMotorStatorCurrentAmps = + upperLeftShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.upperLeftShooterMotorSupplyCurrentAmps = + upperLeftShooterMotor.getSupplyCurrent().getValue().in(Amps); } public void updateSimulation() { @@ -78,33 +78,33 @@ public void updateSimulation() { shooterSim.update(Constants.SIM_TIME_PERIOD); rightMainShooterSimState.setSupplyVoltage(Volts.of(12)); - leftFollowerShooterSimState.setSupplyVoltage(Volts.of(12)); + upperLeftShooterSimState.setSupplyVoltage(Volts.of(12)); // Feed the velocity and acceleration of the roller simulation into the simulation motors to // accurately model them. rightMainShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration()); rightMainShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity()); // The follower roller motor is opposed with the main motor, so it gets negated values. - leftFollowerShooterSimState.setRotorAcceleration( + upperLeftShooterSimState.setRotorAcceleration( shooterSim.getAngularAcceleration().unaryMinus()); - leftFollowerShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity().unaryMinus()); + upperLeftShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity().unaryMinus()); } @Override public void setShooterMotorVelocity(AngularVelocity shooterMotorVelocity) { - rightMainShooterSetpoint = shooterMotorVelocity; - rightMainShooterMotor.setControl( + upperRightShooterSetpoint = shooterMotorVelocity; + upperRightShooterMotor.setControl( shooterVelocityControl.withVelocity(shooterMotorVelocity.in(RotationsPerSecond))); - leftFollowerShooterMotor.setControl(shooterVelocityControl); + upperLeftShooterMotor.setControl(shooterVelocityControl); shooterSim.setAngularVelocity(shooterMotorVelocity.in(RadiansPerSecond)); } @Override public void setShooterMotorVoltage(Voltage shooterMotorVoltage) { - rightMainShooterSetpoint = RotationsPerSecond.of(0); - rightMainShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); - leftFollowerShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); + upperRightShooterSetpoint = RotationsPerSecond.of(0); + upperRightShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); + upperLeftShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); shooterSim.setInputVoltage(shooterMotorVoltage.in(Volts)); } } From f03763baea7ae01aecc5299833ba2a19cdb2ad61 Mon Sep 17 00:00:00 2001 From: Spderman3333 <118777573+spderman3333@users.noreply.github.com> Date: Mon, 23 Mar 2026 16:45:32 -0700 Subject: [PATCH 11/16] Logging upper left motor position --- .../com/team2813/subsystems/shooter/ShooterIO.java | 6 ++++-- .../team2813/subsystems/shooter/ShooterIOReal.java | 11 +++++++---- .../team2813/subsystems/shooter/ShooterIOSim.java | 14 ++++++++------ 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java index 5f681f55..52a2326b 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java @@ -8,17 +8,19 @@ public interface ShooterIO { @AutoLog class ShooterIOInputs { + public double upperRightShooterSetpointRotsPerSec = 0; + public double upperRightShooterMotorVoltageVolts = 0; - public double upperRightShooterMotorAngleRotations = 0; public double upperRightShooterMotorRotPerSec = 0; public double upperRightShooterMotorStatorCurrentAmps = 0; public double upperRightShooterMotorSupplyCurrentAmps = 0; - public double upperRightShooterSetpointRotsPerSec = 0; + public double upperRightShooterMotorAngleRotations = 0; public double upperLeftShooterMotorVoltageVolts = 0; public double upperLeftShooterMotorRotPerSec = 0; public double upperLeftShooterMotorStatorCurrentAmps = 0; public double upperLeftShooterMotorSupplyCurrentAmps = 0; + public double upperLeftShooterMotorAngleRotations = 0; } /** diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java index 047e3556..58016086 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java @@ -15,7 +15,7 @@ public class ShooterIOReal implements ShooterIO { private TalonFX upperRightShooterMotor; private TalonFX upperLeftShooterMotor; - private AngularVelocity rightMainShooterSetpoint = RotationsPerSecond.of(0); + private AngularVelocity upperRightShooterSetpoint = RotationsPerSecond.of(0); public ShooterIOReal() { upperRightShooterMotor = new TalonFX(Constants.RIGHT_MAIN_SHOOTER_MOTOR_ID); @@ -31,6 +31,8 @@ public ShooterIOReal() { @Override public void updateState(ShooterIOInputs inputs) { + inputs.upperRightShooterSetpointRotsPerSec = upperRightShooterSetpoint.in(RotationsPerSecond); + inputs.upperRightShooterMotorVoltageVolts = upperRightShooterMotor.getMotorVoltage().getValue().in(Volts); inputs.upperRightShooterMotorAngleRotations = @@ -41,7 +43,6 @@ public void updateState(ShooterIOInputs inputs) { upperRightShooterMotor.getStatorCurrent().getValue().in(Amps); inputs.upperRightShooterMotorSupplyCurrentAmps = upperRightShooterMotor.getSupplyCurrent().getValue().in(Amps); - inputs.upperRightShooterSetpointRotsPerSec = rightMainShooterSetpoint.in(RotationsPerSecond); inputs.upperLeftShooterMotorVoltageVolts = upperLeftShooterMotor.getMotorVoltage().getValue().in(Volts); @@ -51,13 +52,15 @@ public void updateState(ShooterIOInputs inputs) { upperLeftShooterMotor.getStatorCurrent().getValue().in(Amps); inputs.upperLeftShooterMotorSupplyCurrentAmps = upperLeftShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.upperLeftShooterMotorAngleRotations = + upperLeftShooterMotor.getPosition().getValue().in(Rotations); } @Override public void setShooterMotorVelocity(AngularVelocity shooterMotorVelocity) { // Uses Rot/s rather than passing AngularVelocity because there seems to be some issue with // AngularVelocity converting its value (i.e. Rot/s) to the base unit (rad/s) - rightMainShooterSetpoint = shooterMotorVelocity; + upperRightShooterSetpoint = shooterMotorVelocity; upperRightShooterMotor.setControl(shooterVelocityControl.withVelocity(shooterMotorVelocity)); // same velocity upperLeftShooterMotor.setControl(shooterVelocityControl); @@ -65,7 +68,7 @@ public void setShooterMotorVelocity(AngularVelocity shooterMotorVelocity) { @Override public void setShooterMotorVoltage(Voltage shooterVoltage) { - rightMainShooterSetpoint = RotationsPerSecond.of(0); + upperRightShooterSetpoint = RotationsPerSecond.of(0); upperRightShooterMotor.setVoltage(shooterVoltage.in(Volts)); upperLeftShooterMotor.setVoltage(shooterVoltage.in(Volts)); } diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java index a137bb02..41448f0a 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java @@ -14,7 +14,7 @@ public class ShooterIOSim implements ShooterIO { private final TalonFX upperRightShooterMotor; - private final TalonFXSimState rightMainShooterSimState; + private final TalonFXSimState upperRightShooterSimState; private final TalonFX upperLeftShooterMotor; private final TalonFXSimState upperLeftShooterSimState; @@ -28,7 +28,7 @@ public class ShooterIOSim implements ShooterIO { public ShooterIOSim() { upperRightShooterMotor = new TalonFX(Constants.RIGHT_MAIN_SHOOTER_MOTOR_ID); upperRightShooterMotor.getConfigurator().apply(ShooterConstants.UPPER_RIGHT_SHOOTER_MOTOR_CONFIG); - rightMainShooterSimState = upperRightShooterMotor.getSimState(); + upperRightShooterSimState = upperRightShooterMotor.getSimState(); upperLeftShooterMotor = new TalonFX(Constants.LEFT_FOLLOWER_SHOOTER_MOTOR_ID); upperLeftShooterMotor @@ -50,6 +50,7 @@ public ShooterIOSim() { @Override public void updateState(ShooterIOInputs inputs) { updateSimulation(); + inputs.upperRightShooterSetpointRotsPerSec = upperRightShooterSetpoint.in(RotationsPerSecond); inputs.upperRightShooterMotorVoltageVolts = upperRightShooterMotor.getMotorVoltage().getValue().in(Volts); @@ -59,7 +60,6 @@ public void updateState(ShooterIOInputs inputs) { upperRightShooterMotor.getVelocity().getValue().in(RotationsPerSecond); inputs.upperRightShooterMotorStatorCurrentAmps = upperRightShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.upperRightShooterSetpointRotsPerSec = upperRightShooterSetpoint.in(RotationsPerSecond); inputs.upperRightShooterMotorSupplyCurrentAmps = upperRightShooterMotor.getSupplyCurrent().getValue().in(Amps); @@ -71,19 +71,21 @@ public void updateState(ShooterIOInputs inputs) { upperLeftShooterMotor.getStatorCurrent().getValue().in(Amps); inputs.upperLeftShooterMotorSupplyCurrentAmps = upperLeftShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.upperLeftShooterMotorAngleRotations = + upperLeftShooterMotor.getPosition().getValue().in(Rotations); } public void updateSimulation() { // Update physics simulations every 20ms (like the actual bot). shooterSim.update(Constants.SIM_TIME_PERIOD); - rightMainShooterSimState.setSupplyVoltage(Volts.of(12)); + upperRightShooterSimState.setSupplyVoltage(Volts.of(12)); upperLeftShooterSimState.setSupplyVoltage(Volts.of(12)); // Feed the velocity and acceleration of the roller simulation into the simulation motors to // accurately model them. - rightMainShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration()); - rightMainShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity()); + upperRightShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration()); + upperRightShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity()); // The follower roller motor is opposed with the main motor, so it gets negated values. upperLeftShooterSimState.setRotorAcceleration( shooterSim.getAngularAcceleration().unaryMinus()); From f22c9093b55941f18ea0b95d80954df0e78d5456 Mon Sep 17 00:00:00 2001 From: Spderman3333 <118777573+spderman3333@users.noreply.github.com> Date: Mon, 23 Mar 2026 17:04:05 -0700 Subject: [PATCH 12/16] Prepping to add new motors to ShooterIOReal --- src/main/java/com/team2813/Constants.java | 12 +++--- .../subsystems/shooter/ShooterConstants.java | 39 +++++++++---------- .../subsystems/shooter/ShooterIOReal.java | 21 +++++++--- .../subsystems/shooter/ShooterIOSim.java | 15 ++++--- 4 files changed, 49 insertions(+), 38 deletions(-) diff --git a/src/main/java/com/team2813/Constants.java b/src/main/java/com/team2813/Constants.java index 80bf0e08..2f1604b5 100644 --- a/src/main/java/com/team2813/Constants.java +++ b/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 RIGHT_MAIN_SHOOTER_MOTOR_ID = 19; // Right shooter motor. - public static final int LEFT_FOLLOWER_SHOOTER_MOTOR_ID = 20; // Left shooter motor. + public static final int UPPER_RIGHT_SHOOTER_MOTOR_ID = 19; // Upper right shooter motor. + public static final int LOWER_RIGHT_SHOOTER_MOTOR_ID = 30; // Lower right shooter motor. + + public static final int UPPER_LEFT_SHOOTER_MOTOR_ID = 20; // Upper left shooter motor. + public static final int LOWER_LEFT_SHOOTER_MOTOR_ID = 31; // Lower left shooter motor. // Kicker Motor public static final int KICKER_MOTOR_ID = 21; // 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. diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java index eb5644b8..69710811 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java @@ -50,8 +50,7 @@ public class ShooterConstants { // Reminder: this is the upper right shooter motor when robot is viewed from behind. public static final TalonFXConfiguration UPPER_RIGHT_SHOOTER_MOTOR_CONFIG = new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) .withSlot0( new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) .withCurrentLimits( @@ -62,19 +61,19 @@ public class ShooterConstants { // Reminder: this is the lower right shooter motor. public static final TalonFXConfiguration LOWER_RIGHT_SHOOTER_MOTOR_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)); + .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)) + .withMotorOutput( + new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) .withSlot0( new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) .withCurrentLimits( @@ -84,15 +83,15 @@ public class ShooterConstants { // Reminder: this is the lower left shooter motor. public static final TalonFXConfiguration LOWER_LEFT_SHOOTER_MOTOR_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)); - + 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 double SHOOTER_MOTOR_TO_FLYWHEEL_GEARING = 1.0; diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java index 58016086..75e31610 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java @@ -13,18 +13,29 @@ public class ShooterIOReal implements ShooterIO { // Declaring the control here saves on having to create a new object each time. private final VelocityVoltage shooterVelocityControl; private TalonFX upperRightShooterMotor; + private TalonFX lowerRightShooterMotor; + private TalonFX upperLeftShooterMotor; + private TalonFX lowerLeftShooterMotor; private AngularVelocity upperRightShooterSetpoint = RotationsPerSecond.of(0); public ShooterIOReal() { - upperRightShooterMotor = new TalonFX(Constants.RIGHT_MAIN_SHOOTER_MOTOR_ID); - upperRightShooterMotor.getConfigurator().apply(ShooterConstants.UPPER_RIGHT_SHOOTER_MOTOR_CONFIG); + upperRightShooterMotor = new TalonFX(Constants.UPPER_RIGHT_SHOOTER_MOTOR_ID); + upperRightShooterMotor + .getConfigurator() + .apply(ShooterConstants.UPPER_RIGHT_SHOOTER_MOTOR_CONFIG); - upperLeftShooterMotor = new TalonFX(Constants.LEFT_FOLLOWER_SHOOTER_MOTOR_ID); - upperLeftShooterMotor + lowerRightShooterMotor = new TalonFX(Constants.LOWER_RIGHT_SHOOTER_MOTOR_ID); + lowerRightShooterMotor .getConfigurator() - .apply(ShooterConstants.UPPER_LEFT_SHOOTER_MOTOR_CONFIG); + .apply(ShooterConstants.LOWER_RIGHT_SHOOTER_MOTOR_CONFIG); + + upperLeftShooterMotor = new TalonFX(Constants.UPPER_LEFT_SHOOTER_MOTOR_ID); + upperLeftShooterMotor.getConfigurator().apply(ShooterConstants.UPPER_LEFT_SHOOTER_MOTOR_CONFIG); + + lowerLeftShooterMotor = new TalonFX(Constants.LOWER_LEFT_SHOOTER_MOTOR_ID); + lowerLeftShooterMotor.getConfigurator().apply(ShooterConstants.LOWER_LEFT_SHOOTER_MOTOR_CONFIG); shooterVelocityControl = new VelocityVoltage(RotationsPerSecond.of(0)); } diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java index 41448f0a..3bfcb7d0 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java @@ -26,14 +26,14 @@ public class ShooterIOSim implements ShooterIO { private AngularVelocity upperRightShooterSetpoint = RotationsPerSecond.of(0); public ShooterIOSim() { - upperRightShooterMotor = new TalonFX(Constants.RIGHT_MAIN_SHOOTER_MOTOR_ID); - upperRightShooterMotor.getConfigurator().apply(ShooterConstants.UPPER_RIGHT_SHOOTER_MOTOR_CONFIG); + upperRightShooterMotor = new TalonFX(Constants.UPPER_RIGHT_SHOOTER_MOTOR_ID); + upperRightShooterMotor + .getConfigurator() + .apply(ShooterConstants.UPPER_RIGHT_SHOOTER_MOTOR_CONFIG); upperRightShooterSimState = upperRightShooterMotor.getSimState(); - upperLeftShooterMotor = new TalonFX(Constants.LEFT_FOLLOWER_SHOOTER_MOTOR_ID); - upperLeftShooterMotor - .getConfigurator() - .apply(ShooterConstants.UPPER_LEFT_SHOOTER_MOTOR_CONFIG); + upperLeftShooterMotor = new TalonFX(Constants.UPPER_LEFT_SHOOTER_MOTOR_ID); + upperLeftShooterMotor.getConfigurator().apply(ShooterConstants.UPPER_LEFT_SHOOTER_MOTOR_CONFIG); upperLeftShooterSimState = upperLeftShooterMotor.getSimState(); shooterVelocityControl = new VelocityVoltage(RotationsPerSecond.of(0)); @@ -87,8 +87,7 @@ public void updateSimulation() { upperRightShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration()); upperRightShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity()); // The follower roller motor is opposed with the main motor, so it gets negated values. - upperLeftShooterSimState.setRotorAcceleration( - shooterSim.getAngularAcceleration().unaryMinus()); + upperLeftShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration().unaryMinus()); upperLeftShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity().unaryMinus()); } From 69a8814c5f1f8ad5003ff64c9ebd0275d0203d2a Mon Sep 17 00:00:00 2001 From: Spderman3333 <118777573+spderman3333@users.noreply.github.com> Date: Mon, 23 Mar 2026 17:07:40 -0700 Subject: [PATCH 13/16] Added new logging values to ShooterIO --- .../com/team2813/subsystems/shooter/ShooterIO.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java index 52a2326b..30d79f00 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java @@ -14,13 +14,25 @@ class ShooterIOInputs { public double upperRightShooterMotorRotPerSec = 0; public double upperRightShooterMotorStatorCurrentAmps = 0; public double upperRightShooterMotorSupplyCurrentAmps = 0; + public double upperRightShooterMotorAngleRotations = 0; + public double lowerRightShooterMotorVoltageVolts = 0; + public double lowerRightShooterMotorRotPerSec = 0; + public double lowerRightShooterMotorStatorCurrentAmps = 0; + public double lowerRightShooterMotorSupplyCurrentAmps = 0; + public double upperLeftShooterMotorVoltageVolts = 0; public double upperLeftShooterMotorRotPerSec = 0; public double upperLeftShooterMotorStatorCurrentAmps = 0; public double upperLeftShooterMotorSupplyCurrentAmps = 0; + public double upperLeftShooterMotorAngleRotations = 0; + + public double lowerLeftShooterMotorVoltageVolts = 0; + public double lowerLeftShooterMotorRotPerSec = 0; + public double lowerLeftShooterMotorStatorCurrentAmps = 0; + public double lowerLeftShooterMotorSupplyCurrentAmps = 0; } /** From 54ad251ce52ff08a40a84529e17638337ba667fa Mon Sep 17 00:00:00 2001 From: Spderman3333 <118777573+spderman3333@users.noreply.github.com> Date: Mon, 23 Mar 2026 17:18:47 -0700 Subject: [PATCH 14/16] Created protype code for the wide shooter --- .../subsystems/shooter/ShooterIOReal.java | 27 +++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java index 75e31610..edbeca19 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java @@ -46,8 +46,6 @@ public void updateState(ShooterIOInputs inputs) { inputs.upperRightShooterMotorVoltageVolts = upperRightShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.upperRightShooterMotorAngleRotations = - upperRightShooterMotor.getPosition().getValue().in(Rotations); inputs.upperRightShooterMotorRotPerSec = upperRightShooterMotor.getVelocity().getValue().in(RotationsPerSecond); inputs.upperRightShooterMotorStatorCurrentAmps = @@ -55,6 +53,18 @@ public void updateState(ShooterIOInputs inputs) { inputs.upperRightShooterMotorSupplyCurrentAmps = upperRightShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.upperRightShooterMotorAngleRotations = + upperRightShooterMotor.getPosition().getValue().in(Rotations); + + inputs.lowerRightShooterMotorVoltageVolts = + lowerRightShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.lowerRightShooterMotorRotPerSec = + lowerRightShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.lowerRightShooterMotorStatorCurrentAmps = + lowerRightShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.lowerRightShooterMotorSupplyCurrentAmps = + lowerRightShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.upperLeftShooterMotorVoltageVolts = upperLeftShooterMotor.getMotorVoltage().getValue().in(Volts); inputs.upperLeftShooterMotorRotPerSec = @@ -63,8 +73,18 @@ public void updateState(ShooterIOInputs inputs) { upperLeftShooterMotor.getStatorCurrent().getValue().in(Amps); inputs.upperLeftShooterMotorSupplyCurrentAmps = upperLeftShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.upperLeftShooterMotorAngleRotations = upperLeftShooterMotor.getPosition().getValue().in(Rotations); + + inputs.lowerLeftShooterMotorVoltageVolts = + lowerLeftShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.lowerLeftShooterMotorRotPerSec = + lowerLeftShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.lowerLeftShooterMotorStatorCurrentAmps = + lowerLeftShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.lowerLeftShooterMotorSupplyCurrentAmps = + lowerLeftShooterMotor.getSupplyCurrent().getValue().in(Amps); } @Override @@ -81,6 +101,9 @@ public void setShooterMotorVelocity(AngularVelocity shooterMotorVelocity) { public void setShooterMotorVoltage(Voltage shooterVoltage) { upperRightShooterSetpoint = RotationsPerSecond.of(0); upperRightShooterMotor.setVoltage(shooterVoltage.in(Volts)); + lowerRightShooterMotor.setVoltage(shooterVoltage.in(Volts)); + upperLeftShooterMotor.setVoltage(shooterVoltage.in(Volts)); + lowerLeftShooterMotor.setVoltage(shooterVoltage.in(Volts)); } } From ede1e8bb042b46d35cdd5bb1577d673c15ef0bdb Mon Sep 17 00:00:00 2001 From: Spderman3333 <118777573+spderman3333@users.noreply.github.com> Date: Mon, 23 Mar 2026 17:38:57 -0700 Subject: [PATCH 15/16] Updated sim shooter --- .../subsystems/shooter/ShooterIOSim.java | 57 +++++++++++++++++-- 1 file changed, 53 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java index 3bfcb7d0..dda9d730 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java @@ -16,9 +16,15 @@ public class ShooterIOSim implements ShooterIO { private final TalonFX upperRightShooterMotor; private final TalonFXSimState upperRightShooterSimState; + private final TalonFX lowerRightShooterMotor; + private final TalonFXSimState lowerRightShooterSimState; + private final TalonFX upperLeftShooterMotor; private final TalonFXSimState upperLeftShooterSimState; + private final TalonFX lowerLeftShooterMotor; + private final TalonFXSimState lowerLeftShooterSimState; + private final VelocityVoltage shooterVelocityControl; private final FlywheelSim shooterSim; @@ -32,19 +38,29 @@ public ShooterIOSim() { .apply(ShooterConstants.UPPER_RIGHT_SHOOTER_MOTOR_CONFIG); upperRightShooterSimState = upperRightShooterMotor.getSimState(); + lowerRightShooterMotor = new TalonFX(Constants.LOWER_RIGHT_SHOOTER_MOTOR_ID); + lowerRightShooterMotor + .getConfigurator() + .apply(ShooterConstants.LOWER_RIGHT_SHOOTER_MOTOR_CONFIG); + lowerRightShooterSimState = lowerRightShooterMotor.getSimState(); + upperLeftShooterMotor = new TalonFX(Constants.UPPER_LEFT_SHOOTER_MOTOR_ID); upperLeftShooterMotor.getConfigurator().apply(ShooterConstants.UPPER_LEFT_SHOOTER_MOTOR_CONFIG); upperLeftShooterSimState = upperLeftShooterMotor.getSimState(); + lowerLeftShooterMotor = new TalonFX(Constants.LOWER_LEFT_SHOOTER_MOTOR_ID); + lowerLeftShooterMotor.getConfigurator().apply(ShooterConstants.LOWER_LEFT_SHOOTER_MOTOR_CONFIG); + lowerLeftShooterSimState = lowerLeftShooterMotor.getSimState(); + shooterVelocityControl = new VelocityVoltage(RotationsPerSecond.of(0)); shooterSim = new FlywheelSim( LinearSystemId.createFlywheelSystem( - DCMotor.getKrakenX60(2), + DCMotor.getKrakenX60(4), ShooterConstants.SHOOTER_SIM_MOI, // "Moment of Inertia" taken from OnShape. ShooterConstants.SHOOTER_MOTOR_TO_FLYWHEEL_GEARING), - DCMotor.getKrakenX60(2)); + DCMotor.getKrakenX60(4)); } @Override @@ -54,8 +70,6 @@ public void updateState(ShooterIOInputs inputs) { inputs.upperRightShooterMotorVoltageVolts = upperRightShooterMotor.getMotorVoltage().getValue().in(Volts); - inputs.upperRightShooterMotorAngleRotations = - upperRightShooterMotor.getPosition().getValue().in(Rotations); inputs.upperRightShooterMotorRotPerSec = upperRightShooterMotor.getVelocity().getValue().in(RotationsPerSecond); inputs.upperRightShooterMotorStatorCurrentAmps = @@ -63,6 +77,18 @@ public void updateState(ShooterIOInputs inputs) { inputs.upperRightShooterMotorSupplyCurrentAmps = upperRightShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.upperRightShooterMotorAngleRotations = + upperRightShooterMotor.getPosition().getValue().in(Rotations); + + inputs.lowerRightShooterMotorVoltageVolts = + lowerRightShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.lowerRightShooterMotorRotPerSec = + lowerRightShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.lowerRightShooterMotorStatorCurrentAmps = + lowerRightShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.lowerRightShooterMotorSupplyCurrentAmps = + lowerRightShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.upperLeftShooterMotorVoltageVolts = upperLeftShooterMotor.getMotorVoltage().getValue().in(Volts); inputs.upperLeftShooterMotorRotPerSec = @@ -71,8 +97,18 @@ public void updateState(ShooterIOInputs inputs) { upperLeftShooterMotor.getStatorCurrent().getValue().in(Amps); inputs.upperLeftShooterMotorSupplyCurrentAmps = upperLeftShooterMotor.getSupplyCurrent().getValue().in(Amps); + inputs.upperLeftShooterMotorAngleRotations = upperLeftShooterMotor.getPosition().getValue().in(Rotations); + + inputs.lowerLeftShooterMotorVoltageVolts = + lowerLeftShooterMotor.getMotorVoltage().getValue().in(Volts); + inputs.lowerLeftShooterMotorRotPerSec = + lowerLeftShooterMotor.getVelocity().getValue().in(RotationsPerSecond); + inputs.lowerLeftShooterMotorStatorCurrentAmps = + lowerLeftShooterMotor.getStatorCurrent().getValue().in(Amps); + inputs.lowerLeftShooterMotorSupplyCurrentAmps = + lowerLeftShooterMotor.getSupplyCurrent().getValue().in(Amps); } public void updateSimulation() { @@ -80,15 +116,24 @@ public void updateSimulation() { shooterSim.update(Constants.SIM_TIME_PERIOD); upperRightShooterSimState.setSupplyVoltage(Volts.of(12)); + lowerRightShooterSimState.setSupplyVoltage(Volts.of(12)); upperLeftShooterSimState.setSupplyVoltage(Volts.of(12)); + lowerLeftShooterSimState.setSupplyVoltage(Volts.of(12)); // Feed the velocity and acceleration of the roller simulation into the simulation motors to // accurately model them. upperRightShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration()); upperRightShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity()); + + lowerLeftShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration()); + lowerLeftShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity()); + // The follower roller motor is opposed with the main motor, so it gets negated values. upperLeftShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration().unaryMinus()); upperLeftShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity().unaryMinus()); + + lowerLeftShooterSimState.setRotorAcceleration(shooterSim.getAngularAcceleration().unaryMinus()); + lowerLeftShooterSimState.setRotorVelocity(shooterSim.getAngularVelocity().unaryMinus()); } @Override @@ -96,7 +141,9 @@ public void setShooterMotorVelocity(AngularVelocity shooterMotorVelocity) { upperRightShooterSetpoint = shooterMotorVelocity; upperRightShooterMotor.setControl( shooterVelocityControl.withVelocity(shooterMotorVelocity.in(RotationsPerSecond))); + lowerRightShooterMotor.setControl(shooterVelocityControl); upperLeftShooterMotor.setControl(shooterVelocityControl); + lowerLeftShooterMotor.setControl(shooterVelocityControl); shooterSim.setAngularVelocity(shooterMotorVelocity.in(RadiansPerSecond)); } @@ -105,7 +152,9 @@ public void setShooterMotorVelocity(AngularVelocity shooterMotorVelocity) { public void setShooterMotorVoltage(Voltage shooterMotorVoltage) { upperRightShooterSetpoint = RotationsPerSecond.of(0); upperRightShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); + lowerRightShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); upperLeftShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); + lowerRightShooterMotor.setVoltage(shooterMotorVoltage.in(Volts)); shooterSim.setInputVoltage(shooterMotorVoltage.in(Volts)); } } From e94cd2ea9ffb6dbaa768101679a23bda51a7802b Mon Sep 17 00:00:00 2001 From: Piyou Date: Mon, 23 Mar 2026 19:30:03 -0700 Subject: [PATCH 16/16] spotless apply --- .../java/com/team2813/subsystems/shooter/ShooterConstants.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java index a8bbfa27..47fbc188 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java @@ -71,7 +71,8 @@ public class ShooterConstants { public static final TalonFXConfiguration SHOOTER_MOTOR_3_CONFIG = new TalonFXConfiguration() - .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) + .withMotorOutput( + new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) .withSlot0( new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) .withCurrentLimits(