Fixed Arm Simulation

4201VitruvianBots · Mar 15, 2024 · 4b4d17c · 4b4d17c
1 parent a762abe
commit 4b4d17c
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
@@ -63,9 +63,9 @@ public class Arm extends SubsystemBase {
           SingleJointedArmSim.estimateMOI(ARM.armLength, ARM.mass),
           ARM.armLength,
           Units.degreesToRadians(ARM.minAngleDegrees),
-          Units.degreesToRadians(ARM.maxAngleDegrees - ARM.minAngleDegrees),
+          Units.degreesToRadians(ARM.maxAngleDegrees),
           false,
-          Units.degreesToRadians(ARM.startingAngleDegrees));
+          Units.degreesToRadians(ARM.minAngleDegrees));
 
   private ROBOT.CONTROL_MODE m_controlMode = ROBOT.CONTROL_MODE.CLOSED_LOOP;
 
@@ -115,7 +115,19 @@ public Arm() {
     canCoderConfig.MagnetSensor.MagnetOffset = ARM.canCoderOffset;
     CtreUtils.configureCANCoder(m_armEncoder, canCoderConfig);
 
-    if (RobotBase.isReal()) m_armEncoder.setPosition(m_armEncoder.getAbsolutePosition().getValue());
+    m_armEncoder.setPosition(m_armEncoder.getAbsolutePosition().getValue());
+
+    if (RobotBase.isSimulation()) {
+      m_armEncoder.setPosition(Units.degreesToRotations(180));
+      m_armMotor.setPosition(Units.degreesToRotations(180));
+      CANcoderConfiguration simCanCoderConfig = new CANcoderConfiguration();
+      simCanCoderConfig.MagnetSensor.AbsoluteSensorRange =
+          AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
+      simCanCoderConfig.MagnetSensor.SensorDirection =
+          SensorDirectionValue.CounterClockwise_Positive;
+      simCanCoderConfig.MagnetSensor.MagnetOffset = ARM.canCoderOffset;
+      CtreUtils.configureCANCoder(m_armEncoder, simCanCoderConfig);
+    }
 
     SmartDashboard.putData(this);
   }
@@ -311,5 +323,7 @@ public void simulationPeriodic() {
 
     m_armEncoderSimState.setRawPosition(Units.radiansToRotations(m_armSim.getAngleRads()));
     m_armEncoderSimState.setVelocity(Units.radiansToRotations(m_armSim.getVelocityRadPerSec()));
+
+    Logger.recordOutput("Arm/Model Angle", Units.radiansToDegrees(m_armSim.getAngleRads()));
   }
 }
diff --git a/src/main/java/frc/robot/visualizers/SuperStructureVisualizer.java b/src/main/java/frc/robot/visualizers/SuperStructureVisualizer.java
@@ -225,9 +225,9 @@ public void updateAmpShooter() {
   }
 
   public void updateArm() {
-    m_armVisualizer.update(m_arm.getCurrentAngle(), m_arm.getPercentOutput());
+    m_armVisualizer.update(m_arm.getCurrentAngle() + 170, m_arm.getPercentOutput());
     if (m_armVisualizer2 != null)
-      m_armVisualizer2.update(m_arm.getCurrentAngle(), m_arm.getPercentOutput());
+      m_armVisualizer2.update(m_arm.getCurrentAngle() + 170, m_arm.getPercentOutput());
   }
 
   public void updateClimber() {