[Robot] Improved sim accuracy

SouthwestRoboticsProgramming · Jul 18, 2024 · 5d5e724 · 5d5e724
1 parent fe0b139
commit 5d5e724
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 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
@@ -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
@@ -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,14 +62,16 @@ public void setTargetVelocity(double velocityRPS) {
         leftMotor.setControl(new VelocityVoltage(velocityRPS));
         rightMotor.setControl(new VelocityVoltage(velocityRPS));
         isNeutral = false;
+        logVelocity = velocityRPS;
     }
 
     public void setDutyCycle(double percentOut) {
         DutyCycleOut cmd = new DutyCycleOut(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
@@ -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
@@ -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;
     }