Skip to content

Commit cc2f4e9

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #80 from strykeforce/IRI-auton
IRI Auto
2 parents 923978e + e08ffbf commit cc2f4e9

File tree

9 files changed

+680
-1
lines changed

9 files changed

+680
-1
lines changed

‎src/main/deploy/choreo/2025-project.chor‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -216,6 +216,20 @@
216216
"val":0.0
217217
}
218218
},
219+
"bargeDeep":{
220+
"x":{
221+
"exp":"barge.x",
222+
"val":7.7
223+
},
224+
"y":{
225+
"exp":"7.092 m",
226+
"val":7.092
227+
},
228+
"heading":{
229+
"exp":"barge.heading",
230+
"val":0.0
231+
}
232+
},
219233
"fetch":{
220234
"x":{
221235
"exp":"1.69577419757843 m",

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

Lines changed: 399 additions & 0 deletions
Large diffs are not rendered by default.

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

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
{
2+
"name":"deepBargeToE",
3+
"version":1,
4+
"snapshot":{
5+
"waypoints":[
6+
{"x":7.7, "y":7.092, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
7+
{"x":5.422460748154253, "y":5.032141990263579, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
8+
"constraints":[
9+
{"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
10+
{"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
11+
{"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
12+
{"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":4.0}}, "enabled":true},
13+
{"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":10.0}}, "enabled":true}],
14+
"targetDt":0.05
15+
},
16+
"params":{
17+
"waypoints":[
18+
{"x":{"exp":"bargeDeep.x", "val":7.7}, "y":{"exp":"bargeDeep.y", "val":7.092}, "heading":{"exp":"bargeDeep.heading", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
19+
{"x":{"exp":"I.x", "val":5.422460748154253}, "y":{"exp":"I.y", "val":5.032141990263579}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
20+
"constraints":[
21+
{"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
22+
{"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
23+
{"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
24+
{"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"4 m / s", "val":4.0}}}, "enabled":true},
25+
{"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"10 m / s ^ 2", "val":10.0}}}, "enabled":true}],
26+
"targetDt":{
27+
"exp":"0.05 s",
28+
"val":0.05
29+
}
30+
},
31+
"trajectory":{
32+
"sampleType":"Swerve",
33+
"waypoints":[0.0,1.21908],
34+
"samples":[
35+
{"t":0.0, "x":7.7, "y":7.092, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.92358, "ay":-5.01833, "alpha":-25.51924, "fx":[-2.81779,-184.47719,-199.16664,-141.86725], "fy":[-199.14368,4.39322,0.01542,-139.87727]},
36+
{"t":0.03483, "x":7.69519, "y":7.08896, "heading":0.0, "vx":-0.27599, "vy":-0.17479, "omega":-0.88886, "ax":-7.56482, "ay":-6.43547, "alpha":-21.87993, "fx":[-2.85819,-161.16128,-199.10488,-141.28324], "fy":[-199.08925,-87.98946,-1.61385,-140.41239]},
37+
{"t":0.06966, "x":7.68099, "y":7.07896, "heading":-0.03096, "vx":-0.53947, "vy":-0.39895, "omega":-1.65096, "ax":-6.56358, "ay":-7.49306, "alpha":-21.77628, "fx":[-8.322,-86.08917,-199.01352,-144.222], "fy":[-198.84899,-164.88925,1.41569,-137.30003]},
38+
{"t":0.10449, "x":7.65822, "y":7.06052, "heading":-0.08846, "vx":-0.76809, "vy":-0.65994, "omega":-2.40944, "ax":-6.3062, "ay":-7.72156, "alpha":-22.23992, "fx":[-20.51849,-52.72119,-198.83687,-148.40896], "fy":[-197.81139,-185.67973,1.22134,-132.58884]},
39+
{"t":0.13932, "x":7.62764, "y":7.03285, "heading":-0.17239, "vx":-0.98774, "vy":-0.92888, "omega":-3.18408, "ax":-6.40238, "ay":-7.65209, "alpha":-22.53822, "fx":[-35.14395,-40.14054,-198.30655,-153.30762], "fy":[-195.30897,-188.92036,0.36255,-126.36003]},
40+
{"t":0.17415, "x":7.58935, "y":6.99586, "heading":-0.28329, "vx":-1.21074, "vy":-1.19541, "omega":-3.9691, "ax":-6.62639, "ay":-7.4675, "alpha":-5.91184, "fx":[-88.6434,-97.2752,-134.02985,-121.88646], "fy":[-146.06861,-129.19469,-99.41897,-123.23657]},
41+
{"t":0.20899, "x":7.54316, "y":6.94969, "heading":-0.42154, "vx":-1.44154, "vy":-1.45551, "omega":-4.17502, "ax":-6.78948, "ay":-7.31123, "alpha":18.46137, "fx":[-181.94481,-153.52507,-66.76909,-50.47065], "fy":[-31.49769,-112.7858,-177.0498,-166.1655]},
42+
{"t":0.24382, "x":7.48883, "y":6.89456, "heading":-0.56696, "vx":-1.67803, "vy":-1.71017, "omega":-3.53199, "ax":-7.20535, "ay":-6.88787, "alpha":19.64709, "fx":[-185.01525,-164.13648,-82.41421,-48.87312], "fy":[-16.66583,-98.8397,-172.04903,-171.71537]},
43+
{"t":0.27865, "x":7.42602, "y":6.83081, "heading":-0.68998, "vx":-1.929, "vy":-1.95008, "omega":-2.84767, "ax":-7.81554, "ay":-6.11908, "alpha":22.41507, "fx":[-190.83882,-178.64658,-99.97831,-51.66124], "fy":[17.18313,-78.44613,-167.02739,-179.7183]},
44+
{"t":0.31348, "x":7.35409, "y":6.75918, "heading":-0.78917, "vx":-2.20122, "vy":-2.16321, "omega":-2.06693, "ax":-7.97752, "ay":-4.74944, "alpha":26.80128, "fx":[-179.88677,-189.48288,-115.63813,-46.91772], "fy":[69.1749,-48.89924,-157.22409,-179.73534]},
45+
{"t":0.34831, "x":7.27258, "y":6.68095, "heading":-0.86116, "vx":-2.47908, "vy":-2.32864, "omega":-1.13342, "ax":-7.13851, "ay":-5.25482, "alpha":26.30491, "fx":[-175.59973,-180.76115,-103.42389,-16.1972], "fy":[36.89183,-58.88533,-159.05614,-169.33162]},
46+
{"t":0.38314, "x":7.1819, "y":6.59666, "heading":-0.90064, "vx":-2.72772, "vy":-2.51167, "omega":-0.2172, "ax":-1.33857, "ay":-1.17954, "alpha":5.40644, "fx":[-25.1708,-36.78325,-24.21448,-3.08446], "fy":[1.9355,-16.33803,-38.34543,-25.90128]},
47+
{"t":0.41797, "x":7.08608, "y":6.50846, "heading":-0.9082, "vx":-2.77435, "vy":-2.55276, "omega":-0.02888, "ax":-0.45103, "ay":0.49556, "alpha":-0.00698, "fx":[-8.19075,-7.25394,-7.13455,-7.49438], "fy":[8.37418,8.38353,8.12819,8.15684]},
48+
{"t":0.4528, "x":6.98917, "y":6.41984, "heading":-0.90921, "vx":-2.79006, "vy":-2.53549, "omega":-0.02913, "ax":0.06495, "ay":-0.07213, "alpha":0.00224, "fx":[1.18034,-0.13353,1.81869,1.46493], "fy":[-1.89209,-1.57227,-0.49383,-0.85132]},
49+
{"t":0.48763, "x":6.89203, "y":6.33148, "heading":-0.91022, "vx":-2.78779, "vy":-2.53801, "omega":-0.02905, "ax":-0.06315, "ay":0.07004, "alpha":-0.00126, "fx":[-1.24173,-0.79325,-0.86547,-1.31019], "fy":[1.83015,0.19711,1.11677,1.52637]},
50+
{"t":0.52246, "x":6.79489, "y":6.24313, "heading":-0.91124, "vx":-2.78999, "vy":-2.53557, "omega":-0.02909, "ax":0.01232, "ay":-0.01454, "alpha":0.00247, "fx":[0.38289,-0.54984,0.48808,0.50049], "fy":[-0.72202,-0.41186,0.23781,-0.07324]},
51+
{"t":0.55729, "x":6.69772, "y":6.1548, "heading":-0.91225, "vx":-2.78956, "vy":-2.53607, "omega":-0.02901, "ax":0.00635, "ay":-0.00605, "alpha":-0.00154, "fx":[0.05471,0.27122,0.15715,-0.05953], "fy":[0.12888,-0.24386,-0.21531,-0.07306]},
52+
{"t":0.59213, "x":6.60056, "y":6.06646, "heading":-0.91326, "vx":-2.78934, "vy":-2.53628, "omega":-0.02906, "ax":0.00856, "ay":-0.01119, "alpha":0.00412, "fx":[0.19318,-0.01167,0.22876,0.1602], "fy":[-0.23951,-0.20284,-0.13363,-0.17032]},
53+
{"t":0.62696, "x":6.50341, "y":5.97812, "heading":-0.91427, "vx":-2.78905, "vy":-2.53667, "omega":-0.02892, "ax":0.0569, "ay":-0.06334, "alpha":0.00241, "fx":[0.9351,0.86348,0.96172,1.03358], "fy":[-1.3027,-0.67042,-1.10846,-1.1421]},
54+
{"t":0.66179, "x":6.4063, "y":5.88972, "heading":-0.91528, "vx":-2.78706, "vy":-2.53888, "omega":-0.02883, "ax":-0.00487, "ay":0.00889, "alpha":-0.007, "fx":[-0.35293,0.4989,-0.1344,-0.33603], "fy":[0.52792,0.17258,-0.23204,0.12431]},
55+
{"t":0.69662, "x":6.30922, "y":5.8013, "heading":-0.91628, "vx":-2.78723, "vy":-2.53857, "omega":-0.02908, "ax":-0.04118, "ay":0.03579, "alpha":0.01969, "fx":[-0.92436,-0.90257,-0.44799,-0.47079], "fy":[0.10866,1.39175,0.31176,0.57455]},
56+
{"t":0.73145, "x":6.21212, "y":5.7129, "heading":-0.9173, "vx":-2.78867, "vy":-2.53732, "omega":-0.02839, "ax":-0.54675, "ay":0.61851, "alpha":-0.02943, "fx":[-10.26835,-7.85707,-9.06193,-9.26874], "fy":[10.95211,9.78516,9.64716,10.85665]},
57+
{"t":0.76628, "x":6.11465, "y":5.6249, "heading":-0.91828, "vx":-2.80771, "vy":-2.51578, "omega":-0.02942, "ax":-0.9813, "ay":1.29004, "alpha":-0.37269, "fx":[-18.90541,-15.1744,-13.70401,-17.64749], "fy":[20.64573,18.8577,21.88575,24.62803]},
58+
{"t":0.80111, "x":6.01626, "y":5.53805, "heading":-0.91931, "vx":-2.84189, "vy":-2.47085, "omega":-0.0424, "ax":2.76878, "ay":6.71121, "alpha":-19.75716, "fx":[40.39127,121.13249,51.83289,-28.73985], "fy":[45.13422,97.75426,162.35888,142.24352]},
59+
{"t":0.83594, "x":5.91896, "y":5.45606, "heading":-0.92079, "vx":-2.74545, "vy":-2.23709, "omega":-0.73056, "ax":5.98065, "ay":6.90801, "alpha":-25.69105, "fx":[170.95689,173.12117,81.84599,-27.14571], "fy":[19.1198,82.61132,174.28594,184.59562]},
60+
{"t":0.87077, "x":5.82696, "y":5.38233, "heading":-0.94623, "vx":-2.53714, "vy":-1.99648, "omega":-1.6254, "ax":7.34682, "ay":6.3842, "alpha":-24.18211, "fx":[188.82757,181.29619,103.92894,15.81904], "fy":[-1.38349,71.68157,165.16834,190.21962]},
61+
{"t":0.9056, "x":5.74304, "y":5.31666, "heading":-1.00285, "vx":-2.28124, "vy":-1.77411, "omega":-2.46768, "ax":7.96472, "ay":5.94874, "alpha":-21.38901, "fx":[185.28581,183.39924,118.23906,44.14802], "fy":[2.29909,59.90193,152.69946,181.75017]},
62+
{"t":0.94043, "x":5.66842, "y":5.25848, "heading":-1.0888, "vx":-2.00383, "vy":-1.56691, "omega":-3.21268, "ax":8.31145, "ay":5.49899, "alpha":-19.26284, "fx":[179.5081,183.74968,129.03601,61.89769], "fy":[6.5644,48.41591,139.98071,171.70132]},
63+
{"t":0.97526, "x":5.60366, "y":5.20724, "heading":-1.2007, "vx":-1.71433, "vy":-1.37538, "omega":-3.88362, "ax":8.53407, "ay":5.16378, "alpha":-18.67276, "fx":[177.06905,185.65669,137.51627,68.79319], "fy":[7.58833,36.75193,130.99285,168.97802]},
64+
{"t":1.0101, "x":5.54913, "y":5.16247, "heading":-1.33597, "vx":-1.41708, "vy":-1.19552, "omega":-4.53401, "ax":8.64638, "ay":4.98307, "alpha":-18.81115, "fx":[174.51507,188.17085,143.10026,70.7376], "fy":[10.01707,24.97524,126.35234,170.91673]},
65+
{"t":1.04493, "x":5.50501, "y":5.12385, "heading":-1.49389, "vx":-1.11592, "vy":-1.02195, "omega":-5.18922, "ax":8.59518, "ay":5.05184, "alpha":23.07942, "fx":[146.58574,33.77809,194.63796,198.10815], "fy":[134.04745,195.31007,-3.89776,11.38707]},
66+
{"t":1.07976, "x":5.47136, "y":5.09132, "heading":-1.67464, "vx":-0.81654, "vy":-0.84599, "omega":-4.38534, "ax":8.43236, "ay":5.29077, "alpha":21.87331, "fx":[156.18842,28.31643,179.72016,198.02839], "fy":[123.33385,196.82727,50.8605,-18.24323]},
67+
{"t":1.11459, "x":5.44803, "y":5.06506, "heading":-1.82738, "vx":-0.52284, "vy":-0.66171, "omega":-3.62348, "ax":5.92091, "ay":7.08136, "alpha":27.54934, "fx":[164.83572,44.53603,-11.4016,196.82478], "fy":[111.74537,194.01876,195.89283,-29.4857]},
68+
{"t":1.14942, "x":5.43341, "y":5.04631, "heading":-1.95359, "vx":-0.31661, "vy":-0.41506, "omega":-2.66391, "ax":4.60548, "ay":6.19964, "alpha":36.90159, "fx":[171.10899,48.34177,-105.17446,192.80842], "fy":[101.97834,193.19056,167.75984,-49.54843]},
69+
{"t":1.18425, "x":5.42518, "y":5.03561, "heading":-2.04638, "vx":-0.15619, "vy":-0.19912, "omega":-1.37859, "ax":4.48432, "ay":5.7168, "alpha":39.57963, "fx":[175.64958,53.2177,-118.32491,188.46338], "fy":[94.00814,191.95401,159.45264,-64.22969]},
70+
{"t":1.21908, "x":5.42246, "y":5.03214, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
71+
"splits":[0]
72+
},
73+
"events":[]
74+
}

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

Lines changed: 151 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,151 @@
1+
package frc.robot.commands.auton;
2+
3+
import edu.wpi.first.math.geometry.Pose2d;
4+
import edu.wpi.first.math.geometry.Rotation2d;
5+
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
6+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
7+
import edu.wpi.first.wpilibj2.command.WaitCommand;
8+
import frc.robot.commands.drive.DriveAutonCommand;
9+
import frc.robot.commands.drive.PrepOdomForAutoCommand;
10+
import frc.robot.commands.elevator.ZeroElevatorCommand;
11+
import frc.robot.commands.robotState.AutoScoreAlgaeCommand;
12+
import frc.robot.commands.robotState.ForceBargeCommand;
13+
import frc.robot.commands.robotState.ForceLowFloorAlgaeCommand;
14+
import frc.robot.commands.robotState.SetAutoPlacingCommand;
15+
import frc.robot.subsystems.algae.AlgaeSubsystem;
16+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
17+
import frc.robot.subsystems.coral.CoralSubsystem;
18+
import frc.robot.subsystems.drive.DriveSubsystem;
19+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
20+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
21+
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
22+
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
23+
import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
24+
import frc.robot.subsystems.vision.VisionSubsystem;
25+
import java.util.ArrayList;
26+
import java.util.List;
27+
28+
public class IRIAutonCommand extends SequentialCommandGroup implements AutoCommandInterface {
29+
30+
private DriveSubsystem driveSubsystem;
31+
private CoralSubsystem coralSubsystem;
32+
private RobotStateSubsystem robotStateSubsystem;
33+
private VisionSubsystem visionSubsystem;
34+
35+
private ArrayList<AutoCommandInterface> pathCommands = new ArrayList<>();
36+
37+
public IRIAutonCommand(
38+
DriveSubsystem driveSubsystem,
39+
RobotStateSubsystem robotStateSubsystem,
40+
AlgaeSubsystem algaeSubsystem,
41+
BiscuitSubsystem biscuitSubsystem,
42+
CoralSubsystem coralSubsystem,
43+
ElevatorSubsystem elevatorSubsystem,
44+
TagAlignSubsystem tagAlignSubsystem,
45+
VisionSubsystem visionSubsystem,
46+
List<String> grabPaths,
47+
List<Double> grabYOffsets,
48+
List<Double> delays,
49+
List<String> bargePaths,
50+
List<RobotStateSubsystem.ScoringLevel> algaeLevels,
51+
Pose2d startPose) {
52+
addRequirements(
53+
driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
54+
this.driveSubsystem = driveSubsystem;
55+
this.coralSubsystem = coralSubsystem;
56+
this.robotStateSubsystem = robotStateSubsystem;
57+
this.visionSubsystem = visionSubsystem;
58+
59+
// BooleanSupplier awayCondition =
60+
// () -> DriverStation.getMatchTime() < AutonConstants.kBargeScoreMinTime;
61+
62+
addCommands(
63+
new ParallelCommandGroup(
64+
new PrepOdomForAutoCommand(
65+
robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(90.0), startPose),
66+
new ZeroElevatorCommand(elevatorSubsystem)),
67+
new ForceLowFloorAlgaeCommand(
68+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem),
69+
new SetAutoPlacingCommand(robotStateSubsystem, true),
70+
new ForceBargeCommand(
71+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem),
72+
new SetAutoPlacingCommand(robotStateSubsystem, false));
73+
74+
for (int i = 0; i < grabPaths.size(); i++) {
75+
boolean last = i == grabPaths.size() - 1;
76+
var algaeDrive =
77+
new DriveAlgaeAutonServoCommand(
78+
driveSubsystem,
79+
tagAlignSubsystem,
80+
elevatorSubsystem,
81+
biscuitSubsystem,
82+
robotStateSubsystem,
83+
visionSubsystem,
84+
grabPaths.get(i),
85+
i == 0,
86+
true,
87+
i == 0,
88+
grabYOffsets.get(i),
89+
algaeLevels.get(i));
90+
var bargeDrive =
91+
last
92+
? new DriveAutonCommand(driveSubsystem, bargePaths.get(i), true, false, false)
93+
: new DriveBargeAutonCommand(
94+
driveSubsystem,
95+
tagAlignSubsystem,
96+
elevatorSubsystem,
97+
biscuitSubsystem,
98+
robotStateSubsystem,
99+
visionSubsystem,
100+
bargePaths.get(i),
101+
true,
102+
false);
103+
104+
pathCommands.add(algaeDrive);
105+
pathCommands.add(bargeDrive);
106+
107+
addCommands(algaeDrive, new WaitCommand(delays.get(i)), bargeDrive);
108+
109+
if (!last) {
110+
addCommands(
111+
new AutoScoreAlgaeCommand(
112+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
113+
}
114+
}
115+
/*
116+
addCommands(
117+
new ConditionalCommand(
118+
new DriveAutonCommand(driveSubsystem, "bargeLeave", true, false, false),
119+
new SequentialCommandGroup(
120+
algaeDrive,
121+
new WaitCommand(delays.get(i))),
122+
awayCondition),
123+
new ConditionalCommand(
124+
new DriveAutonCommand(driveSubsystem, "bargeLeave", true, false, false),
125+
new SequentialCommandGroup(
126+
bargeDrive,
127+
new AutoScoreAlgaeCommand(
128+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
129+
.until(awayCondition),
130+
awayCondition));
131+
}
132+
*/
133+
134+
// addCommands(new DriveAutonCommand(driveSubsystem, "bargeLeave", true, false, false));
135+
}
136+
137+
@Override
138+
public void reassignAlliance() {
139+
driveSubsystem.teleResetGyro();
140+
coralSubsystem.setAutoPreload();
141+
robotStateSubsystem.setIsAutoPlacing(true);
142+
robotStateSubsystem.setScoringLevel(ScoringLevel.L4);
143+
robotStateSubsystem.setGetAlgaeOnCycle(true);
144+
robotStateSubsystem.setScoreSide(ScoreSide.LEFT);
145+
visionSubsystem.setVisionUpdating(true);
146+
147+
for (var command : pathCommands) {
148+
command.reassignAlliance();
149+
}
150+
}
151+
}

‎src/main/java/frc/robot/commands/robotState/SetAutoPlacingCommand.java‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
5+
6+
public class SetAutoPlacingCommand extends InstantCommand {
7+
private RobotStateSubsystem robotStateSubsystem;
8+
private boolean isAutoPlacing;
9+
10+
public SetAutoPlacingCommand(RobotStateSubsystem robotState, boolean isAutoPlacing) {
11+
this.robotStateSubsystem = robotState;
12+
this.isAutoPlacing = isAutoPlacing;
13+
}
14+
15+
@Override
16+
public void initialize() {
17+
robotStateSubsystem.setIsAutoPlacing(isAutoPlacing);
18+
}
19+
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ public class AlgaeConstants {
2929
public static final double kHoldSpeed = 1;
3030
public static final double kCoralHoldSpeed = -0.05;
3131
public static final double kBargeScoreSpeed = -1;
32-
public static final double kProcessorScoreSpeed = -1;
32+
public static final double kProcessorScoreSpeed = -0.5; // was -1.0
3333
public static final double kCoralScoreSpeed = 0.3; // 0.5; was 0.4
3434
public static final double kIntakingSpeed = 1; // 0.75
3535
public static final double kCoralIntakingSpeed = -0.3; // -0.75;

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,4 +30,6 @@ public class AutonConstants {
3030

3131
public static final Pose2d kMiddleBargeStart =
3232
new Pose2d(7.1, 3.7209, Rotation2d.fromDegrees(180));
33+
34+
public static final Pose2d kDeepBarge = new Pose2d(7.7, 7.092, Rotation2d.fromDegrees(90));
3335
}

0 commit comments

Comments
 (0)