From 548c0626895450461a49d096acdd4964c1de939d Mon Sep 17 00:00:00 2001
From: m10653 <m10653@users.noreply.github.com>
Date: Sat, 27 Jan 2024 00:54:45 +0000
Subject: [PATCH] Very hacky simulation support.

---
 .../frc2024/subsystems/arm/ArmSubsystem.java  | 32 +++++--
 .../arm/SimableAbsoluteEncoder.java           | 86 +++++++++++++++++++
 2 files changed, 110 insertions(+), 8 deletions(-)
 create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java

diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java
index 63211525..7f0d3d67 100644
--- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java
+++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java
@@ -1,11 +1,13 @@
 package org.jmhsrobotics.frc2024.subsystems.arm;
 
-import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
-import com.revrobotics.SparkAnalogSensor;
 import com.revrobotics.SparkAbsoluteEncoder.Type;
 
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -17,15 +19,17 @@ public class ArmSubsystem extends SubsystemBase {
 
 	private MechanismLigament2d m_arm;
 	private CANSparkMax armPivot = new CANSparkMax(9, MotorType.kBrushless);
-	private AbsoluteEncoder pitchEncoder;
+	private SimableAbsoluteEncoder pitchEncoder;
 	private Mechanism2d mech;
 
 	public ArmSubsystem() {
-		pitchEncoder = armPivot.getAbsoluteEncoder(Type.kDutyCycle);
+		pitchEncoder = new SimableAbsoluteEncoder(armPivot.getAbsoluteEncoder(Type.kDutyCycle));
 		armPivot.setSmartCurrentLimit(40);
-
 		armPivot.setIdleMode(IdleMode.kBrake);
 		init2d();
+		if (RobotBase.isSimulation()) {
+			initSim();
+		}
 	}
 
 	public void setArmPivot(double amount) {
@@ -37,8 +41,6 @@ public double getArmPitch() {
 		return this.pitchEncoder.getPosition();
 	}
 
-	private SparkAnalogSensor pitchencsim;
-
 	public void init2d() {
 		// TODO: finish sim
 		mech = new Mechanism2d(3, 3);
@@ -55,8 +57,22 @@ public void periodic() {
 		m_arm.setAngle(getArmPitch());
 		SmartDashboard.putData("ArmSubsystem/armSIM", mech);
 		SmartDashboard.putNumber("ArmSubsystem/velocity", this.pitchEncoder.getVelocity());
-		SmartDashboard.putString("ArmSubsystem/encoder", pitchEncoder.toString());
+		SmartDashboard.putNumber("ArmSubsystem/encoder", pitchEncoder.getPosition());
 
 	}
+	SingleJointedArmSim armSim;
+	public void initSim() {
+		double armGearRatio = 1;
+		double moi = 1;
+		armSim = new SingleJointedArmSim(DCMotor.getNEO(2), armGearRatio, moi, Units.inchesToMeters(23), 0,
+				Units.degreesToRadians(180), false, 0);
+		// simEncoder = new RevEncoderSimWrapper(null, null);
+	}
+	@Override
+	public void simulationPeriodic() {
+		double armVolts = armPivot.get() * 12;
+		armSim.setInputVoltage(armVolts);
+		pitchEncoder.setPosition(Units.radiansToDegrees(armSim.getAngleRads()));
+	}
 
 }
diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java
new file mode 100644
index 00000000..789a65bc
--- /dev/null
+++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/SimableAbsoluteEncoder.java
@@ -0,0 +1,86 @@
+package org.jmhsrobotics.frc2024.subsystems.arm;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.REVLibError;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+public class SimableAbsoluteEncoder implements AbsoluteEncoder {
+
+	private AbsoluteEncoder enc;
+	private double position;
+
+	SimableAbsoluteEncoder(AbsoluteEncoder enc) {
+		this.enc = enc;
+	}
+
+	@Override
+	public double getPosition() {
+		if (RobotBase.isSimulation()) {
+			return position;
+		} else {
+			return enc.getPosition();
+		}
+
+	}
+
+	@Override
+	public double getVelocity() {
+		return enc.getVelocity();
+	}
+
+	@Override
+	public REVLibError setPositionConversionFactor(double factor) {
+		return enc.setPositionConversionFactor(factor);
+	}
+
+	@Override
+	public double getPositionConversionFactor() {
+		return enc.getPositionConversionFactor();
+	}
+
+	@Override
+	public REVLibError setVelocityConversionFactor(double factor) {
+		return enc.setVelocityConversionFactor(factor);
+	}
+
+	@Override
+	public double getVelocityConversionFactor() {
+		return enc.getVelocityConversionFactor();
+	}
+
+	@Override
+	public REVLibError setInverted(boolean inverted) {
+		return enc.setInverted(inverted);
+	}
+
+	@Override
+	public boolean getInverted() {
+		return enc.getInverted();
+	}
+
+	@Override
+	public REVLibError setAverageDepth(int depth) {
+		return enc.setAverageDepth(depth);
+	}
+
+	@Override
+	public int getAverageDepth() {
+		return enc.getAverageDepth();
+	}
+
+	@Override
+	public REVLibError setZeroOffset(double offset) {
+		return enc.setZeroOffset(offset);
+	}
+
+	@Override
+	public double getZeroOffset() {
+		return enc.getZeroOffset();
+	}
+
+	public void setPosition(double position) {
+		this.position = position;
+	}
+
+}