Skip to content

Commit

Permalink
cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
chrishoffman committed Jan 30, 2025
1 parent a736d2f commit 5f365ba
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 14 deletions.
12 changes: 3 additions & 9 deletions src/main/java/frc/robot/subsystems/coraller/Coraller.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,9 @@
import frc.robot.Constants.CorallerConfig;

public class Coraller extends SubsystemBase {
private final Elevator elevator;
private final Angler angler;
private final Intake intake;

public Coraller() {
elevator = new Elevator(CorallerConfig.kRobotElevatorStowHeightInches);
angler = new Angler();
intake = new Intake();
}
private final Elevator elevator = new Elevator();
private final Angler angler = new Angler();
private final Intake intake = new Intake();

public Command prepareToScore(Configuration cfg) {
return Commands.parallel(
Expand Down
9 changes: 4 additions & 5 deletions src/main/java/frc/robot/subsystems/coraller/Elevator.java
Original file line number Diff line number Diff line change
Expand Up @@ -21,9 +21,8 @@ class Elevator {
private final SparkClosedLoopController pid;

private double setPointHeight;
private double baseHeight;

public Elevator(double base) {
public Elevator() {
var elevatorEncoderConfig = new EncoderConfig()
.positionConversionFactor(CorallerConfig.kElevatorEncoderPositionConversionFactor)
.velocityConversionFactor(CorallerConfig.kElevatorEncoderVelocityConversionFactor);
Expand All @@ -45,7 +44,7 @@ public Elevator(double base) {
// sets height relative to the floor
public void setPosition(double position) {
setPointHeight = position;
double setpoint = position - baseHeight;
double setpoint = position - CorallerConfig.kRobotElevatorStowHeightInches;
changeSetPoint(setpoint);
}

Expand All @@ -54,7 +53,7 @@ private void changeSetPoint(double setpoint) {
}

public boolean isElevatorAtBottom() {
return getHeightFromGround() == baseHeight;
return getHeightFromGround() == CorallerConfig.kRobotElevatorStowHeightInches;
}

public double getSetPoint() {
Expand All @@ -72,6 +71,6 @@ public double getRelativePosition() {
}

private double getHeightFromGround() {
return encoder.getPosition() + baseHeight;
return encoder.getPosition() + CorallerConfig.kRobotElevatorStowHeightInches;
}
}

0 comments on commit 5f365ba

Please sign in to comment.