Skip to content

Commit 7161d56

Browse files
CougarRoboticsProg1CougarRoboticsProg1
committed
barge auto
1 parent 56b58ee commit 7161d56

File tree

6 files changed

+208
-9
lines changed

6 files changed

+208
-9
lines changed

‎src/main/deploy/pathplanner/paths/One piece part 2 barge.path‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
{
2+
"version": "2025.0",
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 5.778073770491804,
7+
"y": 4.186834016393442
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 6.912717627679558,
12+
"y": 4.14838350636806
13+
},
14+
"isLocked": false,
15+
"linkedName": null
16+
},
17+
{
18+
"anchor": {
19+
"x": 6.449385245901639,
20+
"y": 4.186834016393442
21+
},
22+
"prevControl": {
23+
"x": 5.587703157977192,
24+
"y": 4.227767040157686
25+
},
26+
"nextControl": null,
27+
"isLocked": false,
28+
"linkedName": null
29+
}
30+
],
31+
"rotationTargets": [],
32+
"constraintZones": [],
33+
"pointTowardsZones": [],
34+
"eventMarkers": [],
35+
"globalConstraints": {
36+
"maxVelocity": 4.752,
37+
"maxAcceleration": 5.0,
38+
"maxAngularVelocity": 540.0,
39+
"maxAngularAcceleration": 720.0,
40+
"nominalVoltage": 12.0,
41+
"unlimited": false
42+
},
43+
"goalEndState": {
44+
"velocity": 0,
45+
"rotation": -178.80651057601796
46+
},
47+
"reversed": false,
48+
"folder": null,
49+
"idealStartingState": {
50+
"velocity": 0,
51+
"rotation": -178.8982938847937
52+
},
53+
"useDefaultConstraints": true
54+
}

‎src/main/deploy/pathplanner/paths/One piece part 3 barge.path‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
{
2+
"version": "2025.0",
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 5.790061475409836,
7+
"y": 4.00701844262295
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 6.907622236713421,
12+
"y": 3.768111638349025
13+
},
14+
"isLocked": false,
15+
"linkedName": null
16+
},
17+
{
18+
"anchor": {
19+
"x": 7.4084016393442615,
20+
"y": 5.133862704918033
21+
},
22+
"prevControl": {
23+
"x": 7.163310485611466,
24+
"y": 5.084564364561311
25+
},
26+
"nextControl": null,
27+
"isLocked": false,
28+
"linkedName": null
29+
}
30+
],
31+
"rotationTargets": [],
32+
"constraintZones": [],
33+
"pointTowardsZones": [],
34+
"eventMarkers": [],
35+
"globalConstraints": {
36+
"maxVelocity": 4.752,
37+
"maxAcceleration": 5.0,
38+
"maxAngularVelocity": 540.0,
39+
"maxAngularAcceleration": 720.0,
40+
"nominalVoltage": 12.0,
41+
"unlimited": false
42+
},
43+
"goalEndState": {
44+
"velocity": 0,
45+
"rotation": 0.0
46+
},
47+
"reversed": false,
48+
"folder": null,
49+
"idealStartingState": {
50+
"velocity": 0,
51+
"rotation": -179.3634064240365
52+
},
53+
"useDefaultConstraints": true
54+
}

