diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 35ae23d..7da5107 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,9 +137,9 @@ public class RobotContainer { /* Default Commands */ // moves climber in xy space with two-axis input from the operator controller - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4), - // m_robotClimber)); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.2, getOperatorController().getLeftY() * 0.2), + m_robotClimber)); // IK command diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index dd75f58..cac715a 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -6,6 +6,8 @@ package frc4388.robot.subsystems; import java.util.HashMap; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; @@ -45,7 +47,7 @@ public class ClimberRewrite extends SubsystemBase { m_elbow.configFactoryDefault(); m_elbow.setNeutralMode(NeutralMode.Brake); - setClimberGains(); + // setClimberGains(); // shoulderStartPosition = m_shoulder.getSelectedSensorPosition(); // elbowStartPosition = m_elbow.getSelectedSensorPosition(); @@ -62,6 +64,13 @@ public class ClimberRewrite extends SubsystemBase { m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); m_shoulder.configReverseSoftLimitEnable(false); + m_shoulder.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + m_elbow.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + m_shoulder.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + + m_shoulder.overrideLimitSwitchesEnable(true); + m_elbow.overrideLimitSwitchesEnable(true); + tPoint = new Point(); if(groundRelative)