diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a09c1e2..f3846ab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -32,24 +32,25 @@ import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOBoron; import frc.robot.subsystems.drive.ModuleIO; -import frc.robot.subsystems.drive.ModuleIOSim; +import frc.robot.subsystems.drive.ModuleIOSimWPI; import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.feeder.Feeder; import frc.robot.subsystems.feeder.KickerIO; -import frc.robot.subsystems.feeder.KickerIOSim; +import frc.robot.subsystems.feeder.KickerIOSimSpark; import frc.robot.subsystems.feeder.SpindexerIO; -import frc.robot.subsystems.feeder.SpindexerIOSim; +import frc.robot.subsystems.feeder.SpindexerIOSimSpark; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeRollerIO; -import frc.robot.subsystems.intake.IntakeRollerIOSim; +import frc.robot.subsystems.intake.IntakeRollerIOSimTalonFX; import frc.robot.subsystems.launcher.FlywheelIO; -import frc.robot.subsystems.launcher.FlywheelIOSim; +import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX; +import frc.robot.subsystems.launcher.FlywheelIOSimWPI; import frc.robot.subsystems.launcher.HoodIO; -import frc.robot.subsystems.launcher.HoodIOSim; -import frc.robot.subsystems.launcher.HoodIOSimHardwareless; +import frc.robot.subsystems.launcher.HoodIOSimSpark; +import frc.robot.subsystems.launcher.HoodIOSimWPI; import frc.robot.subsystems.launcher.Launcher; import frc.robot.subsystems.launcher.TurretIO; -import frc.robot.subsystems.launcher.TurretIOSim; +import frc.robot.subsystems.launcher.TurretIOSimSpark; import frc.robot.subsystems.launcher.TurretIOSpark; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; @@ -132,8 +133,8 @@ public Robot() { drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSpark(), - new FlywheelIOSim(), - new HoodIOSimHardwareless()); + new FlywheelIOSimWPI(), + new HoodIOSimWPI()); intake = new Intake(new IntakeRollerIO() {}); feeder = new Feeder(new SpindexerIO() {}, new KickerIO() {}); break; @@ -146,10 +147,10 @@ public Robot() { drive = new Drive( new GyroIO() {}, - new ModuleIOSim(DriveConstants.FrontLeft), - new ModuleIOSim(DriveConstants.FrontRight), - new ModuleIOSim(DriveConstants.BackLeft), - new ModuleIOSim(DriveConstants.BackRight)); + new ModuleIOSimWPI(DriveConstants.FrontLeft), + new ModuleIOSimWPI(DriveConstants.FrontRight), + new ModuleIOSimWPI(DriveConstants.BackLeft), + new ModuleIOSimWPI(DriveConstants.BackRight)); vision = new Vision( drive::addVisionMeasurement, @@ -166,11 +167,11 @@ public Robot() { new Launcher( drive::getPose, drive::getRobotRelativeChassisSpeeds, - new TurretIOSim(), - new FlywheelIOSim(), - new HoodIOSim()); - feeder = new Feeder(new SpindexerIOSim(), new KickerIOSim()); - intake = new Intake(new IntakeRollerIOSim()); + new TurretIOSimSpark(), + new FlywheelIOSimTalonFX(), + new HoodIOSimSpark()); + feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); + intake = new Intake(new IntakeRollerIOSimTalonFX()); break; case REPLAY: // Replaying a log @@ -223,6 +224,12 @@ public Robot() { SmartDashboard.putData( "Align Encoders", new InstantCommand(() -> drive.zeroAbsoluteEncoders()).ignoringDisable(true)); + SmartDashboard.putData(CommandScheduler.getInstance()); + SmartDashboard.putData(drive); + SmartDashboard.putData(vision); + SmartDashboard.putData(launcher); + SmartDashboard.putData(feeder); + SmartDashboard.putData(intake); SmartDashboard.putData("Field", field); Field.plotRegions(); @@ -230,16 +237,8 @@ public Robot() { Commands.run( () -> launcher.aim(GameState.getTarget(drive.getPose()).getTranslation()), launcher) .withName("Aim at hub")); - feeder.setDefaultCommand(Commands.run(feeder::spinForward, feeder).withName("Spin forward")); - - intake.setDefaultCommand(Commands.run(intake::intake, intake).withName("Intake Command")); - SmartDashboard.putData(CommandScheduler.getInstance()); - SmartDashboard.putData(drive); - SmartDashboard.putData(vision); - SmartDashboard.putData(launcher); - SmartDashboard.putData(feeder); - SmartDashboard.putData(intake); + intake.setDefaultCommand(Commands.run(intake::intakeFuel, intake).withName("Intake fuel")); } /** This function is called periodically during all modes. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java similarity index 98% rename from src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java rename to src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java index 9331cc5..b190552 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java @@ -26,7 +26,7 @@ import edu.wpi.first.wpilibj.simulation.DCMotorSim; /** Physics sim implementation of module IO. */ -public class ModuleIOSim implements ModuleIO { +public class ModuleIOSimWPI implements ModuleIO { // TunerConstants doesn't support separate sim constants, so they are declared locally private static final double DRIVE_KP = 0.05; private static final double DRIVE_KD = 0.0; @@ -50,7 +50,7 @@ public class ModuleIOSim implements ModuleIO { private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; - public ModuleIOSim( + public ModuleIOSimWPI( SwerveModuleConstants constants) { // Create drive and turn sim models diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSim.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java similarity index 97% rename from src/main/java/frc/robot/subsystems/feeder/KickerIOSim.java rename to src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java index b9e8ca1..f633f73 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSim.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java @@ -23,7 +23,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class KickerIOSim implements KickerIO { +public class KickerIOSimSpark implements KickerIO { private final DCMotorSim kickerSim; @@ -31,7 +31,7 @@ public class KickerIOSim implements KickerIO { private final SparkClosedLoopController controller; private final SparkFlexSim flexSim; - public KickerIOSim() { + public KickerIOSimSpark() { flex = new SparkFlex(CAN2.kicker, MotorType.kBrushless); controller = flex.getClosedLoopController(); diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSim.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java similarity index 97% rename from src/main/java/frc/robot/subsystems/feeder/SpindexerIOSim.java rename to src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java index 126dd8b..f8b3298 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSim.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java @@ -23,7 +23,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class SpindexerIOSim implements SpindexerIO { +public class SpindexerIOSimSpark implements SpindexerIO { private final DCMotorSim spindexerSim; @@ -31,7 +31,7 @@ public class SpindexerIOSim implements SpindexerIO { private final SparkClosedLoopController controller; private final SparkFlexSim flexSim; - public SpindexerIOSim() { + public SpindexerIOSimSpark() { flex = new SparkFlex(CAN2.spindexer, MotorType.kBrushless); controller = flex.getClosedLoopController(); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index c06ab12..adfb677 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -33,7 +33,7 @@ public void stop() { io.setOpenLoop(0.0); } - public void intake() { + public void intakeFuel() { io.setVelocity(MetersPerSecond.of(1.0)); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 6c88b94..f8c79b9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.intake; import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; @@ -8,28 +9,26 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import frc.robot.Constants.MotorConstants.KrakenX60Constants; +import frc.robot.Constants.RobotConstants; public class IntakeConstants { public class IntakeRoller { - public static final double gearboxRatio = 1.0; - public static final Distance rollerRadius = Inches.of(3.0); + public static final Distance rollerRadius = Inches.of(0.75); - // gains + // motor controller + public static final double motorReduction = 1.0; + public static final AngularVelocity maxAngularVelocity = + KrakenX60Constants.kFreeSpeed.div(motorReduction); public static final Slot0Configs intakeGains = new Slot0Configs() - .withKP(1.0) + .withKP(0.1) .withKI(0.0) - .withKD(1.0) + .withKD(0.05) .withKS(0.0) - .withKV(0.0) + .withKV(RobotConstants.kNominalVoltage / maxAngularVelocity.in(RotationsPerSecond)) .withKA(0.0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // motor controller - public static final double motorReudction = 1.0; - public static final AngularVelocity maxAngularVelocity = - KrakenX60Constants.kFreeSpeed.div(motorReudction); - // simulation public static final DCMotor gearbox = DCMotor.getKrakenX60(1); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java similarity index 64% rename from src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSim.java rename to src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java index 18f3213..9313bf6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSimTalonFX.java @@ -1,49 +1,49 @@ package frc.robot.subsystems.intake; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.intake.IntakeConstants.IntakeRoller.*; -import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.motorReduction; import static frc.robot.util.PhoenixUtil.tryUntilOk; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class IntakeRollerIOSim implements IntakeRollerIO { +public class IntakeRollerIOSimTalonFX implements IntakeRollerIO { private final DCMotorSim intakeRollerSim; private final TalonFX intakeMotor; private final TalonFXConfiguration config; + private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); private final VoltageOut voltageRequest = new VoltageOut(0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + // private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + // new VelocityTorqueCurrentFOC(0.0); // Inputs from intake motor private final StatusSignal intakeVelocity; private final StatusSignal intakeAppliedVolts; private final StatusSignal intakeCurrent; - public IntakeRollerIOSim() { + public IntakeRollerIOSimTalonFX() { intakeMotor = new TalonFX(CAN2.intakeRoller, CAN2.bus); config = new TalonFXConfiguration(); config.MotorOutput.withInverted(InvertedValue.CounterClockwise_Positive) @@ -56,7 +56,8 @@ public IntakeRollerIOSim() { intakeMotorSim.setMotorType(MotorType.KrakenX60); intakeRollerSim = - new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + new DCMotorSim( + LinearSystemId.createDCMotorSystem(gearbox, 0.0005, motorReduction), gearbox); intakeVelocity = intakeMotor.getVelocity(); intakeAppliedVolts = intakeMotor.getMotorVoltage(); @@ -68,33 +69,37 @@ public IntakeRollerIOSim() { @Override public void updateInputs(IntakeRollerIOInputs inputs) { + inputs.connected = + connectedDebounce.calculate( + BaseStatusSignal.refreshAll(intakeVelocity, intakeAppliedVolts, intakeCurrent).isOK()); + // Update simulation state var intakeMotorSim = intakeMotor.getSimState(); - intakeMotorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); - intakeRollerSim.setInput(intakeMotorSim.getMotorVoltageMeasure().in(Volts)); + intakeMotorSim.setSupplyVoltage(RobotConstants.kNominalVoltage); + intakeRollerSim.setInput(intakeMotorSim.getMotorVoltage()); intakeRollerSim.update(Robot.defaultPeriodSecs); intakeMotorSim.setRawRotorPosition( - intakeRollerSim.getAngularPositionRotations() * motorReudction); - intakeMotorSim.setRotorVelocity(intakeRollerSim.getAngularVelocityRPM() * motorReudction); - - BaseStatusSignal.refreshAll(intakeVelocity, intakeAppliedVolts, intakeCurrent).isOK(); + intakeRollerSim.getAngularPositionRotations() * motorReduction); + intakeMotorSim.setRotorVelocity(intakeRollerSim.getAngularVelocity().times(motorReduction)); inputs.appliedVolts = intakeAppliedVolts.getValueAsDouble(); - inputs.connected = intakeMotor.isConnected(); inputs.currentAmps = intakeCurrent.getValueAsDouble(); inputs.velocityMetersPerSec = - intakeVelocity.getValueAsDouble() * rollerRadius.in(Meters) * (2 * Math.PI); + intakeVelocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction; } @Override public void setOpenLoop(double output) { - // intakeMotor.setControl(voltageRequest.withOutput(output)); - intakeRollerSim.setInputVoltage(output); + intakeMotor.setControl(voltageRequest.withOutput(output)); } @Override public void setVelocity(LinearVelocity tangentialVelocity) { - // intakeMotor.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); - intakeRollerSim.setState(0.0, tangentialVelocity.in(MetersPerSecond)); + intakeMotor.setControl( + velocityVoltageRequest.withVelocity( + RadiansPerSecond.of( + tangentialVelocity.in(MetersPerSecond) + * motorReduction + / rollerRadius.in(Meters)))); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java index 33a5baa..54a4ca6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerIOTalonFX.java @@ -9,11 +9,12 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.LinearVelocity; @@ -23,10 +24,12 @@ public class IntakeRollerIOTalonFX implements IntakeRollerIO { private final TalonFX intakeMotor; private final TalonFXConfiguration config; + private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); private final VoltageOut voltageRequest = new VoltageOut(0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + // private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + // new VelocityTorqueCurrentFOC(0.0); // Inputs from intake motor private final StatusSignal intakeVelocity; @@ -51,15 +54,14 @@ public IntakeRollerIOTalonFX() { @Override public void updateInputs(IntakeRollerIOInputs inputs) { - var status = BaseStatusSignal.refreshAll(intakeVelocity, intakeAppliedVolts, intakeCurrent); - if (!status.isOK()) { - return; - } + inputs.connected = + connectedDebounce.calculate( + BaseStatusSignal.refreshAll(intakeVelocity, intakeAppliedVolts, intakeCurrent).isOK()); + inputs.appliedVolts = intakeAppliedVolts.getValueAsDouble(); - inputs.connected = intakeMotor.isConnected(); inputs.currentAmps = intakeCurrent.getValueAsDouble(); inputs.velocityMetersPerSec = - intakeVelocity.getValueAsDouble() * rollerRadius.in(Meters) * (2 * Math.PI); + intakeVelocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction; } @Override @@ -69,8 +71,11 @@ public void setOpenLoop(double output) { @Override public void setVelocity(LinearVelocity tangentialVelocity) { - var angularVelocity = - RadiansPerSecond.of(tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters)); - intakeMotor.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); + intakeMotor.setControl( + velocityVoltageRequest.withVelocity( + RadiansPerSecond.of( + tangentialVelocity.in(MetersPerSecond) + * motorReduction + / rollerRadius.in(Meters)))); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java new file mode 100644 index 0000000..fee0bef --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java @@ -0,0 +1,114 @@ +package frc.robot.subsystems.launcher; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; +import static frc.robot.util.PhoenixUtil.tryUntilOk; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Constants.RobotConstants; +import frc.robot.Robot; + +public class FlywheelIOSimTalonFX implements FlywheelIO { + private final DCMotorSim flywheelSim; + + private final TalonFX flywheelLeaderTalon; + private final TalonFX flywheelFollowerTalon; + private final TalonFXConfiguration config; + private final Debouncer flywheelConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + // Voltage control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + // private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + // new VelocityTorqueCurrentFOC(0.0); + + // Inputs from flywheel motor + private final StatusSignal flywheelVelocity; + private final StatusSignal flywheelAppliedVolts; + private final StatusSignal flywheelCurrent; + + public FlywheelIOSimTalonFX() { + flywheelLeaderTalon = new TalonFX(CAN2.flywheelLeader, CAN2.bus); + flywheelFollowerTalon = new TalonFX(CAN2.flywheelFollower, CAN2.bus); + // Configuration + config = new TalonFXConfiguration(); + config.MotorOutput.withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake); + config.Slot0 = flywheelGains; + tryUntilOk(5, () -> flywheelLeaderTalon.getConfigurator().apply(config, 0.25)); + tryUntilOk(5, () -> flywheelFollowerTalon.getConfigurator().apply(config, 0.25)); + + var flywheelMotorSim = flywheelLeaderTalon.getSimState(); + flywheelMotorSim.Orientation = ChassisReference.Clockwise_Positive; + flywheelMotorSim.setMotorType(MotorType.KrakenX60); + + flywheelSim = + new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + + flywheelVelocity = flywheelLeaderTalon.getVelocity(); + flywheelAppliedVolts = flywheelLeaderTalon.getMotorVoltage(); + flywheelCurrent = flywheelLeaderTalon.getStatorCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent, + flywheelVelocity, + flywheelAppliedVolts, + flywheelCurrent); + + flywheelFollowerTalon.setControl( + new Follower(CAN2.flywheelLeader, MotorAlignmentValue.Opposed)); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + inputs.connected = + flywheelConnectedDebounce.calculate( + BaseStatusSignal.refreshAll(flywheelVelocity, flywheelAppliedVolts, flywheelCurrent) + .isOK()); + + // Update simulation state + var flywheelMotorSim = flywheelLeaderTalon.getSimState(); + flywheelMotorSim.setSupplyVoltage(RobotConstants.kNominalVoltage); + flywheelSim.setInput(flywheelMotorSim.getMotorVoltage()); + flywheelSim.update(Robot.defaultPeriodSecs); + flywheelMotorSim.setRawRotorPosition( + flywheelSim.getAngularPositionRotations() * motorReduction); + flywheelMotorSim.setRotorVelocity(flywheelSim.getAngularVelocity().times(motorReduction)); + + inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); + inputs.currentAmps = flywheelCurrent.getValueAsDouble(); + inputs.velocityRadPerSec = flywheelVelocity.getValue().in(RadiansPerSecond) / motorReduction; + } + + @Override + public void setOpenLoop(double output) { + flywheelLeaderTalon.setControl(voltageRequest.withOutput(output)); + } + + @Override + public void setVelocity(AngularVelocity angularVelocity) { + flywheelLeaderTalon.setControl(velocityVoltageRequest.withVelocity(angularVelocity)); + } +} diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java similarity index 96% rename from src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java rename to src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java index 1f60aa5..37b79f9 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java @@ -11,7 +11,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class FlywheelIOSim implements FlywheelIO { +public class FlywheelIOSimWPI implements FlywheelIO { private final DCMotorSim flywheelSim; private boolean closedLoop = false; @@ -19,7 +19,7 @@ public class FlywheelIOSim implements FlywheelIO { private double appliedVolts = 0.0; private double feedforwardVolts = 0.0; - public FlywheelIOSim() { + public FlywheelIOSimWPI() { flywheelSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); } diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java index 28617d4..1a7704b 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.launcher; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; import static frc.robot.util.PhoenixUtil.tryUntilOk; @@ -7,14 +8,13 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -29,12 +29,9 @@ public class FlywheelIOTalonFX implements FlywheelIO { // Voltage control requests private final VoltageOut voltageRequest = new VoltageOut(0); - // private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); - - // Torque-current control requests - // private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + // private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + // new VelocityTorqueCurrentFOC(0.0); // Inputs from flywheel motor private final StatusSignal flywheelVelocity; @@ -71,24 +68,14 @@ public FlywheelIOTalonFX() { @Override public void updateInputs(FlywheelIOInputs inputs) { - BaseStatusSignal.refreshAll( - flywheelVelocity, - flywheelAppliedVolts, - flywheelCurrent, - flywheelVelocity, - flywheelAppliedVolts, - flywheelCurrent) - .isOK(); - - inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); inputs.connected = flywheelConnectedDebounce.calculate( - flywheelAppliedVolts.getStatus().isOK() - && flywheelCurrent.getStatus().isOK() - && flywheelVelocity.getStatus().isOK()); + BaseStatusSignal.refreshAll(flywheelVelocity, flywheelAppliedVolts, flywheelCurrent) + .isOK()); + + inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); inputs.currentAmps = flywheelCurrent.getValueAsDouble(); - inputs.velocityRadPerSec = - Units.rotationsToRadians(flywheelVelocity.getValueAsDouble()) / gearboxRatio; + inputs.velocityRadPerSec = flywheelVelocity.getValue().in(RadiansPerSecond) / motorReduction; } @Override @@ -98,6 +85,6 @@ public void setOpenLoop(double output) { @Override public void setVelocity(AngularVelocity angularVelocity) { - flywheelLeaderTalon.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); + flywheelLeaderTalon.setControl(velocityVoltageRequest.withVelocity(angularVelocity)); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java similarity index 98% rename from src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java rename to src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java index 05b99ae..5944cc4 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java @@ -24,7 +24,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class HoodIOSim implements HoodIO { +public class HoodIOSimSpark implements HoodIO { private final DCMotorSim hoodSim; @@ -32,7 +32,7 @@ public class HoodIOSim implements HoodIO { private final SparkClosedLoopController controller; private final SparkMaxSim maxSim; - public HoodIOSim() { + public HoodIOSimSpark() { max = new SparkMax(CAN2.hood, MotorType.kBrushless); controller = max.getClosedLoopController(); diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimHardwareless.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java similarity index 96% rename from src/main/java/frc/robot/subsystems/launcher/HoodIOSimHardwareless.java rename to src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java index 4543566..918cbd5 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimHardwareless.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java @@ -12,7 +12,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class HoodIOSimHardwareless implements HoodIO { +public class HoodIOSimWPI implements HoodIO { private final DCMotorSim hoodSim; @@ -21,7 +21,7 @@ public class HoodIOSimHardwareless implements HoodIO { private double appliedVolts = 0.0; private double feedforwardVolts = 0.0; - public HoodIOSimHardwareless() { + public HoodIOSimWPI() { hoodSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 8385cbc..3afc659 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -15,6 +15,7 @@ import frc.robot.Constants; import frc.robot.Constants.MotorConstants.KrakenX60Constants; import frc.robot.Constants.MotorConstants.NEO550Constants; +import frc.robot.Constants.RobotConstants; public final class LauncherConstants { @@ -93,24 +94,23 @@ public static final class TurretConstants { public static final class FlywheelConstants { public static final Distance wheelRadius = Inches.of(1.5); - public static final double gearboxRatio = 1.0; // Velocity Controller + + // Motor controller + public static final double motorReduction = 1.0; + public static final AngularVelocity maxAngularVelocity = + KrakenX60Constants.kFreeSpeed.div(motorReduction); public static final Slot0Configs flywheelGains = new Slot0Configs() .withKP(1.0) .withKI(0.0) - .withKD(1.0) + .withKD(0.05) .withKS(0.0) - .withKV(0.0) + .withKV(RobotConstants.kNominalVoltage / maxAngularVelocity.in(RotationsPerSecond)) .withKA(0.0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // Motor controller - public static final double motorReduction = 1.0; - public static final AngularVelocity maxAngularVelocity = - KrakenX60Constants.kFreeSpeed.div(motorReduction); - // Simulation public static final double kPSim = 0.1; public static final DCMotor gearbox = DCMotor.getKrakenX60(2); diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java similarity index 98% rename from src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java rename to src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java index e906a2f..4cc2f55 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java @@ -25,7 +25,7 @@ import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -public class TurretIOSim implements TurretIO { +public class TurretIOSimSpark implements TurretIO { private final DCMotorSim turnSim; @@ -33,7 +33,7 @@ public class TurretIOSim implements TurretIO { private final SparkClosedLoopController controller; private final SparkMaxSim turnSparkSim; - public TurretIOSim() { + public TurretIOSimSpark() { turnSpark = new SparkMax(CAN2.turret, MotorType.kBrushless); controller = turnSpark.getClosedLoopController();