Skip to content

Commit 94958aa

Browse files
Shom770Shom770
committed
wvrox code
1 parent 0f24c30 commit 94958aa

File tree

11 files changed

+492
-18
lines changed

11 files changed

+492
-18
lines changed

‎src/main/deploy/pathplanner/paths/New New New New New New Path.path

+65
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
{
2+
"version": 1.0,
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 2.9121061779449953,
7+
"y": 6.983739254587885
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 3.9121061779449935,
12+
"y": 6.483739254587885
13+
},
14+
"isLocked": false,
15+
"linkedName": null
16+
},
17+
{
18+
"anchor": {
19+
"x": 5.0,
20+
"y": 5.0
21+
},
22+
"prevControl": {
23+
"x": 4.0,
24+
"y": 6.0
25+
},
26+
"nextControl": {
27+
"x": 6.0,
28+
"y": 4.0
29+
},
30+
"isLocked": false,
31+
"linkedName": null
32+
},
33+
{
34+
"anchor": {
35+
"x": 7.0,
36+
"y": 1.0
37+
},
38+
"prevControl": {
39+
"x": 6.75,
40+
"y": 2.5
41+
},
42+
"nextControl": null,
43+
"isLocked": false,
44+
"linkedName": null
45+
}
46+
],
47+
"rotationTargets": [],
48+
"constraintZones": [],
49+
"eventMarkers": [],
50+
"globalConstraints": {
51+
"maxVelocity": 3.5,
52+
"maxAcceleration": 3.0,
53+
"maxAngularVelocity": 270.0,
54+
"maxAngularAcceleration": 600.0
55+
},
56+
"goalEndState": {
57+
"velocity": 0,
58+
"rotation": 0,
59+
"rotateFast": false
60+
},
61+
"reversed": false,
62+
"folder": null,
63+
"previewStartingState": null,
64+
"useDefaultConstraints": false
65+
}

‎src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt

