Added sme mathutil thing

Iron-Panthers · Dec 5, 2023 · b4da06a · b4da06a
1 parent ca2a88e
commit b4da06a
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java
@@ -6,6 +6,7 @@
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.TalonFX;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -61,8 +62,8 @@ public void periodic() {
     // This method will be called once per scheduler run
     double ticks = leftMotor.getSelectedSensorPosition();
     motorPower = controller.calculate(ticksToInches(ticks), targetHeight);
-    leftMotor.set(TalonFXControlMode.PercentOutput, motorPower);
-    rightMotor.set(TalonFXControlMode.PercentOutput, motorPower);
+    leftMotor.set(TalonFXControlMode.PercentOutput, MathUtil.clamp(motorPower, -0.75, 0.75));
+    rightMotor.set(TalonFXControlMode.PercentOutput, MathUtil.clamp(motorPower, -0.75, 0.75));
     currentHeight = ticksToInches(-leftMotor.getSelectedSensorPosition());
   }