From 7f12b4f75e498bc9cdc36d520740e62316cbcae5 Mon Sep 17 00:00:00 2001 From: SorinEngUS Date: Tue, 5 Dec 2023 16:02:32 -0800 Subject: [PATCH] qdded some rqndop m thing --- .../frc/robot/Subsystems/ElevatorSubsystem.java | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java index 6d4ea24..cad92e0 100644 --- a/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java @@ -30,7 +30,16 @@ public class ElevatorSubsystem extends SubsystemBase { public ElevatorSubsystem() { leftMotor = new TalonFX(14); - rightMotor = new TalonFX(15); + rightMotor = new TalonFX(15);< + + + leftMotor.configFactoryDefault(); + rightMotor.configFactoryDefault(); + + rightMotor.clearStickyFaults(); + leftMotor.clearStickyFaults(); + + rightMotor.follow(leftMotor); controller = new PIDController(0.4, 0, 0.0125); @@ -63,7 +72,7 @@ public void periodic() { double ticks = leftMotor.getSelectedSensorPosition(); motorPower = controller.calculate(ticksToInches(ticks), targetHeight); 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()); }