Skip to content

Commit 9577066

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #204 from strykeforce/rkalnins/auto-hatch-place
Rkalnins/tune auto hatch pickup
2 parents 81ff009 + da36a7a commit 9577066

22 files changed

+227
-69
lines changed

‎src/main/java/frc/team2767/deepspace/Robot.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
import edu.wpi.first.wpilibj.command.CommandGroup;
77
import edu.wpi.first.wpilibj.command.Scheduler;
88
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
9-
import frc.team2767.deepspace.command.sequences.SandstormHatchPickupCommandGroup;
9+
import frc.team2767.deepspace.command.sequences.pickup.SandstormHatchPickupCommandGroup;
1010
import frc.team2767.deepspace.control.Controls;
1111
import frc.team2767.deepspace.subsystem.*;
1212
import frc.team2767.deepspace.subsystem.safety.SafetySubsystem;

‎src/main/java/frc/team2767/deepspace/command/approach/OpenLoopDriveUntilCurrentCommand.java‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
package frc.team2767.deepspace.command.approach;
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.DriveSubsystem;
7+
import frc.team2767.deepspace.subsystem.VacuumSubsystem;
8+
import org.slf4j.Logger;
9+
import org.slf4j.LoggerFactory;
10+
11+
public class OpenLoopDriveUntilCurrentCommand extends Command {
12+
13+
private static final DriveSubsystem DRIVE = Robot.DRIVE;
14+
private static final VacuumSubsystem VACUUM = Robot.VACUUM;
15+
private static final double TRANSITION_CURRENT_DIFFERENCE = 50.0;
16+
private static final double DIRECTION = -0.25;
17+
private static final double OUT_DRIVE_SECONDS = 0.5;
18+
private static final double SOLENOID_DELAY = 0.2;
19+
private final Logger logger = LoggerFactory.getLogger(this.getClass());
20+
private DriveState driveState;
21+
private double initialCurrent;
22+
private double outDriveInitTime;
23+
private double solenoidDelayInit;
24+
25+
public OpenLoopDriveUntilCurrentCommand() {
26+
setTimeout(5.0);
27+
requires(DRIVE);
28+
}
29+
30+
@Override
31+
protected void initialize() {
32+
driveState = DriveState.FAST;
33+
initialCurrent = DRIVE.getAverageOutputCurrent();
34+
}
35+
36+
@Override
37+
protected void execute() {
38+
switch (driveState) {
39+
case FAST:
40+
DRIVE.setWheels(DIRECTION, DriveState.FAST.velocity);
41+
double averageCurrent = DRIVE.getAverageOutputCurrent();
42+
if (averageCurrent > initialCurrent + TRANSITION_CURRENT_DIFFERENCE) {
43+
driveState = DriveState.CLOSE_SOLENOID;
44+
logger.debug("set state to {}, current = {}", driveState, averageCurrent);
45+
}
46+
break;
47+
case CLOSE_SOLENOID:
48+
DRIVE.setWheels(DIRECTION, DriveState.CLOSE_SOLENOID.velocity);
49+
VACUUM.setSolenoidsState(VacuumSubsystem.SolenoidStates.PRESSURE_ACCUMULATE);
50+
solenoidDelayInit = Timer.getFPGATimestamp();
51+
driveState = DriveState.WAIT;
52+
case WAIT:
53+
if (Timer.getFPGATimestamp() - solenoidDelayInit > SOLENOID_DELAY) {
54+
driveState = DriveState.OUT;
55+
outDriveInitTime = Timer.getFPGATimestamp();
56+
logger.debug("set state to {}", driveState);
57+
}
58+
break;
59+
case OUT:
60+
DRIVE.setWheels(DIRECTION, DriveState.OUT.velocity);
61+
if (Timer.getFPGATimestamp() - outDriveInitTime > OUT_DRIVE_SECONDS) {
62+
driveState = DriveState.DONE;
63+
logger.debug("set state to {}", driveState);
64+
}
65+
break;
66+
case DONE:
67+
break;
68+
}
69+
}
70+
71+
@Override
72+
protected boolean isFinished() {
73+
return false;
74+
}
75+
76+
private enum DriveState {
77+
FAST(0.5),
78+
CLOSE_SOLENOID(0.06),
79+
WAIT(0.06),
80+
OUT(-0.6),
81+
DONE(0.0);
82+
83+
private double velocity;
84+
85+
DriveState(double velocity) {
86+
this.velocity = velocity;
87+
}
88+
}
89+
}

