Skip to content

Commit

Permalink
removed all traces of climb pid
Browse files Browse the repository at this point in the history
  • Loading branch information
@MysticalApple
MysticalApple committed Apr 1, 2024
1 parent 623fc04 commit 7cd3d99
Show file tree
Hide file tree
Showing 5 changed files with 4 additions and 176 deletions.
11 changes: 2 additions & 9 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,6 @@
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.commands.auton.AutonBuilder;
import frc.robot.commands.climb.ClimbLowerCommand;
import frc.robot.commands.climb.ClimbRaiseCommand;
import frc.robot.commands.elevator.ElevatorToAmpCommand;
import frc.robot.commands.elevator.ElevatorToIntakeCommand;
import frc.robot.commands.elevator.ElevatorToTrapCommand;
Expand Down Expand Up @@ -160,7 +158,6 @@ public RobotContainer() {
elevatorSubsystem = new ElevatorSubsystem();

climbSubsystem = new ClimbSubsystem();
climbSubsystem.setManual();

noteDetector = new NoteDetectionWrapper(NOTE_CAMERA);

Expand Down Expand Up @@ -355,16 +352,12 @@ private void configureBindings() {

/* MECHANISM BINDINGS */

/* Climb Controls -- In manual mode, left and right joystick up/down controls left and right arm up/down,
* respectively. In automatic mode, pressing the left and right joysticks sends the elevator to its lowered
* and raised positions.*/
/* Climb Controls -- Left and right joystick up/down controls left and right arm up/down,
* respectively. */
climbSubsystem.setDefaultCommand(new RunCommand(() -> {
climbSubsystem.setSpeeds(-mechController.getLeftY(), -mechController.getRightY());
}, climbSubsystem));

// leftStickButton.onTrue(new ClimbLowerCommand(climbSubsystem));
// rightStickButton.onTrue(new ClimbRaiseCommand(climbSubsystem));

// rightBumper toggles the amp sequence
// if the elevator is up, lower it and stow the intake
// if the elevator is down, run the amp sequence
Expand Down
34 changes: 0 additions & 34 deletions src/main/java/frc/robot/commands/climb/ClimbLowerCommand.java
View file

This file was deleted.

27 changes: 0 additions & 27 deletions src/main/java/frc/robot/commands/climb/ClimbRaiseCommand.java
View file

This file was deleted.

47 changes: 2 additions & 45 deletions src/main/java/frc/robot/subsystems/climb/ClimbArm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,10 @@
import static frc.robot.Constants.ClimbConstants.LOWER_LIMIT_METERS;
import static frc.robot.Constants.ClimbConstants.RAISE_LIMIT_METERS;

import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkBase.SoftLimitDirection;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkPIDController;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.DigitalInput;
import frc.robot.util.MotorUtil;
Expand All @@ -20,18 +18,10 @@
public class ClimbArm {
private final CANSparkMax winchMotor;
private final RelativeEncoder extensionEncoder;
private final SparkPIDController extensionPID;
private final DigitalInput zeroLimitSwitch;

private static final double EXTENSION_P = 4;
private static final double EXTENSION_I = 0;
private static final double EXTENSION_D = 0;
private static final double EXTENSION_TOLERANCE_METERS = 0.005;

private boolean isUsingPID;
private double winchPower;
private double prevWinchPower;
private double extensionTarget;

/**
* Constructs a new {@link ClimbArm}.
Expand All @@ -54,12 +44,6 @@ public ClimbArm(int winchMotorId, int zeroLimitPort, boolean isInverted) {
extensionEncoder.setPositionConversionFactor(EXTENSION_METERS_PER_ROTATION);
extensionEncoder.setPosition(LOWER_LIMIT_METERS); /* The arm starts lowered. */

extensionPID = MotorUtil.createSparkPIDController(winchMotor, extensionEncoder);
extensionPID.setP(EXTENSION_P);
extensionPID.setI(EXTENSION_I);
extensionPID.setD(EXTENSION_D);
isUsingPID = true;

zeroLimitSwitch = new DigitalInput(zeroLimitPort);

winchPower = 0;
Expand All @@ -68,14 +52,12 @@ public ClimbArm(int winchMotorId, int zeroLimitPort, boolean isInverted) {

/** Run this function every periodic loop.*/
public void update() {
if (zeroLimitSwitch != null && this.isLimitSwitchPressed()) {
if (this.isLimitSwitchPressed()) {
resetEncoder();
winchPower = Math.max(0, winchPower);
}

if (isUsingPID) {
extensionPID.setReference(extensionTarget, ControlType.kPosition);
} else if (winchPower != prevWinchPower) {
if (winchPower != prevWinchPower) {
winchMotor.set(winchPower);
}

Expand All @@ -91,27 +73,11 @@ public void setSpeed(double speed) {
winchPower = MathUtil.clamp(speed, -1.0, +1.0);
}

/**
* Sets the targeted extension height for this climb arm (has no effect if PID mode is disabled).
*
* @param extensionTarget The extension target in meters.
*/
public void setExtensionTarget(double extensionTarget) {
this.extensionTarget = MathUtil.clamp(extensionTarget, LOWER_LIMIT_METERS, RAISE_LIMIT_METERS);
}

/** Returns this climb arm's current extension in meters. */
public double getCurrentExtension() {
return extensionEncoder.getPosition();
}

/**
* Returns true if this climb arm is within tolerance of its target, and false otherwise.
*/
public boolean isAtExtensionTarget() {
return Math.abs(this.getCurrentExtension() - extensionTarget) < EXTENSION_TOLERANCE_METERS;
}

/**
* Enables/disables this climb arm's motor position limits.
*
Expand All @@ -122,15 +88,6 @@ public void enableSoftLimits(boolean enable) {
// winchMotor.enableSoftLimit(SoftLimitDirection.kReverse, enable);
}

/**
* Enables/disables this climb arm's PID controller.
*
* @param enable True to enable, false to disable.
*/
public void enablePID(boolean enable) {
isUsingPID = enable;
}

/** Returns whether or not this climb arm's limit switch at position 0 is pressed. */
public boolean isLimitSwitchPressed() {
return !zeroLimitSwitch.get();
Expand Down
61 changes: 0 additions & 61 deletions src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
import static frc.robot.Constants.ClimbConstants.RIGHT_ZERO_LIMIT_PORT;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.util.GRTUtil;

/**
* Represents the climb mechanism (both arms).
Expand All @@ -15,10 +14,6 @@ public class ClimbSubsystem extends SubsystemBase {
private final ClimbArm leftClimbArm;
private final ClimbArm rightClimbArm;

private static final double ZEROING_SPEED = 0.6;

private boolean isZeroing;

/** Constructs a new {@link ClimbSubsystem}. */
public ClimbSubsystem() {
leftClimbArm = new ClimbArm(LEFT_WINCH_MOTOR_ID, LEFT_ZERO_LIMIT_PORT, true);
Expand All @@ -36,73 +31,17 @@ public void periodic() {
rightClimbArm.update();
}

/** Sets climb to manual mode (full control over each arm's speed). */
public void setManual() {
leftClimbArm.enablePID(false);
rightClimbArm.enablePID(false);
leftClimbArm.enableSoftLimits(false);
rightClimbArm.enableSoftLimits(false);
}

/** Sets climb to automatic mode (each arm controlled by a position PID). */
public void setAutomatic() {
leftClimbArm.enableSoftLimits(true);
rightClimbArm.enableSoftLimits(true);
leftClimbArm.enablePID(true);
rightClimbArm.enablePID(true);
}

/**
* Sets the targeted extension height for both climb arms.
*
* @param height The extension target
*/
public void goToExtension(double height) {
if (isZeroing) {
return;
}

leftClimbArm.setExtensionTarget(height);
rightClimbArm.setExtensionTarget(height);
}

/** Returns true if both climb arms are at their extension target, and false otherwise. */
public boolean isAtTargetExtension() {
return leftClimbArm.isAtExtensionTarget() && rightClimbArm.isAtExtensionTarget();
}

/**
* Sets the climb arms' speeds from -1.0 (downwards) to +1.0 (upwards)
*
* @param leftArmSpeed The left arm's desired speed.
* @param rightArmSpeed The right arm's desired speed.
*/
public void setSpeeds(double leftArmSpeed, double rightArmSpeed) {
if (isZeroing) {
return;
}

leftClimbArm.setSpeed(leftArmSpeed);
rightClimbArm.setSpeed(rightArmSpeed);
}

/** Starts zeroing both climb arms. */
public void startZeroing() {
setSpeeds(-ZEROING_SPEED, -ZEROING_SPEED);
setManual();

isZeroing = true;
}

/** Ends the zeroing process for both climb arms. */
public void endZeroing() {
isZeroing = false;

setSpeeds(0, 0);
goToExtension(0);
// setAutomatic();
}

/**
* Returns whether or not both climb arms are correctly zeroed (and currently at position zero).
*/
Expand Down

0 comments on commit 7cd3d99

Please sign in to comment.