diff --git a/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java index a5950d7..6d4ea24 100644 --- 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()); }