Skip to content

Commit c4914b5

Browse files
mwitcpalekmwitcpalek
committed
Level 2 and Level 3 Climb Tuning
1 parent a33cb55 commit c4914b5

File tree

7 files changed

+275
-13
lines changed

7 files changed

+275
-13
lines changed

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

Lines changed: 13 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ public ClimbDeployCommand() {
2121

2222
@Override
2323
protected void initialize() {
24-
logger.debug("releasing");
24+
logger.info("releasing");
2525
currentState = State.RELEASE;
2626
CLIMB.setSlowTalonConfig(false);
2727
CLIMB.setSlowTalonConfig(false);
@@ -36,17 +36,24 @@ protected void execute() {
3636
if (CLIMB.getStringPotPosition() >= ClimbSubsystem.kLowRelease - GOOD_ENOUGH) {
3737
CLIMB.openLoop(-1.0); // kUpVel
3838
currentState = State.POSITION;
39-
logger.debug("released, climber position = {}", CLIMB.getStringPotPosition());
39+
logger.info("released, climber position = {}", CLIMB.getStringPotPosition());
4040
}
4141
break;
4242
case POSITION:
4343
if (CLIMB.getStringPotPosition() <= ClimbSubsystem.kHighRelease + GOOD_ENOUGH) {
44-
logger.debug("climber position = {}", CLIMB.getStringPotPosition());
45-
currentState = State.DONE;
46-
CLIMB.stop();
44+
logger.info("climber position = {}", CLIMB.getStringPotPosition());
45+
currentState = State.SLOW_DOWN;
46+
CLIMB.setSlowTalonConfig(true);
47+
CLIMB.setVelocity(ClimbSubsystem.kSealOutputVelocity);
4748
}
4849

4950
break;
51+
case SLOW_DOWN:
52+
if (CLIMB.getStringPotPosition() >= ClimbSubsystem.kHabHover - GOOD_ENOUGH) {
53+
CLIMB.stop();
54+
currentState = State.DONE;
55+
logger.info("Climb Deployed, position = {}", CLIMB.getStringPotPosition());
56+
}
5057
}
5158
}
5259

@@ -64,6 +71,7 @@ protected void end() {
6471
private enum State {
6572
RELEASE,
6673
POSITION,
74+
SLOW_DOWN,
6775
DONE
6876
}
6977
}

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

Lines changed: 145 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,145 @@
1+
package frc.team2767.deepspace.command.climb;
2+
3+
import edu.wpi.first.wpilibj.Timer;
4+
import edu.wpi.first.wpilibj.command.Command;
5+
import frc.team2767.deepspace.Robot;
6+
import frc.team2767.deepspace.subsystem.ClimbSubsystem;
7+
import frc.team2767.deepspace.subsystem.VacuumSubsystem;
8+
import frc.team2767.deepspace.subsystem.VisionSubsystem;
9+
import org.slf4j.Logger;
10+
import org.slf4j.LoggerFactory;
11+
12+
public class ClimbLevel2AutoCommand extends Command {
13+
14+
private static final VacuumSubsystem VACUUM = Robot.VACUUM;
15+
private static final ClimbSubsystem CLIMB = Robot.CLIMB;
16+
private static final VisionSubsystem VISION = Robot.VISION;
17+
private static final double PAUSE_WAIT = 0.2;
18+
private static final int GOOD_ENOUGH = 5;
19+
private final Logger logger = LoggerFactory.getLogger(this.getClass());
20+
private double pauseInitTime;
21+
private ClimbState climbState;
22+
23+
public ClimbLevel2AutoCommand() {
24+
requires(CLIMB);
25+
requires(VISION);
26+
requires(VACUUM);
27+
}
28+
29+
@Override
30+
protected void initialize() {
31+
logger.info("BEGIN LVL 2 CLIMB SEQUENCE");
32+
CLIMB.setSlowTalonConfig(true);
33+
if (CLIMB.getStringPotPosition() < ClimbSubsystem.kHabHoverLvl2) {
34+
climbState = ClimbState.FAST_LOWER;
35+
CLIMB.setVelocity(ClimbSubsystem.kDownVelocity);
36+
logger.info("Fast Lower");
37+
} else {
38+
climbState = ClimbState.FORM_SEAL;
39+
CLIMB.setVelocity(ClimbSubsystem.kSealOutputVelocity);
40+
logger.info("forming seal");
41+
}
42+
}
43+
44+
@Override
45+
protected void execute() {
46+
switch (climbState) {
47+
case FAST_LOWER:
48+
if (CLIMB.getStringPotPosition() >= ClimbSubsystem.kHabHoverLvl2 - GOOD_ENOUGH) {
49+
CLIMB.setSlowTalonConfig(true);
50+
climbState = ClimbState.FORM_SEAL;
51+
CLIMB.setVelocity(ClimbSubsystem.kSealOutputVelocity);
52+
logger.info("forming seal");
53+
}
54+
break;
55+
56+
case FORM_SEAL:
57+
if (VACUUM.isClimbOnTarget()) {
58+
logger.info("fast climbing");
59+
CLIMB.setSlowTalonConfig(false);
60+
climbState = ClimbState.FAST_CLIMB;
61+
CLIMB.enableRatchet();
62+
VISION.startLightBlink(VisionSubsystem.LightPattern.CLIMB_GOOD);
63+
CLIMB.setVelocity(ClimbSubsystem.kDownClimbVelocity);
64+
break;
65+
}
66+
if (CLIMB.getStringPotPosition() >= ClimbSubsystem.kTooLowLvl2 - GOOD_ENOUGH) {
67+
logger.info("resetting");
68+
if (VACUUM.isClimbPrecheck()) {
69+
climbState = ClimbState.PRECHECK;
70+
logger.debug("going to precheck");
71+
CLIMB.stop();
72+
break;
73+
}
74+
CLIMB.setSlowTalonConfig(false);
75+
climbState = ClimbState.RESET;
76+
CLIMB.setVelocity(ClimbSubsystem.kUpVelocity);
77+
78+
break;
79+
}
80+
break;
81+
case PRECHECK:
82+
if (VACUUM.isClimbOnTarget()) {
83+
logger.info("fast climbing");
84+
CLIMB.setSlowTalonConfig(false);
85+
climbState = ClimbState.FAST_CLIMB;
86+
CLIMB.enableRatchet();
87+
VISION.startLightBlink(VisionSubsystem.LightPattern.CLIMB_GOOD);
88+
CLIMB.setVelocity(ClimbSubsystem.kDownClimbVelocity);
89+
}
90+
break;
91+
case FAST_CLIMB:
92+
if (CLIMB.getStringPotPosition() >= ClimbSubsystem.kClimb) {
93+
climbState = ClimbState.DONE;
94+
logger.info("done climbing");
95+
}
96+
break;
97+
case PAUSE:
98+
if (Timer.getFPGATimestamp() - pauseInitTime >= PAUSE_WAIT) {
99+
climbState = ClimbState.FORM_SEAL;
100+
CLIMB.setSlowTalonConfig(true);
101+
CLIMB.setVelocity(ClimbSubsystem.kSealOutputVelocity);
102+
}
103+
break;
104+
case RESET:
105+
if (VACUUM.isClimbOnTarget()) {
106+
logger.info("fast climbing during reset");
107+
CLIMB.setSlowTalonConfig(false);
108+
climbState = ClimbState.FAST_CLIMB;
109+
CLIMB.enableRatchet();
110+
VISION.startLightBlink(VisionSubsystem.LightPattern.CLIMB_GOOD);
111+
CLIMB.setVelocity(ClimbSubsystem.kDownClimbVelocity);
112+
break;
113+
}
114+
115+
if (CLIMB.getStringPotPosition() <= ClimbSubsystem.kHabHoverLvl2 + GOOD_ENOUGH) {
116+
climbState = ClimbState.PAUSE;
117+
CLIMB.stop();
118+
pauseInitTime = Timer.getFPGATimestamp();
119+
}
120+
121+
break;
122+
}
123+
}
124+
125+
@Override
126+
protected boolean isFinished() {
127+
return climbState == ClimbState.DONE;
128+
}
129+
130+
@Override
131+
protected void end() {
132+
CLIMB.stop();
133+
logger.info("END LVL 2 CLIMB SEQUENCE");
134+
}
135+
136+
private enum ClimbState {
137+
FAST_LOWER,
138+
FORM_SEAL,
139+
RESET,
140+
PRECHECK,
141+
PAUSE,
142+
FAST_CLIMB,
143+
DONE
144+
}
145+
}

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

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
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 ClimbLevel2DeployCommand 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 ClimbLevel2DeployCommand() {
17+
requires(CLIMB);
18+
}
19+
20+
@Override
21+
protected void initialize() {
22+
logger.info("Releasing - Lvl 2 Climb");
23+
currentState = State.RELEASE;
24+
CLIMB.setSlowTalonConfig(false);
25+
CLIMB.setSlowTalonConfig(false);
26+
CLIMB.setVelocity(ClimbSubsystem.kDownVelocity);
27+
}
28+
29+
@Override
30+
protected void execute() {
31+
switch (currentState) {
32+
case RELEASE:
33+
if (CLIMB.getStringPotPosition() >= ClimbSubsystem.kLowRelease - GOOD_ENOUGH) {
34+
currentState = State.POSITION;
35+
CLIMB.setVelocity(ClimbSubsystem.kUpVelocity);
36+
logger.info("Released: Position = {}", CLIMB.getStringPotPosition());
37+
}
38+
break;
39+
case POSITION:
40+
if (CLIMB.getStringPotPosition() <= ClimbSubsystem.kHighLvl2 + GOOD_ENOUGH) {
41+
logger.info("climber position = {}", CLIMB.getStringPotPosition());
42+
currentState = State.SLOW_DOWN;
43+
CLIMB.setSlowTalonConfig(true);
44+
CLIMB.setVelocity(ClimbSubsystem.kSealOutputVelocity);
45+
}
46+
break;
47+
case SLOW_DOWN:
48+
if (CLIMB.getStringPotPosition() >= ClimbSubsystem.kHabHoverLvl2 - GOOD_ENOUGH) {
49+
CLIMB.stop();
50+
currentState = State.DONE;
51+
logger.info("Climb Deployed: Position = {}", CLIMB.getStringPotPosition());
52+
}
53+
break;
54+
}
55+
}
56+
57+
@Override
58+
protected boolean isFinished() {
59+
return currentState == State.DONE;
60+
}
61+
62+
private enum State {
63+
RELEASE,
64+
POSITION,
65+
SLOW_DOWN,
66+
DONE
67+
}
68+
}
69+
70+
// 58

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

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
package frc.team2767.deepspace.command.climb;
2+
3+
import edu.wpi.first.wpilibj.command.CommandGroup;
4+
import frc.team2767.deepspace.command.elevator.ElevatorSetPositionCommand;
5+
import frc.team2767.deepspace.command.log.LogCommand;
6+
import frc.team2767.deepspace.command.teleop.StowElevatorConditionalCommand;
7+
import frc.team2767.deepspace.command.vacuum.PressureSetCommand;
8+
import frc.team2767.deepspace.command.vacuum.SetSolenoidStatesCommand;
9+
import frc.team2767.deepspace.subsystem.VacuumSubsystem;
10+
11+
public class DeploySequenceLvl2CommandGroup extends CommandGroup {
12+
public DeploySequenceLvl2CommandGroup() {
13+
addSequential(new LogCommand("BEGIN DEPLOY SEQUENCE"));
14+
addSequential(
15+
new CommandGroup() {
16+
{
17+
addSequential(new PressureSetCommand(VacuumSubsystem.kClimbPressureInHg), 0.3);
18+
addSequential(new SetSolenoidStatesCommand(VacuumSubsystem.SolenoidStates.CLIMB));
19+
addParallel(
20+
new CommandGroup() {
21+
{
22+
addSequential(new StowElevatorConditionalCommand());
23+
addSequential(new ElevatorSetPositionCommand(5.0), 0.5);
24+
}
25+
});
26+
addParallel(new EngageRatchetCommand(false));
27+
addParallel(new ClimbLevel2DeployCommand());
28+
}
29+
});
30+
addSequential(new LogCommand("END DEPLOY SEQUENCE"));
31+
}
32+
}

