Skip to content

Commit 03e4589

Browse files
author
Roberts Kalnins
committed
Merge branch 'master' into rkalnins/hatch-place
2 parents d37ca3a + f3d7654 commit 03e4589

20 files changed

+369
-226
lines changed

‎.github/main.workflow‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ action "gradle build" {
1010

1111
workflow "process pull request" {
1212
on = "pull_request"
13-
resolves = ["post gif on fail", "branch cleanup"]
13+
resolves = ["branch cleanup"]
1414
}
1515

1616
action "post gif on fail" {

‎src/main/java/frc/team2767/deepspace/command/climb/ClimbAutoCommand.java‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
package frc.team2767.deepspace.command.climb;
2+
3+
import edu.wpi.first.wpilibj.command.Command;
4+
import frc.team2767.deepspace.Robot;
5+
import frc.team2767.deepspace.subsystem.ClimbSubsystem;
6+
import frc.team2767.deepspace.subsystem.VacuumSubsystem;
7+
import frc.team2767.deepspace.subsystem.VisionSubsystem;
8+
import org.slf4j.Logger;
9+
import org.slf4j.LoggerFactory;
10+
11+
public class ClimbAutoCommand extends Command {
12+
13+
private static final VacuumSubsystem VACUUM = Robot.VACUUM;
14+
private static final ClimbSubsystem CLIMB = Robot.CLIMB;
15+
private static final VisionSubsystem VISION = Robot.VISION;
16+
private static final int GOOD_ENOUGH = 5;
17+
private final Logger logger = LoggerFactory.getLogger(this.getClass());
18+
private ClimbState climbState;
19+
20+
public ClimbAutoCommand() {
21+
requires(VACUUM);
22+
requires(CLIMB);
23+
requires(VISION);
24+
}
25+
26+
@Override
27+
protected void initialize() {
28+
if (!CLIMB.setOpenLoopFeedbackSensor(true)) {
29+
climbState = ClimbState.DONE;
30+
}
31+
climbState = ClimbState.FAST_LOWER;
32+
CLIMB.setLowerLimit(ClimbSubsystem.kHabHover);
33+
CLIMB.openLoop(ClimbSubsystem.kDownOpenLoopOutput);
34+
}
35+
36+
@Override
37+
protected void execute() {
38+
switch (climbState) {
39+
case FAST_LOWER:
40+
if (CLIMB.getStringpotPosition() >= ClimbSubsystem.kHabHover - GOOD_ENOUGH) {
41+
if (!CLIMB.setOpenLoopFeedbackSensor(false)) {
42+
climbState = ClimbState.DONE;
43+
}
44+
climbState = ClimbState.FORM_SEAL;
45+
CLIMB.setVelocity(ClimbSubsystem.kSealVelocity);
46+
logger.debug("forming seal");
47+
}
48+
49+
break;
50+
51+
case FORM_SEAL:
52+
if (VACUUM.isClimbOnTarget()) {
53+
logger.debug("fast climbing");
54+
climbState = ClimbState.FAST_CLIMB;
55+
if (!CLIMB.setOpenLoopFeedbackSensor(true)) {
56+
climbState = ClimbState.DONE;
57+
}
58+
CLIMB.enableRatchet();
59+
CLIMB.releaseKickstand();
60+
VISION.startLightBlink(VisionSubsystem.LightPattern.CLIMB_GOOD);
61+
CLIMB.setLowerLimit(ClimbSubsystem.kClimb);
62+
CLIMB.openLoop(ClimbSubsystem.kDownClimbOutput);
63+
}
64+
65+
if (CLIMB.getStringpotPosition() >= ClimbSubsystem.kTooLowIn - GOOD_ENOUGH) {
66+
logger.debug("resetting");
67+
if (!CLIMB.setOpenLoopFeedbackSensor(true)) {
68+
climbState = ClimbState.DONE;
69+
}
70+
climbState = ClimbState.RESET;
71+
CLIMB.setUpperLimit(ClimbSubsystem.kHabHover);
72+
CLIMB.openLoop(ClimbSubsystem.kUpOpenLoopOutput);
73+
}
74+
75+
break;
76+
case FAST_CLIMB:
77+
if (CLIMB.getStringpotPosition() >= ClimbSubsystem.kClimb) {
78+
climbState = ClimbState.DONE;
79+
logger.debug("done climbing");
80+
}
81+
break;
82+
case RESET:
83+
if (CLIMB.getStringpotPosition() <= ClimbSubsystem.kHabHover + GOOD_ENOUGH) {
84+
climbState = ClimbState.FORM_SEAL;
85+
if (!CLIMB.setOpenLoopFeedbackSensor(false)) {
86+
climbState = ClimbState.DONE;
87+
}
88+
CLIMB.setVelocity(ClimbSubsystem.kSealVelocity);
89+
}
90+
break;
91+
}
92+
}
93+
94+
@Override
95+
protected boolean isFinished() {
96+
return climbState == ClimbState.DONE;
97+
}
98+
99+
@Override
100+
protected void end() {
101+
logger.debug("emd climb");
102+
CLIMB.stop();
103+
}
104+
105+
private enum ClimbState {
106+
FAST_LOWER,
107+
FORM_SEAL,
108+
RESET,
109+
FAST_CLIMB,
110+
DONE
111+
}
112+
}

‎src/main/java/frc/team2767/deepspace/command/climb/ClimbDeployCommand.java‎

Lines changed: 65 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
package frc.team2767.deepspace.command.climb;
2+
3+
import edu.wpi.first.wpilibj.command.Command;
4+
import frc.team2767.deepspace.Robot;
5+
import frc.team2767.deepspace.subsystem.ClimbSubsystem;
6+
import org.slf4j.Logger;
7+
import org.slf4j.LoggerFactory;
8+
9+
public class ClimbDeployCommand extends Command {
10+
11+
private static final ClimbSubsystem CLIMB = Robot.CLIMB;
12+
private static final int GOOD_ENOUGH = 5;
13+
private State currentState;
14+
private final Logger logger = LoggerFactory.getLogger(this.getClass());
15+
16+
public ClimbDeployCommand() {
17+
requires(CLIMB);
18+
}
19+
20+
@Override
21+
protected void initialize() {
22+
currentState = State.RELEASE;
23+
if (!CLIMB.setOpenLoopFeedbackSensor(true)) {
24+
currentState = State.DONE;
25+
}
26+
CLIMB.setLowerLimit(ClimbSubsystem.kLowRelease);
27+
CLIMB.openLoop(ClimbSubsystem.kDownOpenLoopOutput);
28+
}
29+
30+
@Override
31+
protected void execute() {
32+
switch (currentState) {
33+
case RELEASE:
34+
if (CLIMB.getStringpotPosition() >= ClimbSubsystem.kLowRelease - GOOD_ENOUGH) {
35+
CLIMB.setUpperLimit(ClimbSubsystem.kHighRelease);
36+
CLIMB.openLoop(ClimbSubsystem.kUpOpenLoopOutput);
37+
currentState = State.POSITION;
38+
}
39+
break;
40+
case POSITION:
41+
if (CLIMB.getStringpotPosition() <= ClimbSubsystem.kHighRelease + GOOD_ENOUGH) {
42+
currentState = State.DONE;
43+
}
44+
45+
break;
46+
}
47+
}
48+
49+
@Override
50+
protected boolean isFinished() {
51+
return currentState == State.DONE;
52+
}
53+
54+
@Override
55+
protected void end() {
56+
logger.debug("climb deploy finished");
57+
CLIMB.stop();
58+
}
59+
60+
private enum State {
61+
RELEASE,
62+
POSITION,
63+
DONE
64+
}
65+
}

‎src/main/java/frc/team2767/deepspace/command/climb/ClimbFormSealCommandGroup.java‎

Lines changed: 0 additions & 12 deletions
This file was deleted.

‎src/main/java/frc/team2767/deepspace/command/climb/ClimbCommand.java‎ renamed to ‎src/main/java/frc/team2767/deepspace/command/climb/ClimbJogCommand.java‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -4,18 +4,17 @@
44
import frc.team2767.deepspace.Robot;
55
import frc.team2767.deepspace.subsystem.ClimbSubsystem;
66

7-
public class ClimbCommand extends InstantCommand {
7+
public class ClimbJogCommand extends InstantCommand {
88

99
private static final ClimbSubsystem CLIMB = Robot.CLIMB;
10+
private double velocity;
1011

11-
public ClimbCommand() {
12-
requires(CLIMB);
12+
public ClimbJogCommand(double velocity) {
13+
this.velocity = velocity;
1314
}
1415

1516
@Override
1617
protected void initialize() {
17-
CLIMB.enableRatchet();
18-
CLIMB.releaseKickstand();
19-
CLIMB.climb();
18+
CLIMB.setVelocity(velocity);
2019
}
2120
}

‎src/main/java/frc/team2767/deepspace/command/climb/ClimbLockCommand.java‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
package frc.team2767.deepspace.command.climb;
2+
3+
import edu.wpi.first.wpilibj.command.ConditionalCommand;
4+
import frc.team2767.deepspace.subsystem.ClimbSubsystem;
5+
6+
public class ClimbLockCommand extends ConditionalCommand {
7+
8+
public ClimbLockCommand() {
9+
super(new DeploySequenceCommandGroup());
10+
}
11+
12+
@Override
13+
protected boolean condition() {
14+
return !ClimbSubsystem.isReleased;
15+
}
16+
}

‎src/main/java/frc/team2767/deepspace/command/climb/DeploySequenceCommand.java‎ renamed to ‎src/main/java/frc/team2767/deepspace/command/climb/DeploySequenceCommandGroup.java‎

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -4,19 +4,24 @@
44
import frc.team2767.deepspace.command.elevator.ElevatorSetPositionCommand;
55
import frc.team2767.deepspace.command.log.LogCommand;
66
import frc.team2767.deepspace.command.sequences.StowAllCommandGroup;
7+
import frc.team2767.deepspace.command.vacuum.PressureSetCommand;
8+
import frc.team2767.deepspace.command.vacuum.SetSolenoidStatesCommand;
9+
import frc.team2767.deepspace.subsystem.VacuumSubsystem;
710

8-
public class DeploySequenceCommand extends CommandGroup {
9-
public DeploySequenceCommand() {
11+
public class DeploySequenceCommandGroup extends CommandGroup {
12+
public DeploySequenceCommandGroup() {
1013
addSequential(new LogCommand("BEGIN DEPLOY SEQUENCE"));
14+
addSequential(new PressureSetCommand(VacuumSubsystem.kClimbPressureInHg));
15+
addSequential(new SetSolenoidStatesCommand(VacuumSubsystem.SolenoidStates.CLIMB));
1116
addParallel(
1217
new CommandGroup() {
1318
{
1419
addSequential(new StowAllCommandGroup());
1520
addSequential(new ElevatorSetPositionCommand(5.0), 0.5);
1621
}
1722
});
18-
addSequential(new ReleaseClimbCommand());
19-
addSequential(new RaiseClimbCommand());
23+
addParallel(new EngageRatchetCommand(false));
24+
addParallel(new ClimbDeployCommand());
2025
addSequential(new LogCommand("END DEPLOY SEQUENCE"));
2126
}
2227
}

‎src/main/java/frc/team2767/deepspace/command/climb/EngageRatchetCommand.java‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
package frc.team2767.deepspace.command.climb;
2+
3+
import edu.wpi.first.wpilibj.command.InstantCommand;
4+
import frc.team2767.deepspace.Robot;
5+
import frc.team2767.deepspace.subsystem.ClimbSubsystem;
6+
7+
public class EngageRatchetCommand extends InstantCommand {
8+
9+
private static final ClimbSubsystem CLIMB = Robot.CLIMB;
10+
11+
private boolean enabled;
12+
13+
EngageRatchetCommand(boolean enabled) {
14+
this.enabled = enabled;
15+
}
16+
17+
@Override
18+
protected void _initialize() {
19+
if (enabled) {
20+
CLIMB.enableRatchet();
21+
} else {
22+
CLIMB.disableRatchet();
23+
}
24+
}
25+
}

‎src/main/java/frc/team2767/deepspace/command/climb/LowerSuctionCupCommand.java‎

Lines changed: 0 additions & 31 deletions
This file was deleted.

‎src/main/java/frc/team2767/deepspace/command/climb/RaiseClimbCommand.java‎

Lines changed: 0 additions & 18 deletions
This file was deleted.

0 commit comments

Comments
 (0)