Skip to content

Commit ba59675

Browse files
IsaacThomanIsaacThoman
committed
start of c0100 auto
1 parent 1dcc924 commit ba59675

File tree

4 files changed

+221
-9
lines changed

4 files changed

+221
-9
lines changed

‎src/main/deploy/pathplanner/autos/C-01-00.auto

+103-2
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@
6969
{
7070
"type": "named",
7171
"data": {
72-
"name": "enableNoteAssist"
72+
"name": "enableAimAssist"
7373
}
7474
},
7575
{
@@ -153,16 +153,117 @@
153153
"waitTime": 0.4
154154
}
155155
},
156+
{
157+
"type": "named",
158+
"data": {
159+
"name": "disableAimAssist"
160+
}
161+
},
156162
{
157163
"type": "named",
158164
"data": {
159165
"name": "intakeMode"
160166
}
161167
},
168+
{
169+
"type": "parallel",
170+
"data": {
171+
"commands": [
172+
{
173+
"type": "path",
174+
"data": {
175+
"pathName": "speakerCenterToNote00"
176+
}
177+
},
178+
{
179+
"type": "sequential",
180+
"data": {
181+
"commands": [
182+
{
183+
"type": "wait",
184+
"data": {
185+
"waitTime": 0.65
186+
}
187+
},
188+
{
189+
"type": "named",
190+
"data": {
191+
"name": "enableAimAssist"
192+
}
193+
}
194+
]
195+
}
196+
}
197+
]
198+
}
199+
},
200+
{
201+
"type": "named",
202+
"data": {
203+
"name": "holdMode"
204+
}
205+
},
206+
{
207+
"type": "parallel",
208+
"data": {
209+
"commands": [
210+
{
211+
"type": "path",
212+
"data": {
213+
"pathName": "note00ToSpeakerCenter"
214+
}
215+
},
216+
{
217+
"type": "sequential",
218+
"data": {
219+
"commands": [
220+
{
221+
"type": "wait",
222+
"data": {
223+
"waitTime": 0.3
224+
}
225+
},
226+
{
227+
"type": "named",
228+
"data": {
229+
"name": "disableAimAssist"
230+
}
231+
},
232+
{
233+
"type": "named",
234+
"data": {
235+
"name": "readyToShootMode"
236+
}
237+
}
238+
]
239+
}
240+
}
241+
]
242+
}
243+
},
244+
{
245+
"type": "race",
246+
"data": {
247+
"commands": [
248+
{
249+
"type": "named",
250+
"data": {
251+
"name": "waitForArmError"
252+
}
253+
},
254+
{
255+
"type": "wait",
256+
"data": {
257+
"waitTime": 5.0
258+
}
259+
}
260+
]
261+
}
262+
},
162263
{
163264
"type": "named",
164265
"data": {
165-
"name": "disableNoteAssist"
266+
"name": "shootMode"
166267
}
167268
}
168269
]

‎src/main/deploy/pathplanner/paths/note00ToSpeakerCenter.path

+52
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
{
2+
"version": 1.0,
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 2.5300455864069624,
7+
"y": 6.525874630696631
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 2.384953366397961,
12+
"y": 6.154075816923564
13+
},
14+
"isLocked": false,
15+
"linkedName": "note00"
16+
},
17+
{
18+
"anchor": {
19+
"x": 1.4871952550922645,
20+
"y": 5.56
21+
},
22+
"prevControl": {
23+
"x": 1.9768814976226448,
24+
"y": 5.573706936887558
25+
},
26+
"nextControl": null,
27+
"isLocked": false,
28+
"linkedName": "centerSpeaker"
29+
}
30+
],
31+
"rotationTargets": [],
32+
"constraintZones": [],
33+
"eventMarkers": [],
34+
"globalConstraints": {
35+
"maxVelocity": 4.0,
36+
"maxAcceleration": 3.0,
37+
"maxAngularVelocity": 540.0,
38+
"maxAngularAcceleration": 720.0
39+
},
40+
"goalEndState": {
41+
"velocity": 0,
42+
"rotation": 180.0,
43+
"rotateFast": true
44+
},
45+
"reversed": false,
46+
"folder": "speaker",
47+
"previewStartingState": {
48+
"rotation": -128.85337434035333,
49+
"velocity": 0
50+
},
51+
"useDefaultConstraints": true
52+
}

‎src/main/deploy/pathplanner/paths/speakerCenterToNote00.path

