From 5d5e724f408ac5323625f251f939b96dd187e9ce Mon Sep 17 00:00:00 2001
From: Museum25 <91571536+mvog2501@users.noreply.github.com>
Date: Thu, 18 Jul 2024 10:23:43 -0500
Subject: [PATCH] [Robot] Improved sim accuracy

---
 .../java/com/swrobotics/robot/logging/SimView.java    |  9 +++++++++
 .../robot/subsystems/speaker/FlywheelSubsystem.java   | 11 ++++++++++-
 .../robot/subsystems/speaker/PivotSubsystem.java      |  2 ++
 .../robot/subsystems/speaker/ShooterSubsystem.java    |  5 -----
 4 files changed, 21 insertions(+), 6 deletions(-)

diff --git a/Robot/src/main/java/com/swrobotics/robot/logging/SimView.java b/Robot/src/main/java/com/swrobotics/robot/logging/SimView.java
index e374dcc..90a3ec5 100644
--- a/Robot/src/main/java/com/swrobotics/robot/logging/SimView.java
+++ b/Robot/src/main/java/com/swrobotics/robot/logging/SimView.java
@@ -75,11 +75,20 @@ public static void updateAmpArm(double armPivotRot) {
         ampIntake.setAngle(intakeRot * 360 - 180);
     }
 
+    @Deprecated
     public static void updateShooter(AimCalculator.Aim aim) {
         shooterPivot.setAngle(Math.toDegrees(aim.pivotAngle()));
         shooterPivot.setLength(aim.flywheelVelocity() / 100 * maxShooterLength);
     }
 
+    public static void updatePivot(double angleRot) {
+        shooterPivot.setAngle(angleRot * 360.0);
+    }
+
+    public static void updateFlywheels(double flywheelVelocity) {
+        shooterPivot.setLength(flywheelVelocity / 100 * maxShooterLength);
+    }
+
     public static void setShooting(boolean shooting) {
         if (shooting) {
             shooterPivot.setColor(new Color8Bit(Color.kBlueViolet));
diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/FlywheelSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/FlywheelSubsystem.java
index 7b4715e..a5e5b9b 100644
--- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/FlywheelSubsystem.java
+++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/FlywheelSubsystem.java
@@ -11,6 +11,7 @@
 import com.swrobotics.mathlib.MathUtil;
 import com.swrobotics.robot.config.IOAllocation;
 import com.swrobotics.robot.config.NTData;
+import com.swrobotics.robot.logging.SimView;
 import com.swrobotics.robot.utils.TalonFXWithSim;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.RobotBase;
@@ -33,6 +34,7 @@ private static TalonFXWithSim createFlywheel(IOAllocation.CanId id) {
     private double targetVelocity;
     private double simVelocity;
     private boolean isNeutral;
+    private double logVelocity; // Includes duty cycle
 
     public FlywheelSubsystem() {
         TalonFXConfiguration config = new TalonFXConfiguration();
@@ -60,6 +62,7 @@ public void setTargetVelocity(double velocityRPS) {
         leftMotor.setControl(new VelocityVoltage(velocityRPS));
         rightMotor.setControl(new VelocityVoltage(velocityRPS));
         isNeutral = false;
+        logVelocity = velocityRPS;
     }
 
     public void setDutyCycle(double percentOut) {
@@ -67,7 +70,8 @@ public void setDutyCycle(double percentOut) {
         leftMotor.setControl(cmd);
         rightMotor.setControl(cmd);
 
-        isNeutral = true; // It's neutral enough
+        isNeutral = true; // It's neutral enough\
+        logVelocity = percentOut;
     }
 
     public void setNeutral() {
@@ -94,6 +98,11 @@ public void simulationPeriodic() {
 
         leftMotor.updateSim(12);
         rightMotor.updateSim(12);
+
+        if (isNeutral) {
+            logVelocity = 10;
+        }
+        SimView.updateFlywheels(logVelocity);
     }
 
     private double getLeftVelocity() {
diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/PivotSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/PivotSubsystem.java
index 58d5cfe..b7f8855 100644
--- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/PivotSubsystem.java
+++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/PivotSubsystem.java
@@ -165,6 +165,8 @@ public void periodic() {
                     state = State.IDLE;
             }
         }
+
+        SimView.updatePivot(setpoint);
     }
 
     NTBoolean limitSw = new NTBoolean("Shooter/Debug/Limit Switch", false);
diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java
index 5f3ca84..582417f 100644
--- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java
+++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java
@@ -257,11 +257,6 @@ public Command getPoopCommand() {
 
     NTDouble pctErr = new NTDouble("Shooter/Debug/Percent Error", 0);
 
-    @Override
-    public void simulationPeriodic() {
-        SimView.updateShooter(targetAim);
-    }
-
     public boolean isPreparing() {
         return isPreparing;
     }