‎src/main/java/frc/team2767/deepspace/control/DriverControls.java‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,7 @@
1010
import frc.team2767.deepspace.command.biscuit.BiscuitPositiveCommand;
1111
import frc.team2767.deepspace.command.biscuit.BiscuitStopCommand;
1212
import frc.team2767.deepspace.command.biscuit.BiscuitZeroCommand;
13-
import frc.team2767.deepspace.command.climb.ClimbJogCommand;
14-
import frc.team2767.deepspace.command.climb.ClimbSlowVelocityCommand;
15-
import frc.team2767.deepspace.command.climb.StopClimbCommand;
13+
import frc.team2767.deepspace.command.climb.*;
1614
import frc.team2767.deepspace.command.log.LogCommand;
1715
import frc.team2767.deepspace.command.log.SafetyLogDumpCommand;
1816
import frc.team2767.deepspace.command.sequences.pickup.AutoCargoPickupCommandGroup;
@@ -48,6 +46,9 @@ public class DriverControls {
4846
new JoystickButton(joystick, Button.UP.id).whenReleased(new StopClimbCommand());
4947
new JoystickButton(joystick, Button.DOWN.id).whenPressed(new ClimbSlowVelocityCommand());
5048
new JoystickButton(joystick, Button.DOWN.id).whenReleased(new StopClimbCommand());
49+
// new JoystickButton(joystick, Button.UP.id).whenPressed(new ClimbLevel2AutoCommand());
50+
// new JoystickButton(joystick, Button.DOWN.id).whenPressed(new
51+
// DeploySequenceLvl2CommandGroup());
5152

5253
// vision
5354
new JoystickButton(joystick, Trim.RIGHT_Y_NEG.id)

‎src/main/java/frc/team2767/deepspace/control/SmartDashboardControls.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,7 @@
1717
import frc.team2767.deepspace.command.approach.sequences.AutoHatchPlaceCommandGroup;
1818
import frc.team2767.deepspace.command.biscuit.BiscuitExecutePlanCommand;
1919
import frc.team2767.deepspace.command.biscuit.BiscuitSetPositionCommand;
20-
import frc.team2767.deepspace.command.climb.ClimbAutoCommand;
21-
import frc.team2767.deepspace.command.climb.DeploySequenceCommandGroup;
22-
import frc.team2767.deepspace.command.climb.StopClimbCommand;
20+
import frc.team2767.deepspace.command.climb.*;
2321
import frc.team2767.deepspace.command.elevator.ElevatorSafeZeroCommand;
2422
import frc.team2767.deepspace.command.elevator.ElevatorSetPositionCommand;
2523
import frc.team2767.deepspace.command.intake.IntakePositionCommand;
@@ -66,6 +64,8 @@ private void addMatchCommands() {
6664
SmartDashboard.putData("Game/SandstormHatchPickUp", new SandstormHatchPickupCommandGroup());
6765
// SmartDashboard.putData("Game/hatchPlace", new AutoHatchPlaceCommandGroup(0.0));
6866
SmartDashboard.putData("Game/Gyro", Robot.DRIVE.getGyro());
67+
SmartDashboard.putData("Game/Lvl2Deploy", new ClimbLevel2DeployCommand());
68+
SmartDashboard.putData("Game/Lvl2Climb", new ClimbLevel2AutoCommand());
6969
}
7070

7171
private void addClimbTab() {

‎src/main/java/frc/team2767/deepspace/subsystem/ClimbSubsystem.java‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -35,13 +35,16 @@ public class ClimbSubsystem extends Subsystem implements Item {
3535
public static double kJogUpPercent = -0.20;
3636
public static double kJogDownPercent = 0.20;
3737
public static int kDownVelocity = 2500;
38-
public static int kUpVelocity = -2500;
38+
public static int kUpVelocity = -1200;
3939
public static int kDownClimbVelocity = 1750;
4040
public static int kHabHover;
41+
public static int kHabHoverLvl2;
4142
public static int kLowRelease;
4243
public static int kHighRelease;
44+
public static int kHighLvl2;
4345
public static int kClimb;
4446
public static int kTooLowIn;
47+
public static int kTooLowLvl2;
4548
public static boolean isReleased;
4649
private static double kLeftKickstandHold;
4750
private static double kLeftKickstandRelease;
@@ -66,11 +69,14 @@ public ClimbSubsystem() {
6669
}
6770

6871
private void climbPrefs() {
69-
kHabHover = (int) getPrefs("low_position", 209);
72+
kHabHover = (int) getPrefs("lowf_position", 209);
73+
kHabHoverLvl2 = (int) getPrefs("lvl2_hover_position", 683);
7074
kLowRelease = (int) getPrefs("medium_position", 679);
7175
kHighRelease = (int) getPrefs("high_position", 189);
76+
kHighLvl2 = (int) getPrefs("Lvl2_high_position", 600);
7277
kClimb = (int) getPrefs("climb_position", 884);
7378
kTooLowIn = (int) getPrefs("too_low_position", 240);
79+
kTooLowLvl2 = (int) getPrefs("too_low_Lvl2", 656);
7480
kSealOutputVelocity = (int) getPrefs("seal_velocity", 300);
7581

7682
kLeftKickstandHold = getPrefs("L_kickstand_hold", 0.4);

0 commit comments

Comments
 (0)