+25-3
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,8 @@ package com.team4099.robot2023.auto
33
import com.team4099.robot2023.auto.mode.ExamplePathAuto
44
import com.team4099.robot2023.auto.mode.FiveNoteAutoPath
55
import com.team4099.robot2023.auto.mode.FourNoteAutoPath
6+
import com.team4099.robot2023.auto.mode.FourNoteAutoPathWithFirstCenterSide
7+
import com.team4099.robot2023.auto.mode.FourNoteAutoPathWithFirstSourceSide
68
import com.team4099.robot2023.auto.mode.FourNoteLeftCenterLine
79
import com.team4099.robot2023.auto.mode.FourNoteMiddleCenterLine
810
import com.team4099.robot2023.auto.mode.FourNoteRightCenterLine
@@ -46,7 +48,9 @@ object AutonomousSelector {
4648
// orientationChooser.addOption("Right", 270.degrees)
4749
// autoTab.add("Starting Orientation", orientationChooser)
4850

49-
autonomousModeChooser.addOption("Four Note Wing Auto", AutonomousMode.FOUR_NOTE_AUTO_PATH)
51+
autonomousModeChooser.addOption("Four Note Wing Auto (Amp Side Note First, Default)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST)
52+
autonomousModeChooser.addOption("Four Note Wing Auto (Center Wing Note First)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST)
53+
autonomousModeChooser.addOption("Four Note Wing Auto (Source Side Note First)", AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST)
5054

5155
/*
5256
@@ -169,14 +173,30 @@ object AutonomousSelector {
169173
)
170174
})
171175
.andThen(SixNoteAutoPath(drivetrain, superstructure))
172-
AutonomousMode.FOUR_NOTE_AUTO_PATH ->
176+
AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST ->
173177
return WaitCommand(waitTime.inSeconds)
174178
.andThen({
175179
val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPath.startingPose)
176180
drivetrain.tempZeroGyroYaw(flippedPose.rotation)
177181
drivetrain.resetFieldFrameEstimator(flippedPose)
178182
})
179183
.andThen(FourNoteAutoPath(drivetrain, superstructure))
184+
AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST ->
185+
return WaitCommand(waitTime.inSeconds)
186+
.andThen({
187+
val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPathWithFirstCenterSide.startingPose)
188+
drivetrain.tempZeroGyroYaw(flippedPose.rotation)
189+
drivetrain.resetFieldFrameEstimator(flippedPose)
190+
})
191+
.andThen(FourNoteAutoPathWithFirstCenterSide(drivetrain, superstructure))
192+
AutonomousMode.FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST ->
193+
return WaitCommand(waitTime.inSeconds)
194+
.andThen({
195+
val flippedPose = AllianceFlipUtil.apply(FourNoteAutoPathWithFirstSourceSide.startingPose)
196+
drivetrain.tempZeroGyroYaw(flippedPose.rotation)
197+
drivetrain.resetFieldFrameEstimator(flippedPose)
198+
})
199+
.andThen(FourNoteAutoPathWithFirstSourceSide(drivetrain, superstructure))
180200
AutonomousMode.FOUR_NOTE_RIGHT_AUTO_PATH ->
181201
return WaitCommand(waitTime.inSeconds)
182202
.andThen({
@@ -321,7 +341,9 @@ object AutonomousSelector {
321341

322342
private enum class AutonomousMode {
323343
TEST_AUTO_PATH,
324-
FOUR_NOTE_AUTO_PATH,
344+
FOUR_NOTE_AUTO_PATH_WITH_AMP_SIDE_FIRST,
345+
FOUR_NOTE_AUTO_PATH_WITH_CENTER_SIDE_FIRST,
346+
FOUR_NOTE_AUTO_PATH_WITH_SOURCE_SIDE_FIRST,
325347
FOUR_NOTE_RIGHT_AUTO_PATH,
326348
FOUR_NOTE_MIDDLE_AUTO_PATH,
327349
FOUR_NOTE_LEFT_AUTO_PATH,

‎src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt

+7-7
Original file line numberDiff line numberDiff line change
@@ -36,14 +36,14 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
3636
startingPose.rotation.inRotation2ds
3737
),
3838
FieldWaypoint(
39-
Translation2d(2.9.meters - 0.4.meters, 6.9.meters + 0.1.meters)
39+
Translation2d(2.91.meters - 0.75.meters, 6.82.meters)
4040
.translation2d,
4141
null,
4242
180.degrees.inRotation2ds,
4343
),
4444
FieldWaypoint(
45-
Translation2d(2.9.meters, 6.9.meters + 0.1.meters).translation2d,
46-
null,
45+
Translation2d(2.91.meters - 0.25.meters, 7.meters).translation2d,
46+
0.degrees.inRotation2ds,
4747
180.degrees.inRotation2ds,
4848
),
4949
FieldWaypoint(
@@ -73,13 +73,13 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
7373
180.degrees.inRotation2ds
7474
), // Subwoofer
7575
FieldWaypoint(
76-
Translation2d(2.41.meters - 0.4.meters, 4.1.meters).translation2d,
76+
Translation2d(2.41.meters - 0.4.meters, 4.4.meters).translation2d,
7777
null,
7878
180.degrees.inRotation2ds
7979
),
8080
FieldWaypoint(
81-
Translation2d(2.41.meters + 0.225.meters, 4.1.meters).translation2d,
82-
null,
81+
Translation2d(2.41.meters, 4.1.meters).translation2d,
82+
0.degrees.inRotation2ds,
8383
180.degrees.inRotation2ds
8484
),
8585
FieldWaypoint(
@@ -181,6 +181,6 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
181181
}
182182

183183
companion object {
184-
val startingPose = Pose2d(Translation2d(1.42.meters - 3.inches, 5.535.meters), 180.degrees)
184+
val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees)
185185
}
186186
}

‎src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPathWithFirstCenterSide.kt

+186
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,186 @@
1+
package com.team4099.robot2023.auto.mode
2+
3+
import com.team4099.lib.trajectory.FieldWaypoint
4+
import com.team4099.robot2023.commands.drivetrain.DrivePathCommand
5+
import com.team4099.robot2023.config.constants.FlywheelConstants
6+
import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
7+
import com.team4099.robot2023.subsystems.superstructure.Superstructure
8+
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup
9+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
10+
import edu.wpi.first.wpilibj2.command.WaitCommand
11+
import org.team4099.lib.geometry.Pose2d
12+
import org.team4099.lib.geometry.Translation2d
13+
import org.team4099.lib.units.base.inSeconds
14+
import org.team4099.lib.units.base.inches
15+
import org.team4099.lib.units.base.meters
16+
import org.team4099.lib.units.derived.degrees
17+
import org.team4099.lib.units.derived.inRotation2ds
18+
19+
class FourNoteAutoPathWithFirstCenterSide(val drivetrain: Drivetrain, val superstructure: Superstructure) :
20+
SequentialCommandGroup() {
21+
init {
22+
addRequirements(drivetrain, superstructure)
23+
24+
addCommands(
25+
superstructure.prepSpeakerLowCommand(),
26+
superstructure.scoreCommand().withTimeout(0.5),
27+
WaitCommand(0.5),
28+
ParallelCommandGroup(
29+
DrivePathCommand.createPathInFieldFrame(
30+
drivetrain,
31+
{
32+
listOf(
33+
FieldWaypoint(
34+
startingPose.translation.translation2d,
35+
null,
36+
180.degrees.inRotation2ds
37+
),
38+
FieldWaypoint(
39+
Translation2d(
40+
((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 +
41+
0.25.meters,
42+
5.55.meters
43+
)
44+
.translation2d,
45+
null,
46+
180.degrees.inRotation2ds
47+
),
48+
FieldWaypoint(
49+
Translation2d(2.34.meters + 0.3.meters + 0.25.meters, 5.535.meters)
50+
.translation2d,
51+
null,
52+
180.degrees.inRotation2ds
53+
),
54+
FieldWaypoint(
55+
Translation2d(
56+
((1.43.meters) + (2.34.meters + 0.3.meters)) / 2 +
57+
0.25.meters,
58+
5.45.meters
59+
)
60+
.translation2d,
61+
null,
62+
180.degrees.inRotation2ds
63+
),
64+
FieldWaypoint(
65+
startingPose.translation.translation2d,
66+
null,
67+
180.degrees.inRotation2ds
68+
) // Subwoofer
69+
)
70+
}
71+
)
72+
.withTimeout(3.235 + 0.5),
73+
WaitCommand(0.3).andThen(superstructure.groundIntakeCommand())
74+
),
75+
superstructure.prepSpeakerLowCommand(),
76+
superstructure
77+
.scoreCommand()
78+
.withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
79+
WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds),
80+
ParallelCommandGroup(
81+
DrivePathCommand.createPathInFieldFrame(
82+
drivetrain,
83+
{
84+
listOf(
85+
FieldWaypoint(
86+
startingPose.translation.translation2d,
87+
null,
88+
180.degrees.inRotation2ds
89+
), // Subwoofer
90+
FieldWaypoint(
91+
Translation2d(2.41.meters - 0.4.meters, 4.4.meters).translation2d,
92+
null,
93+
180.degrees.inRotation2ds
94+
),
95+
FieldWaypoint(
96+
Translation2d(2.41.meters, 4.1.meters).translation2d,
97+
0.degrees.inRotation2ds,
98+
180.degrees.inRotation2ds
99+
),
100+
FieldWaypoint(
101+
startingPose.translation.translation2d,
102+
null,
103+
180.degrees.inRotation2ds
104+
)
105+
)
106+
}
107+
)
108+
.withTimeout(3.235 + 0.5),
109+
WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
110+
),
111+
superstructure.prepSpeakerLowCommand(),
112+
superstructure
113+
.scoreCommand()
114+
.withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
115+
WaitCommand(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds),
116+
ParallelCommandGroup(
117+
DrivePathCommand.createPathInFieldFrame(
118+
drivetrain,
119+
{
120+
listOf(
121+
FieldWaypoint(
122+
startingPose.translation.translation2d,
123+
null,
124+
startingPose.rotation.inRotation2ds
125+
),
126+
FieldWaypoint(
127+
Translation2d(2.91.meters - 0.75.meters, 6.9.meters)
128+
.translation2d,
129+
null,
130+
180.degrees.inRotation2ds,
131+
),
132+
FieldWaypoint(
133+
Translation2d(2.91.meters - 0.25.meters, 7.1.meters).translation2d,
134+
0.degrees.inRotation2ds,
135+
180.degrees.inRotation2ds,
136+
),
137+
FieldWaypoint(
138+
startingPose.translation.translation2d,
139+
null,
140+
180.degrees.inRotation2ds
141+
)
142+
)
143+
}
144+
)
145+
.withTimeout(3.235 + 0.5),
146+
WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
147+
),
148+
superstructure.prepSpeakerLowCommand(),
149+
superstructure
150+
.scoreCommand()
151+
.withTimeout(FlywheelConstants.SPEAKER_SCORE_TIME.inSeconds + 0.5),
152+
ParallelCommandGroup(
153+
DrivePathCommand.createPathInFieldFrame(
154+
drivetrain,
155+
{
156+
listOf(
157+
FieldWaypoint(
158+
startingPose.translation.translation2d, null, 180.degrees.inRotation2ds
159+
),
160+
FieldWaypoint(
161+
Translation2d(4.35.meters, 4.85.meters).translation2d,
162+
null,
163+
180.degrees.inRotation2ds
164+
),
165+
FieldWaypoint(
166+
Translation2d(5.92.meters, 3.9.meters).translation2d,
167+
null,
168+
180.degrees.inRotation2ds
169+
),
170+
FieldWaypoint(
171+
Translation2d(8.29.meters, 4.09.meters).translation2d,
172+
null,
173+
180.degrees.inRotation2ds
174+
)
175+
)
176+
}
177+
),
178+
WaitCommand(1.0).andThen(superstructure.groundIntakeCommand())
179+
)
180+
)
181+
}
182+
183+
companion object {
184+
val startingPose = Pose2d(Translation2d(1.42.meters + 1.inches, 5.535.meters), 180.degrees)
185+
}
186+
}

0 commit comments

Comments
 (0)