Skip to content

Commit 92fb7a8

Browse files
PotatoBoyH4PotatoBoyH4
committed
made paths work in theory, but it goes crazy
1 parent 5400cc5 commit 92fb7a8

File tree

11 files changed

+213
-90
lines changed

11 files changed

+213
-90
lines changed

‎src/main/deploy/choreo/startToJ.traj‎

Lines changed: 61 additions & 41 deletions
Large diffs are not rendered by default.

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 20 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -118,6 +118,8 @@ public class RobotContainer {
118118

119119
private final TelemetryService telemetryService = new TelemetryService(TelemetryController::new);
120120

121+
private NonProcessorShallowAutonCommand nonProcessorShallowAutonCommand;
122+
121123
public RobotContainer() {
122124

123125
algaeIO = new AlgaeIOFX();
@@ -167,6 +169,23 @@ public RobotContainer() {
167169

168170
pathHandler = new PathHandler(driveSubsystem, tagAlignSubsystem, robotStateSubsystem);
169171

172+
nonProcessorShallowAutonCommand =
173+
new NonProcessorShallowAutonCommand(
174+
driveSubsystem,
175+
pathHandler,
176+
robotStateSubsystem,
177+
algaeSubsystem,
178+
biscuitSubsystem,
179+
coralSubsystem,
180+
elevatorSubsystem,
181+
tagAlignSubsystem,
182+
"startToJ",
183+
new ArrayList<Character>(Arrays.asList('k', 'l', 'a')),
184+
new ArrayList<Integer>(Arrays.asList(4, 4, 4)),
185+
'j');
186+
187+
nonProcessorShallowAutonCommand.reassignAlliance();
188+
170189
configureTelemetry();
171190
configureDriverBindings();
172191
configureOperatorBindings();
@@ -456,21 +475,7 @@ public void configurePitDashboard() {
456475
.withSize(1, 1);
457476

458477
Shuffleboard.getTab("Pit")
459-
.add(
460-
"Start Auton",
461-
new NonProcessorShallowAutonCommand(
462-
driveSubsystem,
463-
pathHandler,
464-
robotStateSubsystem,
465-
algaeSubsystem,
466-
biscuitSubsystem,
467-
coralSubsystem,
468-
elevatorSubsystem,
469-
tagAlignSubsystem,
470-
"startToJ",
471-
new ArrayList<Character>(Arrays.asList('k', 'l', 'm')),
472-
new ArrayList<Integer>(Arrays.asList(4, 4, 4)),
473-
'j'))
478+
.add("Start Auton", nonProcessorShallowAutonCommand)
474479
.withPosition(3, 0)
475480
.withSize(1, 1);
476481
}

‎src/main/java/frc/robot/commands/auton/AutoCommandInterface.java‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,5 @@
11
package frc.robot.commands.auton;
22

3-
import edu.wpi.first.wpilibj2.command.Command;
4-
53
public interface AutoCommandInterface {
64
public void reassignAlliance();
75
}

‎src/main/java/frc/robot/constants/DriveConstants.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -44,14 +44,14 @@ public class DriveConstants {
4444
public static final double kFieldMaxY = 8.0518;
4545
public static final double kCenterLineX = 8.763;
4646

47-
public static final double kPOmega = 4.5;
47+
public static final double kPOmega = 6.0;
4848
public static final double kIOmega = 0.0;
4949
public static final double kDOmega = 0.0;
5050
public static final double kMaxVelOmega =
5151
(kMaxSpeedMetersPerSecond / Math.hypot(kRobotWidth / 2.0, kRobotLength / 2.0)) / 2.0;
5252
public static final double kMaxAccelOmega = 5.0;
5353

54-
public static final double kPHolonomic = 3.0; // was 3
54+
public static final double kPHolonomic = 4.0; // was 3
5555
public static final double kIHolonomic = 0.0000;
5656
public static final double kDHolonomic = 0.00; // kPHolonomic/100
5757

‎src/main/java/frc/robot/constants/PathHandlerConstants.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,4 +31,5 @@ public class PathHandlerConstants {
3131
"fetchToL"
3232
}
3333
};
34+
public static final double kWaitingTime = 2.0;
3435
}

‎src/main/java/frc/robot/subsystems/auto/AutoSwitch.java‎

Lines changed: 64 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,66 @@
11
package frc.robot.subsystems.auto;
22

3-
public class AutoSwitch {}
3+
import frc.robot.subsystems.algae.AlgaeSubsystem;
4+
import frc.robot.subsystems.battMon.BattMonSubsystem;
5+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
6+
import frc.robot.subsystems.climb.ClimbSubsystem;
7+
import frc.robot.subsystems.coral.CoralSubsystem;
8+
import frc.robot.subsystems.drive.DriveSubsystem;
9+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
10+
import frc.robot.subsystems.funnel.FunnelSubsystem;
11+
import frc.robot.subsystems.led.LEDSubsystem;
12+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
13+
import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
14+
import frc.robot.subsystems.vision.VisionSubsystem;
15+
import java.util.Set;
16+
import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
17+
import org.strykeforce.telemetry.measurable.Measure;
18+
19+
public class AutoSwitch extends MeasurableSubsystem {
20+
21+
private RobotStateSubsystem robotStateSubsystem;
22+
private AlgaeSubsystem algaeSubsystem;
23+
private BattMonSubsystem battMonSubsystem;
24+
private BiscuitSubsystem biscuitSubsystem;
25+
private ClimbSubsystem climbSubsystem;
26+
private CoralSubsystem coralSubsystem;
27+
private DriveSubsystem driveSubsystem;
28+
private ElevatorSubsystem elevatorSubsystem;
29+
private FunnelSubsystem funnelSubsystem;
30+
private LEDSubsystem ledSubsystem;
31+
private TagAlignSubsystem tagAlignSubsystem;
32+
private VisionSubsystem visionSubsystem;
33+
34+
public AutoSwitch(
35+
RobotStateSubsystem robotStateSubsystem,
36+
AlgaeSubsystem algaeSubsystem,
37+
BattMonSubsystem battMonSubsystem,
38+
BiscuitSubsystem biscuitSubsystem,
39+
ClimbSubsystem climbSubsystem,
40+
CoralSubsystem coralSubsystem,
41+
DriveSubsystem driveSubsystem,
42+
ElevatorSubsystem elevatorSubsystem,
43+
FunnelSubsystem funnelSubsystem,
44+
LEDSubsystem ledSubsystem,
45+
TagAlignSubsystem tagAlignSubsystem,
46+
VisionSubsystem visionSubsystem) {
47+
this.robotStateSubsystem = robotStateSubsystem;
48+
this.algaeSubsystem = algaeSubsystem;
49+
this.battMonSubsystem = battMonSubsystem;
50+
this.biscuitSubsystem = biscuitSubsystem;
51+
this.climbSubsystem = climbSubsystem;
52+
this.coralSubsystem = coralSubsystem;
53+
this.driveSubsystem = driveSubsystem;
54+
this.elevatorSubsystem = elevatorSubsystem;
55+
this.funnelSubsystem = funnelSubsystem;
56+
this.ledSubsystem = ledSubsystem;
57+
this.tagAlignSubsystem = tagAlignSubsystem;
58+
this.visionSubsystem = visionSubsystem;
59+
}
60+
61+
@Override
62+
public Set<Measure> getMeasures() {
63+
// TODO Auto-generated method stub
64+
throw new UnsupportedOperationException("Unimplemented method 'getMeasures'");
65+
}
66+
}

‎src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,17 @@ public boolean hasCoral() {
7676
}
7777
}
7878

79+
public boolean hasCoralAuton() {
80+
switch (curState) {
81+
case HAS_CORAL, EJECTING, CORAL_LOADING -> {
82+
return true;
83+
}
84+
default -> {
85+
return false;
86+
}
87+
}
88+
}
89+
7990
public void intake() {
8091
io.enableFwdLimitSwitch(true);
8192
// setSpeed(CoralConstants.kIntakingSpeed);

‎src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -105,6 +105,7 @@ public void move(double vXmps, double vYmps, double vOmegaRadps, boolean isField
105105

106106
// Choreo Holonomic Controller
107107
public void calculateController(SwerveSample desiredState) {
108+
Logger.recordOutput("DriveSubsystem/desiredPose", desiredState.getPose());
108109
holoContInput = desiredState;
109110
double xFF = desiredState.vx;
110111
double yFF = desiredState.vy;

‎src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java‎

Lines changed: 47 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,8 @@ public class PathHandler extends MeasurableSubsystem {
3838

3939
private PathStates curState = PathStates.DONE;
4040
private boolean isHandling = false;
41-
private Timer timer = new Timer();
41+
private Timer pathTimer = new Timer();
42+
private Timer waitingTimer = new Timer();
4243
private List<Trajectory<SwerveSample>> fetchPaths = new ArrayList<>();
4344
private List<Trajectory<SwerveSample>> placePaths = new ArrayList<>();
4445
private Trajectory<SwerveSample> currPath;
@@ -125,7 +126,7 @@ public void reassignAlliance() {
125126
} else {
126127
logger.error("path does not exist: {}", pathNames[0][i]);
127128
}
128-
temp = Choreo.loadTrajectory(pathNames[0][i]);
129+
temp = Choreo.loadTrajectory(pathNames[1][i]);
129130
if (!temp.isEmpty()) {
130131
placePaths.add(temp.get());
131132
} else {
@@ -135,14 +136,16 @@ public void reassignAlliance() {
135136
}
136137

137138
private void startPath(Trajectory<SwerveSample> path) {
139+
waitingTimer.stop();
140+
waitingTimer.reset();
138141
if (isHandling && path != null) {
139142
driveSubsystem.setAutoDebugMsg("Start " + currPathString);
140143
logger.info("start Path:" + currPathString);
141144
currPath = path;
142145
runningPath = true;
143-
timer.stop();
144-
timer.reset();
145-
timer.start();
146+
pathTimer.stop();
147+
pathTimer.reset();
148+
pathTimer.start();
146149
driveSubsystem.calculateController(
147150
mirrorToProcessor(currPath.getInitialSample(mirrorTrajectory).get()));
148151
if (curState == PathStates.PLACE) {
@@ -156,19 +159,21 @@ private void startPath(Trajectory<SwerveSample> path) {
156159
private void drivePath() {
157160
if (isHandling && runningPath && currPath != null) {
158161
driveSubsystem.calculateController(
159-
mirrorToProcessor(currPath.sampleAt(timer.get(), mirrorTrajectory).get()));
160-
if (timer.hasElapsed(currPath.getTotalTime())) {
162+
mirrorToProcessor(currPath.sampleAt(pathTimer.get(), mirrorTrajectory).get()));
163+
if (pathTimer.hasElapsed(currPath.getTotalTime())) {
161164
driveSubsystem.setAutoDebugMsg("End " + currPathString);
162165
runningPath = false;
163-
timer.stop();
164-
timer.reset();
166+
pathTimer.stop();
167+
pathTimer.reset();
165168
driveSubsystem.calculateController(
166169
mirrorToProcessor(currPath.getFinalSample(mirrorTrajectory).get()));
167170
driveSubsystem.drive(0, 0, 0);
168171
if (curState == PathStates.DRIVE_FETCH) {
169172
curState = PathStates.FETCH;
173+
waitingTimer.start();
170174
} else if (curState == PathStates.DRIVE_PLACE) {
171175
curState = PathStates.PLACE;
176+
waitingTimer.start();
172177
}
173178
} else if (shouldTransitionToServoing()) {
174179
curState = PathStates.DRIVE_PLACE_SERVO;
@@ -180,36 +185,44 @@ private void drivePath() {
180185
private void drivePathServo() {
181186
if (isHandling && runningPath && currPath != null && isServoing) {
182187
driveSubsystem.calculateControllerServo(
183-
mirrorToProcessor(currPath.sampleAt(timer.get(), mirrorTrajectory).get()),
188+
mirrorToProcessor(currPath.sampleAt(pathTimer.get(), mirrorTrajectory).get()),
184189
tagAlignSubsystem.calculateAlignY());
185-
if (timer.hasElapsed(currPath.getTotalTime())) {
190+
if (pathTimer.hasElapsed(currPath.getTotalTime())) {
186191
driveSubsystem.setAutoDebugMsg("End " + currPathString);
187192
runningPath = false;
188-
timer.stop();
189-
timer.reset();
193+
pathTimer.stop();
194+
pathTimer.reset();
190195
driveSubsystem.calculateControllerServo(
191196
mirrorToProcessor(currPath.getFinalSample(mirrorTrajectory).get()), 0.0);
192197
if (curState == PathStates.DRIVE_FETCH) {
193198
curState = PathStates.FETCH;
199+
waitingTimer.start();
194200
} else if (curState == PathStates.DRIVE_PLACE_SERVO) {
195201
isServoing = false;
196202
robotStateSubsystem.toPrepCoral();
197203
curState = PathStates.PLACE;
204+
waitingTimer.start();
198205
}
199206
}
200207
}
201208
}
202209

203210
private Trajectory<SwerveSample> nextPath() {
204211
if (nodeNames.size() > 0) {
205-
if (curState == PathStates.DRIVE_FETCH) {
206-
logger.info("drfd:" + nodeNames.get(0));
207-
logger.info("drfd:" + (nodeNames.get(0) - 'a'));
208-
currPathString = pathNames[0][nodeNames.get(0) - 'a'];
209-
return fetchPaths.get(nodeNames.get(0) - 'a');
210-
} else if (curState == PathStates.DRIVE_PLACE) {
211-
currPathString = pathNames[1][nodeNames.get(0) - 'a'];
212-
return placePaths.get(nodeNames.get(0) - 'a');
212+
int index;
213+
char nextNode = nodeNames.get(0);
214+
if (nextNode >= 97 && nextNode <= 108) {
215+
index = nextNode - 'a';
216+
if (curState == PathStates.DRIVE_FETCH) {
217+
currPathString = pathNames[0][index];
218+
return fetchPaths.get(index);
219+
} else if (curState == PathStates.DRIVE_PLACE) {
220+
currPathString = pathNames[1][index];
221+
return placePaths.get(index);
222+
}
223+
} else {
224+
logger.error("Bad node name! Check the node list for upper case and any letter past \"l\"");
225+
killPathHandler();
213226
}
214227
} else {
215228
killPathHandler();
@@ -219,9 +232,9 @@ private Trajectory<SwerveSample> nextPath() {
219232

220233
private void advanceNodes() {
221234
if (nodeNames.size() > 0) {
222-
nodeNames.remove(0);
223235
tagAlignSubsystem.setup(
224236
mirrorTrajectory ? Alliance.Red : Alliance.Blue, (nodeNames.get(0) - 'a') % 2 == 0);
237+
nodeNames.remove(0);
225238
}
226239
}
227240

@@ -230,8 +243,8 @@ public void killPathHandler() {
230243
curState = PathStates.DONE;
231244
runningPath = false;
232245
robotStateSubsystem.setIsAutoPlacing(true);
233-
timer.stop();
234-
timer.reset();
246+
pathTimer.stop();
247+
pathTimer.reset();
235248
}
236249

237250
public void killPathHandlerAfterPath() {
@@ -257,9 +270,10 @@ private boolean shouldTransitionToServoing() {
257270
FastMath.abs(pos - goal) < FastMath.toRadians(TagServoingConstants.kAngleCloseEnough);
258271
}
259272
return curState == PathStates.DRIVE_PLACE
260-
&& timer.hasElapsed(currPath.getTotalTime() - 1.0)
273+
&& pathTimer.hasElapsed(currPath.getTotalTime() - 1.0)
261274
// && TagAlignSubsystem.canSeeTag(desiredTag)
262-
&& isCloseEnough;
275+
&& isCloseEnough
276+
&& false; // TODO remove, this is for testing
263277
}
264278

265279
private SwerveSample mirrorToProcessor(SwerveSample sample) {
@@ -303,7 +317,10 @@ public void periodic() {
303317
drivePath();
304318
}
305319
case FETCH -> {
306-
if (robotStateSubsystem.hasCoral()) {
320+
if (
321+
// robotStateSubsystem.hasCoral() ||
322+
// (robotStateSubsystem.hasCoralAuton() &&
323+
waitingTimer.hasElapsed(PathHandlerConstants.kWaitingTime)) {
307324
advanceNodes();
308325
curState = PathStates.DRIVE_PLACE;
309326
}
@@ -323,7 +340,9 @@ public void periodic() {
323340
}
324341
}
325342
case PLACE -> {
326-
if (!robotStateSubsystem.hasCoral()) {
343+
if (
344+
// !robotStateSubsystem.hasCoral() &&
345+
waitingTimer.hasElapsed(PathHandlerConstants.kWaitingTime)) {
327346
curState = PathStates.DRIVE_FETCH;
328347
}
329348
}

‎src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@
2929
public class RobotStateSubsystem extends MeasurableSubsystem {
3030
private org.slf4j.Logger logger = LoggerFactory.getLogger(this.getClass());
3131

32-
private Alliance allianceColor = Alliance.Red;
32+
private Alliance allianceColor = Alliance.Blue;
3333

3434
private AlgaeSubsystem algaeSubsystem;
3535
private BattMonSubsystem battMonSubsystem;
@@ -135,6 +135,10 @@ public boolean hasCoral() {
135135
return coralSubsystem.hasCoral();
136136
}
137137

138+
public boolean hasCoralAuton() {
139+
return coralSubsystem.hasCoralAuton();
140+
}
141+
138142
public boolean hasAlgae() {
139143
return algaeSubsystem.hasAlgae();
140144
}

0 commit comments

Comments
 (0)