Fixed Climber Sim

Try this?
4201VitruvianBots · Mar 15, 2024 · 0b89114 · 0b89114
1 parent 4b4d17c
commit 0b89114
Showing 1 changed file with 9 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java
@@ -6,6 +6,7 @@
 
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
@@ -101,8 +102,8 @@ public Climber() {
     CtreUtils.configureTalonFx(elevatorClimbMotors[1], config);
 
     elevatorClimbMotors[0].setInverted(false);
-    elevatorClimbMotors[1].setInverted(true);
-    // elevatorClimbMotors[1].setControl(new Follower(elevatorClimbMotors[0].getDeviceID(), true));
+//    elevatorClimbMotors[1].setInverted(true);
+     elevatorClimbMotors[1].setControl(new Follower(elevatorClimbMotors[0].getDeviceID(), false));
 
     SmartDashboard.putData(this);
   }
@@ -138,7 +139,7 @@ private void setPercentOutput(double output, boolean enforceLimits) {
     }
 
     elevatorClimbMotors[0].set(output);
-    elevatorClimbMotors[1].set(output);
+//    elevatorClimbMotors[1].set(output);
   }
 
   public double getAvgCurrentDraw() {
@@ -269,7 +270,7 @@ public void periodic() {
     switch (m_controlMode) {
       case CLOSED_LOOP:
         elevatorClimbMotors[0].setControl(m_request.withPosition(m_desiredPositionMeters));
-        elevatorClimbMotors[1].setControl(m_request.withPosition(m_desiredPositionMeters));
+//        elevatorClimbMotors[1].setControl(m_request.withPosition(m_desiredPositionMeters));
         break;
       case OPEN_LOOP:
       default:
@@ -302,18 +303,18 @@ public void simulationPeriodic() {
     m_simState1.setRawRotorPosition(
         leftElevatorSim.getPositionMeters()
             * CLIMBER.gearRatio
-            * CLIMBER.sprocketRotationsToMeters);
+            / CLIMBER.sprocketRotationsToMeters);
     m_simState1.setRotorVelocity(
         leftElevatorSim.getVelocityMetersPerSecond()
             * CLIMBER.gearRatio
-            * CLIMBER.sprocketRotationsToMeters);
+            / CLIMBER.sprocketRotationsToMeters);
     m_simState2.setRawRotorPosition(
         rightElevatorSim.getPositionMeters()
             * CLIMBER.gearRatio
-            * CLIMBER.sprocketRotationsToMeters);
+            / CLIMBER.sprocketRotationsToMeters);
     m_simState2.setRotorVelocity(
         rightElevatorSim.getVelocityMetersPerSecond()
             * CLIMBER.gearRatio
-            * CLIMBER.sprocketRotationsToMeters);
+            / CLIMBER.sprocketRotationsToMeters);
   }
 }