diff --git a/src/main/java/com/team2813/Constants.java b/src/main/java/com/team2813/Constants.java index 4b8a5aa8..29d17999 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 MAIN_SHOOTER_MOTOR_ID = 19; // Right shooter motor. - public static final int FOLLOWER_SHOOTER_MOTOR_ID = 20; // Left shooter motor. + public static final int SHOOTER_MOTOR_1_ID = 19; + public static final int SHOOTER_MOTOR_2_ID = 20; + public static final int SHOOTER_MOTOR_3_ID = 32; // 2 and 3 are motors on the other side + public static final int SHOOTER_MOTOR_4_ID = 33; // Kicker Motor - public static final int KICKER_MOTOR_ID = 21; + public static final int KICKER_MOTOR_1_ID = 21; + public static final int KICKER_MOTOR_2_ID = 34; // Climb motors - // TODO(Stefan): ID these motors! - public static final int INNER_CLIMB_MOTOR_ID = 30; - public static final int OUTER_CLIMB_MOTOR_ID = 31; + // public static final int INNER_CLIMB_MOTOR_ID = 30; + // public static final int OUTER_CLIMB_MOTOR_ID = 31; /** * Returns true if the robot is on the red alliance. diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index 86af2b2e..ab9e53c9 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -161,6 +161,7 @@ public RobotContainer( intakeExtension = new IntakeExtension(new IntakeExtensionIOSim()); intakeRoller = new IntakeRoller(new IntakeRollerIOSim()); + // todo add sim code for shooter and kicker if we go with the drum shooter shooter = new Shooter(new ShooterIOSim()); kicker = new Kicker(new KickerIOSim()); break; @@ -265,6 +266,7 @@ private void configureButtonBindings() { operatorController.rightTrigger().whileTrue(shooter.spoolShooterTrenchSpeedCommand()); operatorController.x().whileTrue(shooter.spoolShooterHubSpeedCommand()); operatorController.y().whileTrue(shooter.spoolShooterHerdSpeedCommand()); + operatorController.a().whileTrue(kicker.shootCommand()); // Driver controls // Default command, normal field-relative drive diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java b/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java index 083bbb49..66d9b3e7 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java @@ -15,6 +15,12 @@ class KickerConstants { static final String RESIST_FUEL_PREFERENCE_NT = "Kicker/RESIST_FUEL_VOLTAGE"; static final TalonFXConfiguration KICKER_MOTOR_CONFIG = + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) + .withCurrentLimits(new CurrentLimitsConfigs().withStatorCurrentLimit(Amps.of(35))); + + static final TalonFXConfiguration KICKER_MOTOR_2_CONFIG = new TalonFXConfiguration() .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) .withCurrentLimits(new CurrentLimitsConfigs().withStatorCurrentLimit(Amps.of(35))); diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerIO.java b/src/main/java/com/team2813/subsystems/kicker/KickerIO.java index 0028aad8..4c544c40 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerIO.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerIO.java @@ -10,10 +10,15 @@ public interface KickerIO extends AutoCloseable { @AutoLog class KickerIOInputs { - public Voltage motorVoltage = Volts.of(0); - public AngularVelocity motorRotationalVelocity = RotationsPerSecond.of(0); - public Current motorStatorCurrent = Amps.of(0); - public Current motorSupplyCurrent = Amps.of(0); + public Voltage motor1Voltage = Volts.of(0); + public AngularVelocity motor1RotationalVelocity = RotationsPerSecond.of(0); + public Current motor1StatorCurrent = Amps.of(0); + public Current motor1SupplyCurrent = Amps.of(0); + + public Voltage motor2Voltage = Volts.of(0); + public AngularVelocity motor2RotationalVelocity = RotationsPerSecond.of(0); + public Current motor2StatorCurrent = Amps.of(0); + public Current motor2SupplyCurrent = Amps.of(0); } @Override diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java b/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java index 66b2578b..e082326c 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerIOReal.java @@ -7,28 +7,39 @@ import edu.wpi.first.units.measure.Voltage; public class KickerIOReal implements KickerIO { - private final TalonFX motor; + private final TalonFX motor1; + private final TalonFX motor2; public KickerIOReal() { - motor = new TalonFX(Constants.KICKER_MOTOR_ID); - motor.getConfigurator().apply(KickerConstants.KICKER_MOTOR_CONFIG); + motor1 = new TalonFX(Constants.KICKER_MOTOR_1_ID); + motor1.getConfigurator().apply(KickerConstants.KICKER_MOTOR_CONFIG); + + motor2 = new TalonFX(Constants.KICKER_MOTOR_2_ID); + motor2.getConfigurator().apply(KickerConstants.KICKER_MOTOR_2_CONFIG); } @Override public void updateState(KickerIOInputs inputs) { - inputs.motorVoltage = motor.getMotorVoltage().getValue(); - inputs.motorRotationalVelocity = motor.getVelocity().getValue(); - inputs.motorStatorCurrent = motor.getStatorCurrent().getValue(); - inputs.motorSupplyCurrent = motor.getSupplyCurrent().getValue(); + inputs.motor1Voltage = motor1.getMotorVoltage().getValue(); + inputs.motor1RotationalVelocity = motor1.getVelocity().getValue(); + inputs.motor1StatorCurrent = motor1.getStatorCurrent().getValue(); + inputs.motor1SupplyCurrent = motor1.getSupplyCurrent().getValue(); + + inputs.motor2Voltage = motor2.getMotorVoltage().getValue(); + inputs.motor2RotationalVelocity = motor2.getVelocity().getValue(); + inputs.motor2StatorCurrent = motor2.getStatorCurrent().getValue(); + inputs.motor2SupplyCurrent = motor2.getSupplyCurrent().getValue(); } @Override public void setMotorVoltage(Voltage kickerMotorVoltage) { - motor.setVoltage(kickerMotorVoltage.in(Volts)); + motor1.setVoltage(kickerMotorVoltage.in(Volts)); + motor2.setVoltage(kickerMotorVoltage.in(Volts)); } @Override public void close() { - motor.close(); + motor1.close(); + motor2.close(); } } diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java b/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java index b20a3900..f7c6cb8c 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerIOSim.java @@ -1,60 +1,7 @@ -package com.team2813.subsystems.kicker; - -import static edu.wpi.first.units.Units.KilogramSquareMeters; -import static edu.wpi.first.units.Units.Volts; +// todo re add sim code -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.sim.TalonFXSimState; -import com.team2813.Constants; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; +package com.team2813.subsystems.kicker; public class KickerIOSim implements KickerIO { - private final FlywheelSim flywheelSim; - private final TalonFX motor; - - public KickerIOSim() { - motor = new TalonFX(Constants.KICKER_MOTOR_ID); - flywheelSim = - new FlywheelSim( - LinearSystemId.createFlywheelSystem( - DCMotor.getKrakenX60(1), - KickerConstants.KICKER_SIM_MOI.in( - KilogramSquareMeters), // "Moment of Inertia" taken from OnShape. - KickerConstants.KICKER_MOTOR_TO_FLYWHEEL_GEARING), - DCMotor.getKrakenX60(1)); - } - - @Override - public void setMotorVoltage(Voltage kickerMotorVoltage) { - double volts = kickerMotorVoltage.in(Volts); - motor.setVoltage(volts); - flywheelSim.setInputVoltage(volts); - } - - @Override - public void updateState(KickerIOInputs inputs) { - updateSimulation(); - - inputs.motorVoltage = motor.getMotorVoltage().getValue(); - inputs.motorRotationalVelocity = motor.getVelocity().getValue(); - inputs.motorStatorCurrent = motor.getStatorCurrent().getValue(); - inputs.motorSupplyCurrent = motor.getSupplyCurrent().getValue(); - } - - private void updateSimulation() { - flywheelSim.update(Constants.SIM_TIME_PERIOD); - - TalonFXSimState simState = motor.getSimState(); - simState.setRotorAcceleration(flywheelSim.getAngularAcceleration()); - simState.setRotorVelocity(flywheelSim.getAngularVelocity()); - simState.setSupplyVoltage(Volts.of(12)); - } - - @Override - public void close() { - motor.close(); - } + // todo implement sim code if drum shooter is used } 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 24b54c42..cbcf9316 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java @@ -48,7 +48,49 @@ public class ShooterConstants { } // Reminder: this is the right shooter motor when robot is viewed from behind. - public static final TalonFXConfiguration MAIN_SHOOTER_MOTOR_CONFIG = + public static final TalonFXConfiguration SHOOTER_MOTOR_1_CONFIG = + new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) + .withSlot0( + new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(70)) + .withSupplyCurrentLimit(50)); + + public static final TalonFXConfiguration SHOOTER_MOTOR_2_CONFIG = + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) + .withSlot0( + new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(70)) + .withSupplyCurrentLimit(50)); + + public static final TalonFXConfiguration SHOOTER_MOTOR_3_CONFIG = + new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) + .withSlot0( + new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(70)) + .withSupplyCurrentLimit(50)); + + public static final TalonFXConfiguration SHOOTER_MOTOR_4_CONFIG = + new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) + .withSlot0( + new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(70)) + .withSupplyCurrentLimit(50)); + + // Reminder: this is the upper left shooter motor when the robot is viewed from behind. + public static final TalonFXConfiguration UPPER_LEFT_SHOOTER_MOTOR_CONFIG = new TalonFXConfiguration() .withMotorOutput( new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) @@ -59,9 +101,11 @@ public class ShooterConstants { .withStatorCurrentLimit(Amps.of(80)) .withSupplyCurrentLimit(60)); - public static final TalonFXConfiguration FOLLOWER_SHOOTER_MOTOR_CONFIG = + // Reminder: this is the lower left shooter motor. + public static final TalonFXConfiguration LOWER_LEFT_SHOOTER_MOTOR_CONFIG = new TalonFXConfiguration() - .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) + .withMotorOutput( + new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) .withSlot0( new Slot0Configs().withKS(0.099892).withKV(0.115).withKA(0.0020241).withKP(0.026743)) .withCurrentLimits( diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java index a0a6be55..9c7194b6 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIO.java @@ -8,17 +8,33 @@ public interface ShooterIO { @AutoLog class ShooterIOInputs { - public double mainShooterMotorVoltageVolts = 0; - public double mainShooterMotorAngleRotations = 0; - public double mainShooterMotorRotPerSec = 0; - public double mainShooterMotorStatorCurrentAmps = 0; - public double mainShooterMotorSupplyCurrentAmps = 0; - public double mainShooterSetpointRotsPerSec = 0; - - public double followerShooterMotorVoltageVolts = 0; - public double followerShooterMotorRotPerSec = 0; - public double followerShooterMotorStatorCurrentAmps = 0; - public double followerShooterMotorSupplyCurrentAmps = 0; + public double shooterMotor1VoltageVolts = 0; + public double shooterMotor1AngleRotations = 0; + public double shooterMotor1RotPerSec = 0; + public double shooterMotor1SupplyCurrentAmps = 0; + public double shooterMotor1StatorCurrentAmps = 0; + public double shooter1SetpointRotsPerSec = 0; + + public double shooterMotor2VoltageVolts = 0; + public double shooterMotor2AngleRotations = 0; + public double shooterMotor2RotPerSec = 0; + public double shooterMotor2SupplyCurrentAmps = 0; + public double shooterMotor2StatorCurrentAmps = 0; + public double shooter2SetpointRotsPerSec = 0; + + public double shooterMotor3VoltageVolts = 0; + public double shooterMotor3AngleRotations = 0; + public double shooterMotor3RotPerSec = 0; + public double shooterMotor3SupplyCurrentAmps = 0; + public double shooterMotor3StatorCurrentAmps = 0; + public double shooter3SetpointRotsPerSec = 0; + + public double shooterMotor4VoltageVolts = 0; + public double shooterMotor4AngleRotations = 0; + public double shooterMotor4RotPerSec = 0; + public double shooterMotor4SupplyCurrentAmps = 0; + public double shooterMotor4StatorCurrentAmps = 0; + public double shooter4SetpointRotsPerSec = 0; } /** diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java index e0fac0a0..3d314e51 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOReal.java @@ -12,41 +12,58 @@ 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.mainShooterMotorStatorCurrentAmps = - mainShooterMotor.getStatorCurrent().getValue().in(Amps); - inputs.mainShooterMotorSupplyCurrentAmps = - mainShooterMotor.getSupplyCurrent().getValue().in(Amps); - inputs.mainShooterSetpointRotsPerSec = mainShooterSetpoint.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.shooterMotor1VoltageVolts = shooterMotor1.getMotorVoltage().getValue().in(Volts); + inputs.shooterMotor1AngleRotations = shooterMotor1.getPosition().getValue().in(Rotations); + inputs.shooterMotor1RotPerSec = shooterMotor1.getVelocity().getValue().in(RotationsPerSecond); + inputs.shooterMotor1StatorCurrentAmps = shooterMotor1.getStatorCurrent().getValue().in(Amps); + inputs.shooterMotor1SupplyCurrentAmps = shooterMotor1.getSupplyCurrent().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.shooterMotor2StatorCurrentAmps = shooterMotor2.getStatorCurrent().getValue().in(Amps); + inputs.shooterMotor2SupplyCurrentAmps = shooterMotor2.getSupplyCurrent().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.shooterMotor3StatorCurrentAmps = shooterMotor3.getStatorCurrent().getValue().in(Amps); + inputs.shooterMotor3SupplyCurrentAmps = shooterMotor3.getSupplyCurrent().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.shooterMotor4StatorCurrentAmps = shooterMotor4.getStatorCurrent().getValue().in(Amps); + inputs.shooterMotor4SupplyCurrentAmps = shooterMotor4.getSupplyCurrent().getValue().in(Amps); + inputs.shooter4SetpointRotsPerSec = mainShooterSetpoint.in(RotationsPerSecond); } @Override @@ -54,15 +71,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 4390d2a4..89eaa3a6 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterIOSim.java @@ -1,105 +1,8 @@ +// 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; - - 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(); - - 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); - } - - 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)); - - // 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 }