‎src/main/deploy/pathplanner/paths/OneP Center.path‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -3,25 +3,25 @@
33
"waypoints": [
44
{
55
"anchor": {
6-
"x": 7.61219262295082,
6+
"x": 7.252561475409835,
77
"y": 4.168852459016394
88
},
99
"prevControl": null,
1010
"nextControl": {
11-
"x": 6.422023805391631,
12-
"y": 4.192444655441673
11+
"x": 6.6246686906903065,
12+
"y": 4.194390695050413
1313
},
1414
"isLocked": false,
1515
"linkedName": null
1616
},
1717
{
1818
"anchor": {
19-
"x": 6.171749999999999,
19+
"x": 6.605225409836065,
2020
"y": 4.168852459016394
2121
},
2222
"prevControl": {
23-
"x": 7.062123889342438,
24-
"y": 4.185162909488503
23+
"x": 7.066232772066507,
24+
"y": 4.158843936152193
2525
},
2626
"nextControl": null,
2727
"isLocked": false,

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

Lines changed: 50 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010

1111
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
1212

13+
import com.ctre.phoenix.motorcontrol.RemoteFeedbackDevice;
1314
import com.fasterxml.jackson.databind.util.Named;
1415
import com.pathplanner.lib.auto.AutoBuilder;
1516
import com.pathplanner.lib.auto.NamedCommands;
@@ -168,7 +169,7 @@ private Command getAlignCommand(ReefSelect select) {
168169
case drive: default: /* do nothing */ break;
169170
}
170171
}
171-
else {
172+
else if(select == ReefSelect.RIGHT){
172173
target = CougarUtil.addDistanceToPoseLeft(target,((m_coralIntake.getAlignOffset() - 0.201)) + 0.03);
173174
switch(Blackbox.reefLevel) {
174175
case L1: target = CougarUtil.addDistanceToPose(target, Units.inchesToMeters(0)); break;
@@ -200,6 +201,44 @@ private Command getAlignCommand(ReefSelect select) {
200201
});
201202
}
202203

204+
private Command getAlignCommandCenter() {
205+
final double timeout = 1.4;
206+
Command vibrationCmd = new ControllerVibrationCommand(m_driverController.getHID(), 0.28, 1);
207+
return Commands.sequence(
208+
Blackbox.setAligningCmd(true),
209+
new DeferredCommand(() -> {
210+
211+
Pose2d currentPose = m_swerve.getPose();
212+
Pose2d target = Blackbox.getNearestAlignPositionReef(currentPose);
213+
if (target == null) return Commands.none();
214+
215+
216+
target = CougarUtil.addDistanceToPoseLeft(target,((m_coralIntake.getAlignOffset() - 0.201)) + 0.13);
217+
target = CougarUtil.addDistanceToPose(target, 0.15);
218+
219+
220+
Command finalAlign = new AlignCommand(m_swerve, target);
221+
if (DriverStation.isAutonomous()) finalAlign = finalAlign.withTimeout(timeout);
222+
223+
if(CougarUtil.getDistance(target, m_swerve.getPose()) > 0.2)
224+
return Commands.sequence(
225+
AutoBuilder.pathfindToPose(target, TunerConstants.kAutoAlignConstraints),
226+
finalAlign
227+
);
228+
else
229+
return finalAlign;
230+
}, Set.of(m_swerve)),
231+
Blackbox.setAligningCmd(false)
232+
).finallyDo((interrupted) -> {
233+
if(!interrupted){
234+
vibrationCmd.schedule();
235+
}
236+
//just in case
237+
Blackbox.setAligning(false);
238+
});
239+
}
240+
241+
203242
private Command getAlignCommand(ReefSelect select, double timeout) {
204243
Command vibrationCmd = new ControllerVibrationCommand(m_driverController.getHID(), 0.28, 1);
205244
return Commands.sequence(
@@ -483,7 +522,16 @@ private void configureBindings() {
483522
new WaitUntilDebounced(() -> m_wrist.isAtSetpoint() && m_elevator.isAtSetpoint(), 0.1).withTimeout(3));
484523
NamedCommands.registerCommand("ReefAlignL", getAlignCommand(Blackbox.ReefSelect.LEFT));
485524
NamedCommands.registerCommand("ReefAlignR", getAlignCommand(Blackbox.ReefSelect.RIGHT));
525+
NamedCommands.registerCommand("ReefAlignCenter", getAlignCommandCenter());
486526
NamedCommands.registerCommand("Loading", Blackbox.robotStateCmd(Blackbox.State.loading));
527+
NamedCommands.registerCommand("Barge L3", Commands.sequence(
528+
Blackbox.robotStateCmd(State.MoveElevator),
529+
new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.L3Algae).asProxy(),
530+
new WristCommand(m_wrist, Constants.Wrist.Setpoints.Source).asProxy()
531+
));
532+
533+
NamedCommands.registerCommand("Algae Harvest",
534+
new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.release).asProxy());
487535

488536
NamedCommands.registerCommand("AutoWiggle",
489537
Commands.sequence(
@@ -509,6 +557,7 @@ private void configureBindings() {
509557
m_autoChooser.addOption("TWO PIECE PROCESSOR UNTESTED", AutoHelper.getTwoPieceProc(m_swerve));
510558
m_autoChooser.addOption("TWO PIECE PROCESSOR + ALGAE REMOVAL UNTESTED", AutoHelper.getTwoPieceProc_algaeRemoval(m_swerve));
511559
m_autoChooser.addOption("wiggle test", AutoHelper.wiggle(m_swerve));
560+
m_autoChooser.addOption("One piece algae", AutoHelper.getOnePCenterAlgae(m_swerve));
512561
//m_autoChooser.addOption("Testing Auto Align", AutoHelper.testAutoAlign(m_swerve));
513562
//m_autoChooser.addOption("Test 2 Piece", AutoHelper.getTwoPieceProcTest(m_swerve));
514563

‎src/main/java/team1403/robot/commands/auto/AutoHelper.java‎

Lines changed: 42 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import edu.wpi.first.wpilibj.DriverStation.Alliance;
99
import edu.wpi.first.wpilibj2.command.Command;
1010
import edu.wpi.first.wpilibj2.command.Commands;
11+
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
1112
import team1403.lib.util.AutoUtil;
1213
import team1403.lib.util.CougarUtil;
1314
import team1403.robot.commands.AlignCommand;
@@ -42,11 +43,51 @@ public static Command getOnePCenter(SwerveSubsystem m_swerve) {
4243
Commands.waitSeconds(0.75),
4344
NamedCommands.getCommand("CoralL4"))
4445
),
46+
Commands.waitSeconds(1),
4547
NamedCommands.getCommand("ReefAlignR"),
4648
NamedCommands.getCommand("WaitForSetpoint"),
4749
NamedCommands.getCommand("CoralScore"),
4850
Commands.waitSeconds(1),
49-
NamedCommands.getCommand("Loading")
51+
NamedCommands.getCommand("Loading"),
52+
AutoUtil.loadPathPlannerPath("One piece part 2 barge", m_swerve),
53+
NamedCommands.getCommand("Barge L3"),
54+
NamedCommands.getCommand("ReefAlignCenter"),
55+
Commands.race(
56+
NamedCommands.getCommand("Algae Harvest"),
57+
Commands.sequence(
58+
Commands.waitSeconds(3.0),
59+
AutoUtil.loadPathPlannerPath("One piece part 3 barge", m_swerve),
60+
NamedCommands.getCommand("AutoBarge")
61+
)
62+
),
63+
AutoUtil.loadPathPlannerPath("One piece part 4 barge", m_swerve)
64+
65+
);
66+
} catch (Exception e) {
67+
System.err.println("Could not load auto: " + e.getMessage());
68+
return Commands.none();
69+
}
70+
}
71+
72+
public static Command getOnePCenterAlgae(SwerveSubsystem m_swerve) {
73+
try {
74+
return Commands.sequence(
75+
Commands.parallel(
76+
AutoUtil.loadPathPlannerPath("OneP Center", m_swerve, true),
77+
Commands.sequence(
78+
Commands.waitSeconds(0.75),
79+
NamedCommands.getCommand("CoralL4"))
80+
),
81+
NamedCommands.getCommand("ReefAlignR"),
82+
NamedCommands.getCommand("WaitForSetpoint"),
83+
NamedCommands.getCommand("CoralScore"),
84+
Commands.waitSeconds(1),
85+
NamedCommands.getCommand("Loading"),
86+
AutoUtil.loadPathPlannerPath("One piece part 2 barge", m_swerve),
87+
NamedCommands.getCommand("Barge L3")
88+
// NamedCommands.getCommand("ReefAlignCenter"),
89+
// NamedCommands.getCommand("Algae Harvest"),
90+
// Commands.waitSeconds(4)
5091
);
5192
} catch (Exception e) {
5293
System.err.println("Could not load auto: " + e.getMessage());

‎src/main/java/team1403/robot/subsystems/Blackbox.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,8 @@ public enum State {
3232

3333
public enum ReefSelect {
3434
LEFT,
35-
RIGHT
35+
RIGHT,
36+
CENTER
3637
}
3738

3839
public enum ReefScoreLevel {

0 commit comments

Comments
 (0)