Skip to content

Commit e869a6c

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #228 from strykeforce/rabsamu/hatch-place
Rabsamu/hatch place
2 parents bbd59b7 + de3f28d commit e869a6c

12 files changed

+348
-30
lines changed

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,7 @@ public void robotInit() {
8888
public void autonomousInit() {
8989
BISCUIT.setPosition(BISCUIT.getPosition());
9090
DRIVE.setAngleAdjustment(true);
91+
VISION.startSide = StartSide.RIGHT;
9192
getHatch.start();
9293
}
9394

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

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
6+
import frc.team2767.deepspace.Robot;
7+
import frc.team2767.deepspace.subsystem.*;
8+
import org.slf4j.Logger;
9+
import org.slf4j.LoggerFactory;
10+
import org.strykeforce.thirdcoast.swerve.SwerveDrive;
11+
12+
public class HoldHeadingUntilCompressionCommand extends Command {
13+
private static final DriveSubsystem DRIVE = Robot.DRIVE;
14+
private static final BiscuitSubsystem BISCUIT = Robot.BISCUIT;
15+
private static final VacuumSubsystem VACUUM = Robot.VACUUM;
16+
private static final VisionSubsystem VISION = Robot.VISION;
17+
private static final double STRAFE_OUTPUT = 0.2;
18+
private static final double OUT_TIME_SEC = 1.0;
19+
private static final double PAUSE_TIME = 0.2;
20+
private static DriveState driveState;
21+
private static double strafe;
22+
private static double outInitTime;
23+
private final Logger logger = LoggerFactory.getLogger(this.getClass());
24+
private double pauseInitTime;
25+
26+
public HoldHeadingUntilCompressionCommand() {
27+
requires(DRIVE);
28+
}
29+
30+
@Override
31+
protected void initialize() {
32+
driveState = DriveState.PLACE;
33+
DRIVE.setDriveMode(SwerveDrive.DriveMode.CLOSED_LOOP);
34+
if (VISION.startSide == StartSide.LEFT) {
35+
strafe = STRAFE_OUTPUT;
36+
} else {
37+
strafe = STRAFE_OUTPUT * -1;
38+
}
39+
logger.info("Hold Heading Until Compression");
40+
DRIVE.drive(0.0, strafe, 0.0);
41+
}
42+
43+
@Override
44+
protected void execute() {
45+
switch (driveState) {
46+
case PLACE:
47+
if (BISCUIT.isCompressed()) {
48+
driveState = DriveState.PAUSE;
49+
DRIVE.stop();
50+
VACUUM.setSolenoidsState(VacuumSubsystem.SolenoidStates.PRESSURE_ACCUMULATE);
51+
logger.info("Compression reached");
52+
pauseInitTime = Timer.getFPGATimestamp();
53+
}
54+
break;
55+
case PAUSE:
56+
logger.debug("{} - {} > {}", Timer.getFPGATimestamp(), pauseInitTime, PAUSE_TIME);
57+
if (Timer.getFPGATimestamp() - pauseInitTime > PAUSE_TIME) {
58+
if (VISION.startSide == StartSide.LEFT) {
59+
strafe = STRAFE_OUTPUT * -1;
60+
} else {
61+
strafe = STRAFE_OUTPUT;
62+
}
63+
DRIVE.drive(0.0, strafe, 0.0);
64+
driveState = DriveState.OUT;
65+
logger.debug("pause hatch place");
66+
VISION.enableLights(false);
67+
SmartDashboard.putBoolean("Game/haveHatch", false);
68+
outInitTime = Timer.getFPGATimestamp();
69+
}
70+
break;
71+
72+
case OUT:
73+
if (Timer.getFPGATimestamp() - outInitTime > OUT_TIME_SEC) {
74+
driveState = DriveState.DONE;
75+
logger.info("Done Auto Hatch Place");
76+
}
77+
break;
78+
}
79+
}
80+
81+
@Override
82+
protected boolean isFinished() {
83+
return driveState == DriveState.DONE;
84+
}
85+
86+
@Override
87+
protected void end() {
88+
DRIVE.stop();
89+
}
90+
91+
private enum DriveState {
92+
PLACE,
93+
PAUSE,
94+
OUT,
95+
DONE;
96+
97+
DriveState() {}
98+
}
99+
}

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -20,14 +20,14 @@
2020
import org.strykeforce.thirdcoast.util.RateLimit;
2121

2222
public class VisionAutoAlignPickupCommand extends Command implements Item {
23-
public static final double kP_STRAFE = 0.1; // 0.09
23+
public static final double kP_STRAFE = 0.06; // 0.1
2424
private static final double DRIVE_EXPO = 0.5;
2525
private static final double YAW_EXPO = 0.5;
2626
private static final double DEADBAND = 0.05;
2727
private static final double kP_YAW = 0.01; // 0.00625 tuning for NT method, 0.01 pyeye
2828
private static final double MAX_YAW = 0.3;
2929
private static final double MIN_RANGE = 35.0;
30-
private static final double FWD_SCALE = 0.3;
30+
private static final double FWD_SCALE = 0.5;
3131
private static final double FWD_SCALE_FAST = 0.5;
3232
private static final double goodEnoughYaw = 1.5;
3333

@@ -50,7 +50,7 @@ public VisionAutoAlignPickupCommand() {
5050
requires(DRIVE);
5151
this.driveExpo = new ExpoScale(DEADBAND, DRIVE_EXPO);
5252
this.yawExpo = new ExpoScale(DEADBAND, YAW_EXPO);
53-
rateLimit = new RateLimit(0.015);
53+
rateLimit = new RateLimit(0.04); // 0.015
5454

5555
TelemetryService telemetryService = Robot.TELEMETRY;
5656
telemetryService.stop();
@@ -107,7 +107,7 @@ protected boolean isFinished() {
107107

108108
@Override
109109
protected void end() {
110-
logger.info("End Auto Align Vision");
110+
logger.info("End Auto Align Pickup Vision");
111111
}
112112

113113
private double deadband(double value) {
@@ -118,7 +118,7 @@ private double deadband(double value) {
118118
@NotNull
119119
@Override
120120
public String getDescription() {
121-
return "VisionYawCommand";
121+
return "VisionPickupCommand";
122122
}
123123

124124
@Override

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

Lines changed: 144 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,144 @@
1+
package frc.team2767.deepspace.command.approach;
2+
3+
import edu.wpi.first.wpilibj.command.Command;
4+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
5+
import frc.team2767.deepspace.Robot;
6+
import frc.team2767.deepspace.subsystem.DriveSubsystem;
7+
import frc.team2767.deepspace.subsystem.StartSide;
8+
import frc.team2767.deepspace.subsystem.VisionSubsystem;
9+
import java.util.Set;
10+
import java.util.function.DoubleSupplier;
11+
import org.jetbrains.annotations.NotNull;
12+
import org.slf4j.Logger;
13+
import org.slf4j.LoggerFactory;
14+
import org.strykeforce.thirdcoast.swerve.SwerveDrive;
15+
import org.strykeforce.thirdcoast.telemetry.TelemetryService;
16+
import org.strykeforce.thirdcoast.telemetry.grapher.Measure;
17+
import org.strykeforce.thirdcoast.telemetry.item.Item;
18+
import org.strykeforce.thirdcoast.util.RateLimit;
19+
20+
public class VisionAutoAlignPlaceCommand extends Command implements Item {
21+
private static final double kP_FORWARD = -0.11; // 0.1
22+
private static final double kP_YAW = 0.01; // 0.00625 tuning for NT method, 0.01 pyeye
23+
private static final double MAX_YAW = 0.3;
24+
private static final double MIN_RANGE = 35.0;
25+
private static final double STRAFE_OUTPUT = 0.2;
26+
private static final double goodEnoughYaw = 1.5;
27+
28+
private static final DriveSubsystem DRIVE = Robot.DRIVE;
29+
private static final VisionSubsystem VISION = Robot.VISION;
30+
31+
private static double range;
32+
private static double strafe;
33+
private static double forwardError;
34+
private static double yawError;
35+
private final Logger logger = LoggerFactory.getLogger(this.getClass());
36+
private double targetYaw;
37+
private boolean isGood = false;
38+
private static double forwardCorrection;
39+
private RateLimit rateLimit;
40+
41+
public VisionAutoAlignPlaceCommand() {
42+
requires(DRIVE);
43+
rateLimit = new RateLimit(0.05); // 0.015
44+
45+
TelemetryService telemetryService = Robot.TELEMETRY;
46+
telemetryService.stop();
47+
telemetryService.register(this);
48+
}
49+
50+
@Override
51+
protected void initialize() {
52+
SmartDashboard.putBoolean("Game/haveHatch", true);
53+
logger.info("Begin Vision Auto Align Place");
54+
DRIVE.setDriveMode(SwerveDrive.DriveMode.CLOSED_LOOP);
55+
forwardCorrection = VISION.getStrafeCorrection();
56+
if (VISION.startSide == StartSide.LEFT) {
57+
targetYaw = 180.0;
58+
strafe = STRAFE_OUTPUT;
59+
} else {
60+
targetYaw = 0.0;
61+
strafe = STRAFE_OUTPUT * -1;
62+
}
63+
logger.info("Target Yaw: {}", targetYaw);
64+
}
65+
66+
@SuppressWarnings("Duplicates")
67+
@Override
68+
protected void execute() {
69+
// Pyeye Method:
70+
VISION.queryPyeye(); // gets corrected heading and range from NT
71+
range = VISION.getRawRange();
72+
isGood = range >= 0; // check if range is good (we have a target), not -1
73+
74+
// Calculate Yaw Term based on gyro
75+
yawError = targetYaw - Math.IEEEremainder(DRIVE.getGyro().getAngle(), 360.0);
76+
double yaw = kP_YAW * yawError;
77+
if (yaw > MAX_YAW) yaw = MAX_YAW;
78+
if (yaw < -MAX_YAW) yaw = -MAX_YAW;
79+
80+
// Determine if actual yaw is close enough to target
81+
boolean onTarget = Math.abs(yawError) <= goodEnoughYaw;
82+
83+
double forward;
84+
forwardError = Math.sin(Math.toRadians(VISION.getRawBearing())) * range - forwardCorrection;
85+
// Only take over strafe control if pyeye has a target and the robot is straight to the field
86+
if (isGood && onTarget) forward = forwardError * kP_FORWARD * strafe;
87+
else forward = 0;
88+
89+
DRIVE.drive(rateLimit.apply(forward), strafe, yaw);
90+
}
91+
92+
@Override
93+
protected boolean isFinished() {
94+
return (range <= MIN_RANGE && isGood);
95+
}
96+
97+
@Override
98+
protected void end() {
99+
logger.info("End Auto Align Place Vision");
100+
}
101+
102+
@NotNull
103+
@Override
104+
public String getDescription() {
105+
return "VisionPlaceCommand";
106+
}
107+
108+
@Override
109+
public int getDeviceId() {
110+
return 0;
111+
}
112+
113+
@NotNull
114+
@Override
115+
public Set<Measure> getMeasures() {
116+
return Set.of(Measure.ANGLE, Measure.RANGE);
117+
}
118+
119+
@NotNull
120+
@Override
121+
public String getType() {
122+
return "VisionCommand";
123+
}
124+
125+
@Override
126+
public int compareTo(@NotNull Item item) {
127+
return 0;
128+
}
129+
130+
@NotNull
131+
@Override
132+
public DoubleSupplier measurementFor(@NotNull Measure measure) {
133+
switch (measure) {
134+
case ANGLE:
135+
return () -> yawError;
136+
case RANGE:
137+
return () -> forwardError;
138+
// case COMPONENT_STRAFE:
139+
// return () -> strafe;
140+
default:
141+
return () -> 2767;
142+
}
143+
}
144+
}

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

Lines changed: 2 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -4,10 +4,7 @@
44
import frc.team2767.deepspace.command.approach.ApproachDirectionCommand;
55
import frc.team2767.deepspace.command.approach.HoldHeadingUntilSuctionCommand;
66
import frc.team2767.deepspace.command.approach.VisionAutoAlignPickupCommand;
7-
import frc.team2767.deepspace.command.biscuit.BiscuitConfigMotionAccelCommand;
8-
import frc.team2767.deepspace.command.biscuit.BiscuitExecutePlanCommand;
9-
import frc.team2767.deepspace.command.biscuit.BiscuitPositionAboveCameraCommand;
10-
import frc.team2767.deepspace.command.biscuit.ReleaseKrakenCommand;
7+
import frc.team2767.deepspace.command.biscuit.*;
118
import frc.team2767.deepspace.command.elevator.ElevatorExecutePlanCommand;
129
import frc.team2767.deepspace.command.log.LogCommand;
1310
import frc.team2767.deepspace.command.states.SetActionCommand;
@@ -39,7 +36,7 @@ public AutoHatchPickupCommandGroup() {
3936
addParallel(new BiscuitPositionAboveCameraCommand());
4037
addParallel(new ElevatorExecutePlanCommand());
4138
addSequential(new VisionAutoAlignPickupCommand());
42-
addSequential(new BiscuitExecutePlanCommand());
39+
addSequential(new BiscuitFastExecuteCommand());
4340
addSequential(new HoldHeadingUntilSuctionCommand());
4441
addParallel(new BiscuitConfigMotionAccelCommand(BiscuitSubsystem.kSlowAccel));
4542
addParallel(new SetActionCommand(Action.PLACE));

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

Lines changed: 16 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -6,11 +6,14 @@
66
import frc.team2767.deepspace.command.approach.*;
77
import frc.team2767.deepspace.command.biscuit.BiscuitExecutePlanCommand;
88
import frc.team2767.deepspace.command.biscuit.BiscuitPositionAboveCameraCommand;
9+
import frc.team2767.deepspace.command.elevator.ElevatorSetPositionCommand;
910
import frc.team2767.deepspace.command.log.LogCommand;
11+
import frc.team2767.deepspace.command.states.SetActionCommand;
1012
import frc.team2767.deepspace.command.states.SetFieldDirectionCommand;
11-
import frc.team2767.deepspace.command.teleop.DriverPlaceAssistCommand;
13+
import frc.team2767.deepspace.command.states.SetGamePieceCommand;
14+
import frc.team2767.deepspace.command.states.SetLevelCommand;
1215
import frc.team2767.deepspace.command.vision.LightsOnCommand;
13-
import frc.team2767.deepspace.subsystem.FieldDirection;
16+
import frc.team2767.deepspace.subsystem.*;
1417

1518
public class AutoHatchPlaceCommandGroup extends CommandGroup {
1619

@@ -25,10 +28,19 @@ protected boolean condition() {
2528
}
2629
});
2730
addSequential(new LightsOnCommand());
31+
addSequential(
32+
new CommandGroup() {
33+
{
34+
addParallel(new SetActionCommand(Action.PLACE));
35+
addParallel(new SetGamePieceCommand(GamePiece.HATCH));
36+
addParallel(new SetLevelCommand(ElevatorLevel.ONE));
37+
}
38+
});
39+
addSequential(new ElevatorSetPositionCommand(ElevatorSubsystem.kHatchLowPositionInches));
2840
addSequential(new BiscuitPositionAboveCameraCommand());
29-
addSequential(new DriverPlaceAssistCommand());
41+
addSequential(new VisionAutoAlignPlaceCommand());
3042
addSequential(new BiscuitExecutePlanCommand());
31-
addSequential(new HoldHeadingCommand());
43+
addSequential(new HoldHeadingUntilCompressionCommand());
3244
addSequential(new LogCommand("END HATCH PLACE"));
3345
}
3446
}

‎src/main/java/frc/team2767/deepspace/command/biscuit/BiscuitFastExecuteCommand.java‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
package frc.team2767.deepspace.command.biscuit;
2+
3+
import edu.wpi.first.wpilibj.command.Command;
4+
import frc.team2767.deepspace.Robot;
5+
import frc.team2767.deepspace.subsystem.BiscuitSubsystem;
6+
7+
public class BiscuitFastExecuteCommand extends Command {
8+
9+
private static final BiscuitSubsystem BISCUIT = Robot.BISCUIT;
10+
11+
public BiscuitFastExecuteCommand() {
12+
requires(BISCUIT);
13+
}
14+
15+
@Override
16+
protected void initialize() {
17+
BISCUIT.setMotionMagicAccel(BiscuitSubsystem.kFastAccel);
18+
BISCUIT.executePlan();
19+
}
20+
21+
@Override
22+
protected boolean isFinished() {
23+
return BISCUIT.onTarget();
24+
}
25+
26+
@Override
27+
protected void end() {
28+
BISCUIT.setMotionMagicAccel(BiscuitSubsystem.kSlowAccel);
29+
}
30+
}

0 commit comments

Comments
 (0)