Merge remote-tracking branch 'origin/pvc-pipe-code' into sw-bot-integ…

…ration-cleanup

src/main/java/frc/robot/Robot.java

@@ -85,11 +85,15 @@ public void teleopPeriodic() {}
   public void testInit() {
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
+
+    m_robotContainer.testInit();
   }
 
   /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {}
+  public void testPeriodic() {
+    m_robotContainer.testPeriodic();
+  }
 
   /** This function is called once when the robot is first started up. */
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -174,4 +174,12 @@ private void configureBindings() {
   public Command getAutonomousCommand() {
     return new WaitCommand(0);
   }
+
+  public void testInit() {
+    m_coralOuttake.testInit();
+  }
+
+  public void testPeriodic() {
+    m_coralOuttake.testPeriodic();
+  }
 }

src/main/java/frc/robot/constants/CORALOUTTAKE.java

@@ -7,6 +7,6 @@ public class CORALOUTTAKE {
   public static final double kI = 0.0;
   public static final double kD = 0.0;
   public static final double gearRatio = 1.0;
-  public static final double Inertia = 0.0;
+  public static final double Inertia = 0.001;
   public static final DCMotor Gearbox = DCMotor.getKrakenX60(1);
 }

src/main/java/frc/robot/subsystems/CoralOuttake.java

@@ -4,13 +4,23 @@
 
 package frc.robot.subsystems;
 
+import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.TorqueCurrentFOC;
 import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.sim.TalonFXSimState;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.CAN;
@@ -19,15 +29,21 @@
 
 public class CoralOuttake extends SubsystemBase {
   private boolean m_isOuttaking = false;
+  private LinearSystem outtakePlant = LinearSystemId.createDCMotorSystem(1, 1);
   private final TalonFX outtakeMotor = new TalonFX(CAN.coralOuttakeMotor);
-  // private final DCMotorSim outtakeMotorSim = new DCMotorSim(CORALOUTTAKE.Gearbox,
-  // CORALOUTTAKE.gearRatio, CORALOUTTAKE.Inertia ); //TODO implement sim code
+  private final DCMotorSim outtakeMotorSim =
+      new DCMotorSim(outtakePlant, CORALOUTTAKE.Gearbox); // TODO implement sim code
   private double m_desiredPercentOutput;
   private final DutyCycleOut m_dutyCycleRequest = new DutyCycleOut(0);
   private double m_rpmSetpoint;
   private final VoltageOut m_voltageRequest = new VoltageOut(0);
   private final TorqueCurrentFOC m_TorqueCurrentFOC = new TorqueCurrentFOC(0);
   private final VelocityTorqueCurrentFOC m_focVelocityControl = new VelocityTorqueCurrentFOC(0);
+  private TalonFXSimState m_outtakeMotorSimState = outtakeMotor.getSimState();
+  // Test mode setup
+  private DoubleSubscriber m_kP_subscriber, m_kI_subscriber, m_kD_subscriber;
+  private final NetworkTable coralOuttakeTab =
+      NetworkTableInstance.getDefault().getTable("Shuffleboard").getSubTable("CoralOuttake");
 
   /* Creates a new CoralOuttake. */
   public CoralOuttake() {
@@ -94,6 +110,36 @@ public double getRPMsetpoint() {
     return m_rpmSetpoint;
   }
 
+  @Override
+  public void simulationPeriodic() {
+    m_outtakeMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage());
+    outtakeMotorSim.setInputVoltage(
+        MathUtil.clamp(m_outtakeMotorSimState.getMotorVoltage(), -12, 12));
+
+    // TODO Right now this thing has no idea what time it is, so gotta update that.
+
+    m_outtakeMotorSimState.setRawRotorPosition(
+        outtakeMotorSim.getAngularPositionRotations() * CORALOUTTAKE.gearRatio);
+    m_outtakeMotorSimState.setRotorVelocity(
+        outtakeMotorSim.getAngularVelocityRPM() * CORALOUTTAKE.gearRatio / 60.0);
+  }
+
+  public void testInit() {
+    coralOuttakeTab.getDoubleTopic("kP").publish().set(CORALOUTTAKE.kP);
+    coralOuttakeTab.getDoubleTopic("kI").publish().set(CORALOUTTAKE.kI);
+    coralOuttakeTab.getDoubleTopic("kD").publish().set(CORALOUTTAKE.kD);
+    m_kP_subscriber = coralOuttakeTab.getDoubleTopic("kP").subscribe(CORALOUTTAKE.kP);
+    m_kI_subscriber = coralOuttakeTab.getDoubleTopic("kI").subscribe(CORALOUTTAKE.kI);
+    m_kD_subscriber = coralOuttakeTab.getDoubleTopic("kD").subscribe(CORALOUTTAKE.kD);
+  }
+
+  public void testPeriodic() {
+    Slot0Configs slot0Configs = new Slot0Configs();
+    slot0Configs.kP = m_kP_subscriber.get(CORALOUTTAKE.kP);
+    slot0Configs.kI = m_kI_subscriber.get(CORALOUTTAKE.kI);
+    slot0Configs.kD = m_kD_subscriber.get(CORALOUTTAKE.kD);
+  }
+
   private void updateSmartDashboard() {
     SmartDashboard.putNumber("CoralOuttake/DesiredPercentOutput", m_desiredPercentOutput);
     SmartDashboard.putNumber("CoralOuttake/rpmSetpoint", m_rpmSetpoint);