Skip to content

Commit

Permalink
Merged LiftSubsystem.java with Rachit
Browse files Browse the repository at this point in the history
  • Loading branch information
@crkalapat
crkalapat committed Jan 28, 2025
1 parent b2daa96 commit 843f615
Showing 1 changed file with 67 additions and 25 deletions.
92 changes: 67 additions & 25 deletions src/main/java/frc/robot/subsystems/lift/LiftSubsystem.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package frc.robot.subsystems.lift;

import static edu.wpi.first.units.Units.Centimeters;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Meters;
Expand Down Expand Up @@ -43,6 +44,12 @@ public static enum USER_INPUT {
static final DutyCycleOut HOMING_SPEED = new DutyCycleOut(-0.05);
static final Distance HOMING_EPSILON = Millimeters.of(5);

// Tolerance in cm of top and bottom minimum clearance
static final Distance ELEVATOR_TOLERANCE = Centimeters.of(0.5);

// Tolerance in degrees of arm
static final Angle ARM_TOLERANCE = Degrees.of(0.5);

static final Angle SCORING_L1_ANGLE = Degrees.of(0);
static final Angle SCORING_L2_ANGLE = Degrees.of(0);
static final Angle SCORING_L3_ANGLE = Degrees.of(0);
Expand All @@ -61,6 +68,7 @@ public static enum USER_INPUT {
static final Distance L3_HEIGHT = Inches.of(0);
static final Distance L4_HEIGHT = Inches.of(0);


/* TODO: Actually get user input */
private static USER_INPUT getUserInput() { return USER_INPUT.STOW; }

Expand Down Expand Up @@ -96,6 +104,7 @@ public LiftStates nextState() {
@Override
public void initialize() {
s_liftinstance.setElevatorHeight(STOW_HEIGHT);
s_liftinstance.setArmAngle(STOW_ANGLE);
}

@Override
Expand Down Expand Up @@ -165,7 +174,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getArmAngle().gte(SAFE_INTAKE_ANGLE)) {
if (s_liftinstance.getArmAngle().lte(SAFE_INTAKE_ANGLE)) {
return STOW_L4_S1;
}
return this;
Expand Down Expand Up @@ -302,22 +311,21 @@ public void initialize() {
s_liftinstance.setElevatorHeight(L4_HEIGHT);
}

@Override
public void execute() {
if (s_liftinstance.getElevatorHeight() == CLEAR_HEIGHT && s_liftinstance.getArmAngle() == STOW_ANGLE) {
s_liftinstance.setArmAngle(SCORING_L4_ANGLE);
}
}

@Override
public SystemState nextState() {
if (s_liftinstance.getElevatorHeight() == L4_HEIGHT && s_liftinstance.getArmAngle() == SCORING_L4_ANGLE) {
if (s_liftinstance.getElevatorHeight().gte(CLEAR_HEIGHT)) {
return L4;
}
return this;
}
},
L4 {
@Override
public void initialize() {
s_liftinstance.setElevatorHeight(L4_HEIGHT);
s_liftinstance.setArmAngle(SCORING_L4_ANGLE);
}

@Override
public SystemState nextState() {
if (getUserInput() == USER_INPUT.STOW) {
Expand All @@ -343,7 +351,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getArmAngle() == STOW_ANGLE) {
if (s_liftinstance.getArmAngle().gte(STOW_ANGLE)) {
return STOW;
}
return this;
Expand All @@ -357,7 +365,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getArmAngle() == STOW_ANGLE) {
if (s_liftinstance.getArmAngle().gte(STOW_ANGLE)) {
return STOW;
}
return this;
Expand All @@ -371,7 +379,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getElevatorHeight() == CLEAR_HEIGHT) {
if (s_liftinstance.getElevatorHeight().gte(CLEAR_HEIGHT)) {
return L3_STOW_S2;
}
return this;
Expand All @@ -385,7 +393,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getArmAngle() == STOW_ANGLE) {
if (s_liftinstance.getArmAngle().gte(STOW_ANGLE)) {
return STOW;
}
return this;
Expand All @@ -399,7 +407,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getArmAngle() == STOW_ANGLE) {
if (s_liftinstance.getArmAngle().gte(STOW_ANGLE)) {
return STOW;
}
return this;
Expand All @@ -413,7 +421,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_BOTTOM) {
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
return STOW_L3_S1;
}
return this;
Expand All @@ -427,7 +435,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_BOTTOM) {
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
return STOW_L4_S1;
}
return this;
Expand All @@ -441,7 +449,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_BOTTOM) {
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
return STOW_L1_S1;
}
return this;
Expand All @@ -455,7 +463,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_BOTTOM) {
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
return STOW_L3_S1;
}
return this;
Expand All @@ -469,7 +477,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_BOTTOM) {
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
return STOW_L4_S1;
}
return this;
Expand All @@ -483,7 +491,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getArmAngle() == SAFE_REEF_ANGLE_TOP) {
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_TOP)) {
return L3_L1_S2;
}
return this;
Expand All @@ -497,7 +505,7 @@ public void initialize() {

@Override
public SystemState nextState() {
if (s_liftinstance.getElevatorHeight() == CLEAR_HEIGHT) {
if (s_liftinstance.getElevatorHeight().gte(CLEAR_HEIGHT)) {
return L3_L1_S3;
}
return this;
Expand All @@ -506,17 +514,31 @@ public SystemState nextState() {
L3_L1_S3 {
@Override
public void initialize() {
s_liftinstance.setElevatorHeight(CLEAR_HEIGHT);
s_liftinstance.setArmAngle(SAFE_REEF_ANGLE_BOTTOM);
}

@Override
public SystemState nextState() {
if (s_liftinstance.getElevatorHeight() == CLEAR_HEIGHT) {
return L3_L1_S3;
if (s_liftinstance.getArmAngle().gte(SAFE_REEF_ANGLE_BOTTOM)) {
return L3_L1_S4;
}
return this;
}
},
L3_L1_S4 {
@Override
public void initialize() {
s_liftinstance.setElevatorHeight(L1_HEIGHT);
}

@Override
public SystemState nextState() {
if (s_liftinstance.getElevatorHeight().lte(L1_HEIGHT)) {
return L1;
}
return this;
}
};
},
}

private static LiftSubsystem s_liftinstance;
Expand Down Expand Up @@ -705,4 +727,24 @@ public void close() {
m_pivotMotor.close();
s_liftinstance = null;
}

/**
* Return whether the elevator is at a target height or not
* @return Boolean of if elevator is at target height
*/
public boolean elevatorAt(Distance target_height) {

Distance cur_height = getElevatorHeight();
return (cur_height.gte(target_height.minus(ELEVATOR_TOLERANCE)) && cur_height.lte(target_height.plus(ELEVATOR_TOLERANCE)));
}

/**
* Return whether the arm is at a target angle or not
* @return Boolean of if arm is at target angle
*/
public boolean armAt(Angle target_angle) {

Angle cur_angle = getArmAngle();
return (cur_angle.gte(target_angle.minus(ARM_TOLERANCE)) && cur_angle.lte(target_angle.plus(ARM_TOLERANCE)));
}
}

0 comments on commit 843f615

Please sign in to comment.