Flywheel talon sim

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,23 +224,21 @@ 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();
 
     launcher.setDefaultCommand(
         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. */

src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java → 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<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
           constants) {
     // Create drive and turn sim models

src/main/java/frc/robot/subsystems/feeder/KickerIOSim.java → src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java

@@ -23,15 +23,15 @@
 import frc.robot.Constants.RobotConstants;
 import frc.robot.Robot;
 
-public class KickerIOSim implements KickerIO {
+public class KickerIOSimSpark implements KickerIO {
 
   private final DCMotorSim kickerSim;
 
   private final SparkFlex flex;
   private final SparkClosedLoopController controller;
   private final SparkFlexSim flexSim;
 
-  public KickerIOSim() {
+  public KickerIOSimSpark() {
     flex = new SparkFlex(CAN2.kicker, MotorType.kBrushless);
     controller = flex.getClosedLoopController();
 

src/main/java/frc/robot/subsystems/feeder/SpindexerIOSim.java → src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java

@@ -23,15 +23,15 @@
 import frc.robot.Constants.RobotConstants;
 import frc.robot.Robot;
 
-public class SpindexerIOSim implements SpindexerIO {
+public class SpindexerIOSimSpark implements SpindexerIO {
 
   private final DCMotorSim spindexerSim;
 
   private final SparkFlex flex;
   private final SparkClosedLoopController controller;
   private final SparkFlexSim flexSim;
 
-  public SpindexerIOSim() {
+  public SpindexerIOSimSpark() {
     flex = new SparkFlex(CAN2.spindexer, MotorType.kBrushless);
     controller = flex.getClosedLoopController();
 

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));
   }
 }

src/main/java/frc/robot/subsystems/intake/IntakeConstants.java

@@ -1,35 +1,34 @@
 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;
 import edu.wpi.first.math.system.plant.DCMotor;
 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);
   }

src/main/java/frc/robot/subsystems/intake/IntakeRollerIOSim.java → 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<AngularVelocity> intakeVelocity;
   private final StatusSignal<Voltage> intakeAppliedVolts;
   private final StatusSignal<Current> 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))));
   }
 }