‎src/main/java/frc/team2767/deepspace/command/approach/OpenLoopDriveUntilSuctionCommand.java‎

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -49,17 +49,18 @@ protected void execute() {
4949
switch (driveState) {
5050
case FAST:
5151
DRIVE.setWheels(direction, DriveState.FAST.velocity);
52-
53-
if (VACUUM.getPressure() - initialPressure > TRANSITION_PRESSURE_DIFFERENCE) {
54-
logger.debug("current pressure = {}", VACUUM.getPressure());
52+
double currentFastPressure = VACUUM.getPressure();
53+
if (currentFastPressure - initialPressure > TRANSITION_PRESSURE_DIFFERENCE) {
54+
logger.debug("current pressure = {}", currentFastPressure);
5555
driveState = DriveState.SLOW;
5656
}
5757
break;
5858

5959
case SLOW:
6060
DRIVE.setWheels(direction, DriveState.SLOW.velocity);
61-
if (VACUUM.getPressure() - initialPressure > HATCH_SEAL_GOOD_ENOUGH) {
62-
logger.debug("current pressure = {}", VACUUM.getPressure());
61+
double currentSlowPressure = VACUUM.getPressure();
62+
if (currentSlowPressure - initialPressure > HATCH_SEAL_GOOD_ENOUGH) {
63+
logger.debug("pressure reached: current pressure = {}", currentSlowPressure);
6364
outDriveInitTime = Timer.getFPGATimestamp();
6465
driveState = DriveState.OUT;
6566
}
@@ -82,13 +83,13 @@ protected boolean isFinished() {
8283
protected void end() {
8384
if (isTimedOut()) {
8485
logger.info("Timed Out");
85-
} else logger.info("Pressure Reached");
86+
}
8687
}
8788

8889
private enum DriveState {
89-
SLOW(0.06),
9090
FAST(0.2),
91-
OUT(-0.4),
91+
SLOW(0.06),
92+
OUT(-0.9),
9293
DONE(0.0);
9394

9495
private double velocity;

‎src/main/java/frc/team2767/deepspace/command/approach/AutoHatchPickupCommandGroup.java‎ renamed to ‎src/main/java/frc/team2767/deepspace/command/approach/sequences/AutoHatchPickupCommandGroup.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
1-
package frc.team2767.deepspace.command.approach;
1+
package frc.team2767.deepspace.command.approach.sequences;
22

33
import edu.wpi.first.wpilibj.command.CommandGroup;
4+
import frc.team2767.deepspace.command.approach.ApproachDirectionCommand;
45
import frc.team2767.deepspace.command.log.LogCommand;
56

67
public class AutoHatchPickupCommandGroup extends CommandGroup {

‎src/main/java/frc/team2767/deepspace/command/approach/sequences/SandstormHatchPlaceCommandGroup.java‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
package frc.team2767.deepspace.command.approach.sequences;
2+
3+
import edu.wpi.first.wpilibj.command.CommandGroup;
4+
import frc.team2767.deepspace.command.approach.CalculateRotationCommand;
5+
import frc.team2767.deepspace.command.approach.OpenLoopDriveUntilCurrentCommand;
6+
import frc.team2767.deepspace.command.approach.YawToTargetCommand;
7+
import frc.team2767.deepspace.command.biscuit.BiscuitExecutePlanCommand;
8+
import frc.team2767.deepspace.command.elevator.ElevatorExecutePlanCommand;
9+
import frc.team2767.deepspace.command.states.SetActionCommand;
10+
import frc.team2767.deepspace.command.states.SetFieldDirectionCommand;
11+
import frc.team2767.deepspace.command.states.SetGamePieceCommand;
12+
import frc.team2767.deepspace.command.vision.LightsOnCommand;
13+
import frc.team2767.deepspace.command.vision.QueryPyeyeCommand;
14+
import frc.team2767.deepspace.subsystem.Action;
15+
import frc.team2767.deepspace.subsystem.FieldDirection;
16+
import frc.team2767.deepspace.subsystem.GamePiece;
17+
18+
public class SandstormHatchPlaceCommandGroup extends CommandGroup {
19+
20+
public SandstormHatchPlaceCommandGroup() {
21+
addSequential(
22+
new CommandGroup() {
23+
{
24+
addSequential(new SetFieldDirectionCommand(FieldDirection.LEFT));
25+
addParallel(new SetActionCommand(Action.PLACE));
26+
addParallel(new SetGamePieceCommand(GamePiece.HATCH));
27+
}
28+
});
29+
30+
addSequential(new LightsOnCommand());
31+
addSequential(new QueryPyeyeCommand());
32+
addSequential(new CalculateRotationCommand());
33+
addSequential(new YawToTargetCommand());
34+
addSequential(new BiscuitExecutePlanCommand());
35+
addSequential(new ElevatorExecutePlanCommand());
36+
addSequential(new OpenLoopDriveUntilCurrentCommand(), 5.0);
37+
}
38+
}

‎src/main/java/frc/team2767/deepspace/command/approach/SnapDriveCommandGroup.java‎ renamed to ‎src/main/java/frc/team2767/deepspace/command/approach/sequences/SnapDriveCommandGroup.java‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,9 @@
1-
package frc.team2767.deepspace.command.approach;
1+
package frc.team2767.deepspace.command.approach.sequences;
22

33
import edu.wpi.first.wpilibj.command.CommandGroup;
4+
import frc.team2767.deepspace.command.approach.CalculateRotationCommand;
5+
import frc.team2767.deepspace.command.approach.OpenLoopDriveUntilSuctionCommand;
6+
import frc.team2767.deepspace.command.approach.YawToTargetCommand;
47
import frc.team2767.deepspace.command.biscuit.BiscuitExecutePlanCommand;
58
import frc.team2767.deepspace.command.elevator.ElevatorExecutePlanCommand;
69
import frc.team2767.deepspace.command.log.LogCommand;
@@ -17,7 +20,7 @@
1720
public class SnapDriveCommandGroup extends CommandGroup {
1821

1922
public SnapDriveCommandGroup() {
20-
addSequential(new LogCommand("BEGIN ATUO SNAP DRIVE HATCH PICKUP"));
23+
addSequential(new LogCommand("BEGIN AUTO SNAP DRIVE HATCH PICKUP"));
2124
addSequential(new LightsOnCommand());
2225
addSequential(new SetSolenoidStatesCommand(VacuumSubsystem.SolenoidStates.PRESSURE_ACCUMULATE));
2326
addSequential(

‎src/main/java/frc/team2767/deepspace/command/approach/SquareTwistCommandGroup.java‎ renamed to ‎src/main/java/frc/team2767/deepspace/command/approach/sequences/SquareTwistCommandGroup.java‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,10 @@
1-
package frc.team2767.deepspace.command.approach;
1+
package frc.team2767.deepspace.command.approach.sequences;
22

33
import edu.wpi.first.wpilibj.command.CommandGroup;
44
import edu.wpi.first.wpilibj.command.WaitCommand;
5+
import frc.team2767.deepspace.command.approach.CalculateTwistCommand;
6+
import frc.team2767.deepspace.command.approach.FieldSquarePickupAlignmentCommand;
7+
import frc.team2767.deepspace.command.approach.VisionTwistCommand;
58
import frc.team2767.deepspace.command.log.LogCommand;
69
import frc.team2767.deepspace.command.vision.LightsOnCommand;
710
import frc.team2767.deepspace.command.vision.QueryPyeyeCommand;

‎src/main/java/frc/team2767/deepspace/command/sequences/StowAllCommandGroup.java‎

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,14 @@
1010
public class StowAllCommandGroup extends CommandGroup {
1111
public StowAllCommandGroup() {
1212
addSequential(new LogCommand("BEGIN STOW ALL"));
13-
addParallel(new StowValveControlCommand());
14-
addParallel(new StowElevatorConditionalCommand());
15-
addParallel(new IntakePositionCommand(IntakeSubsystem.kStowPositionDeg));
13+
addSequential(
14+
new CommandGroup() {
15+
{
16+
addParallel(new StowValveControlCommand());
17+
addParallel(new StowElevatorConditionalCommand());
18+
addParallel(new IntakePositionCommand(IntakeSubsystem.kStowPositionDeg));
19+
}
20+
});
1621
addSequential(new LogCommand("END STOW ALL"));
1722
}
1823
}

‎src/main/java/frc/team2767/deepspace/command/sequences/CargoGroundPickupCommandGroup.java‎ renamed to ‎src/main/java/frc/team2767/deepspace/command/sequences/pickup/CargoGroundPickupCommandGroup.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
package frc.team2767.deepspace.command.sequences;
1+
package frc.team2767.deepspace.command.sequences.pickup;
22

33
import edu.wpi.first.wpilibj.command.CommandGroup;
44
import frc.team2767.deepspace.command.biscuit.BiscuitSetPositionCommand;

‎src/main/java/frc/team2767/deepspace/command/sequences/CoconutPickupCommandGroup.java‎ renamed to ‎src/main/java/frc/team2767/deepspace/command/sequences/pickup/CoconutPickupCommandGroup.java‎

Lines changed: 13 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
package frc.team2767.deepspace.command.sequences;
1+
package frc.team2767.deepspace.command.sequences.pickup;
22

33
import edu.wpi.first.wpilibj.command.CommandGroup;
44
import frc.team2767.deepspace.command.biscuit.BiscuitSetPositionCommand;
@@ -32,7 +32,7 @@ public CoconutPickupCommandGroup() {
3232
{
3333
addSequential(
3434
new SetSolenoidStatesCommand(
35-
VacuumSubsystem.SolenoidStates.PRESSURE_ACCUMULATE));
35+
VacuumSubsystem.SolenoidStates.GAME_PIECE_PICKUP));
3636
addSequential(new PressureSetCommand(VacuumSubsystem.kBallPressureInHg), 0.5);
3737
}
3838
});
@@ -41,18 +41,19 @@ public CoconutPickupCommandGroup() {
4141
}
4242
});
4343

44+
/*addSequential(
45+
new CommandGroup() {
46+
{
47+
addParallel(
48+
new SetSolenoidStatesCommand(VacuumSubsystem.SolenoidStates.GAME_PIECE_PICKUP));
49+
addParallel(new ElevatorDownFastOpenLoopCommand());
50+
}
51+
});*/
52+
4453
addParallel(new IntakePositionCommand(105));
4554
addSequential(new BiscuitSetPositionCommand(BiscuitSubsystem.kDownPosition));
46-
addSequential(
47-
new CommandGroup() {
48-
{
49-
addParallel(
50-
new SetSolenoidStatesCommand(VacuumSubsystem.SolenoidStates.GAME_PIECE_PICKUP));
51-
addParallel(new ElevatorDownFastOpenLoopCommand());
52-
}
53-
});
54-
55-
addSequential(new WaitForPressureCommand(VacuumSubsystem.kBallPressureInHg));
55+
addSequential(new ElevatorDownFastOpenLoopCommand());
56+
addSequential(new WaitForPressureCommand());
5657
addSequential(new ElevatorSetPositionCommand(25.0));
5758
addParallel(new SetActionCommand(Action.PLACE));
5859
addParallel(new RollerStopCommand());

0 commit comments

Comments
 (0)