Added lots of comments, also added a few simple methods as backup

This commit is contained in:
NoahLacks63 2025-01-21 00:58:38 +00:00
parent 198d105741
commit ce7246114f
3 changed files with 55 additions and 7 deletions

View File

@ -86,7 +86,7 @@ public class RobotContainer {
);
elevator.setDefaultCommand(
elevator.runElevator(operator::getLeftY)
elevator.runAssistedElevator(operator::getLeftY)
);
indexer.setDefaultCommand(
@ -222,4 +222,14 @@ public class RobotContainer {
() -> elevatorFirst
);
}
}
/*
* A moveManipulator method that will guarantee a safe movement.
* Here in case we need want to skip moveManipulator debugging
*/
@SuppressWarnings("unused")
private Command safeMoveManipulator(double elevatorPosition, double armPosition) {
return moveManipulatorUtil(elevatorPosition, ArmConstants.kArmSafeStowPosition, false, true)
.andThen(arm.goToSetpoint(armPosition, 2));
}
}

View File

@ -43,6 +43,7 @@ public class ElevatorConstants {
public static final double kElevatorL2AlgaePosition = 0;
public static final double kElevatorL3AlgaePosition = 0;
public static final double kElevatorBracePosition = 0;
public static final double kElevatorMaxHeight = 0;
// 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = 1;

View File

@ -97,8 +97,13 @@ public class Elevator extends SubsystemBase {
return motionTarget > ElevatorConstants.kElevatorBracePosition;
}
//manual command that keeps ouput speed consistent no matter the direction
public Command runElevator(DoubleSupplier speed) {
/**
* A manual translation command that will move the elevator using a consistent velocity disregarding direction
*
* @param speed How fast the elevator moves
* @return Sets motor voltage to move the elevator relative to the speed parameter
*/
public Command runAssistedElevator(DoubleSupplier speed) {
return run(() -> {
double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
@ -108,11 +113,29 @@ public class Elevator extends SubsystemBase {
) + feedForward.calculate(realSpeedTarget);
elevatorMotor1.setVoltage(voltsOut);
}).until(bottomLimitSwitch::get);
}).until(
() -> bottomLimitSwitch.get() || encoder.getPosition() >= ElevatorConstants.kElevatorMaxHeight);
}
/**
* A manual translation command that uses feed forward calculation to maintain position
*
* @param speed The speed at which the elevator translates
* @return Sets motor voltage to translate the elevator and maintain position
*/
public Command runManualElevator(double speed) {
return run(() -> {
elevatorMotor1.set(speed);
});
}
//go to setpoint command
/**
* Moves the elevator to a target destination (setpoint)
*
* @param setpoint Target destination of the subsystem
* @param timeout Time to achieve the setpoint before quitting
* @return Sets motor voltage to achieve the target destination
*/
public Command goToSetpoint(double setpoint, double timeout) {
return run(() -> {
double voltsOut = positionController.calculate(
@ -126,7 +149,21 @@ public class Elevator extends SubsystemBase {
).withTimeout(timeout);
}
/**
* Returns the current encoder position
*
* @return Current encoder position
*/
public double getEncoderPosition() {
return encoder.getPosition();
}
}
/**
* Returns the value of the bottom limit switch on the elevator (false = disabled, true = enabled)
*
* @return The value of bottomLimitSwitch
*/
public boolean getBottomLimitSwitch() {
return bottomLimitSwitch.get();
}
}