+58
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
{
2+
"version": 1.0,
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 1.4871952550922645,
7+
"y": 5.56
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 1.858994068865331,
12+
"y": 5.573706936887558
13+
},
14+
"isLocked": false,
15+
"linkedName": "centerSpeaker"
16+
},
17+
{
18+
"anchor": {
19+
"x": 2.5300455864069624,
20+
"y": 6.525874630696631
21+
},
22+
"prevControl": {
23+
"x": 2.212656355137272,
24+
"y": 6.126871025671877
25+
},
26+
"nextControl": null,
27+
"isLocked": false,
28+
"linkedName": "note00"
29+
}
30+
],
31+
"rotationTargets": [
32+
{
33+
"waypointRelativePos": 0.4,
34+
"rotationDegrees": -132.19,
35+
"rotateFast": false
36+
}
37+
],
38+
"constraintZones": [],
39+
"eventMarkers": [],
40+
"globalConstraints": {
41+
"maxVelocity": 4.0,
42+
"maxAcceleration": 3.0,
43+
"maxAngularVelocity": 540.0,
44+
"maxAngularAcceleration": 720.0
45+
},
46+
"goalEndState": {
47+
"velocity": 0,
48+
"rotation": -132.19,
49+
"rotateFast": false
50+
},
51+
"reversed": false,
52+
"folder": "speaker",
53+
"previewStartingState": {
54+
"rotation": 180.0,
55+
"velocity": 0
56+
},
57+
"useDefaultConstraints": true
58+
}

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

+8-7
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ public enum Objective{SPEAKER,AMP,SOURCE,TRAP}
3232
public enum SelectedTrap{AMP,SOURCE,REAR}
3333

3434
public enum UseFedPoseIntention{YES,NO}
35-
boolean intakeAssistPPEnabled = false;
35+
boolean aimAssistPPEnabled = false;
3636

3737
public enum DuckMode {UNDUCK, DOWN}
3838

@@ -72,19 +72,19 @@ public Optional<Rotation2d> getRotationTargetOverride(){
7272
return Optional.of(AllianceFlipUtil.apply(Rotation2d.fromDegrees(180)));
7373

7474

75-
if(armState ==ArmState.INTAKE && intakeAssistPPEnabled)
75+
if(armState ==ArmState.INTAKE && aimAssistPPEnabled)
7676
return Optional.of(getRobotPose().getRotation().minus(Rotation2d.fromRadians(noteAlignAngleDiff)));
7777

78-
if(armState ==ArmState.SPEAKER )
78+
if(armState ==ArmState.SPEAKER && aimAssistPPEnabled)
7979
return Optional.of(getRobotPose().getRotation().minus(Rotation2d.fromRadians(MBUtils.angleDiffRad(angleToObjective(objective),robotPose.getRotation().getRadians()))));
8080

8181

8282

8383
return Optional.empty();
8484
}
8585

86-
public void setIntakeAssistPPEnabled(boolean intakeAssistPPEnabled){
87-
this.intakeAssistPPEnabled = intakeAssistPPEnabled;
86+
public void setPPAimAssistEnabled(boolean intakeAssistPPEnabled){
87+
this.aimAssistPPEnabled = intakeAssistPPEnabled;
8888
}
8989

9090

@@ -257,8 +257,8 @@ public StateControllerSub(CommandXboxController driver, CommandXboxController op
257257
NamedCommands.registerCommand("readyToShootMode",new InstantCommand(this::readyToShootPressed));
258258
NamedCommands.registerCommand("shootMode",new InstantCommand(this::shootPressed));
259259

260-
NamedCommands.registerCommand("enableNoteAssist",new InstantCommand(()->setIntakeAssistPPEnabled(true)));
261-
NamedCommands.registerCommand("disableNoteAssist",new InstantCommand(()->setIntakeAssistPPEnabled(false)));
260+
NamedCommands.registerCommand("enableAimAssist",new InstantCommand(()-> setPPAimAssistEnabled(true)));
261+
NamedCommands.registerCommand("disableAimAssist",new InstantCommand(()-> setPPAimAssistEnabled(false)));
262262

263263
NamedCommands.registerCommand("enableUseFedPoseIntention",new InstantCommand(()->setUseFedPoseIntention(UseFedPoseIntention.YES)));
264264
NamedCommands.registerCommand("disableUseFedPoseIntention",new InstantCommand(()->setUseFedPoseIntention(UseFedPoseIntention.NO)));
@@ -435,6 +435,7 @@ public void resetPressed(){
435435
efState = EFState.HOLD;
436436
climbState = ClimbState.DOWN;
437437
useFedPoseIntention = UseFedPoseIntention.NO;
438+
438439
}
439440

440441
public void intakePressed(){

0 commit comments

Comments
 (0)