Skip to content

Commit 34b641a

Browse files
YaypixelYaypixel
committed
auto v1
1 parent f0ea0ba commit 34b641a

12 files changed

Lines changed: 428 additions & 309 deletions

‎src/main/deploy/pathplanner/autos/center to back depot side.auto‎

Lines changed: 45 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5,16 +5,59 @@
55
"data": {
66
"commands": [
77
{
8-
"type": "path",
8+
"type": "named",
9+
"data": {
10+
"name": "hopper deploy"
11+
}
12+
},
13+
{
14+
"type": "named",
915
"data": {
10-
"pathName": "1 Depot side center"
16+
"name": "idle"
17+
}
18+
},
19+
{
20+
"type": "parallel",
21+
"data": {
22+
"commands": [
23+
{
24+
"type": "named",
25+
"data": {
26+
"name": "intake"
27+
}
28+
},
29+
{
30+
"type": "path",
31+
"data": {
32+
"pathName": "1 Depot side center"
33+
}
34+
}
35+
]
36+
}
37+
},
38+
{
39+
"type": "named",
40+
"data": {
41+
"name": "idle"
1142
}
1243
},
1344
{
1445
"type": "path",
1546
"data": {
1647
"pathName": "2 Center back depot"
1748
}
49+
},
50+
{
51+
"type": "named",
52+
"data": {
53+
"name": "shoot long"
54+
}
55+
},
56+
{
57+
"type": "named",
58+
"data": {
59+
"name": "idle"
60+
}
1861
}
1962
]
2063
}

‎src/main/deploy/pathplanner/autos/center to back outpost side.auto‎

Lines changed: 45 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5,16 +5,59 @@
55
"data": {
66
"commands": [
77
{
8-
"type": "path",
8+
"type": "named",
9+
"data": {
10+
"name": "hopper deploy"
11+
}
12+
},
13+
{
14+
"type": "named",
915
"data": {
10-
"pathName": "1 Outpost side center"
16+
"name": "idle"
17+
}
18+
},
19+
{
20+
"type": "parallel",
21+
"data": {
22+
"commands": [
23+
{
24+
"type": "named",
25+
"data": {
26+
"name": "intake"
27+
}
28+
},
29+
{
30+
"type": "path",
31+
"data": {
32+
"pathName": "1 Outpost side center"
33+
}
34+
}
35+
]
36+
}
37+
},
38+
{
39+
"type": "named",
40+
"data": {
41+
"name": "idle"
1142
}
1243
},
1344
{
1445
"type": "path",
1546
"data": {
1647
"pathName": "2 center to trench outpost"
1748
}
49+
},
50+
{
51+
"type": "named",
52+
"data": {
53+
"name": "shoot long"
54+
}
55+
},
56+
{
57+
"type": "named",
58+
"data": {
59+
"name": "idle"
60+
}
1861
}
1962
]
2063
}

‎src/main/deploy/pathplanner/paths/1 Depot side center.path‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -16,12 +16,12 @@
1616
},
1717
{
1818
"anchor": {
19-
"x": 8.309956814236111,
20-
"y": 3.9790998263888886
19+
"x": 7.816159962751549,
20+
"y": 4.003464287441951
2121
},
2222
"prevControl": {
23-
"x": 8.221710648148148,
24-
"y": 7.543523220486112
23+
"x": 7.727913796663586,
24+
"y": 7.567887681539174
2525
},
2626
"nextControl": null,
2727
"isLocked": false,

‎src/main/deploy/pathplanner/paths/1 Outpost side center.path‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -16,12 +16,12 @@
1616
},
1717
{
1818
"anchor": {
19-
"x": 8.168662291343168,
20-
"y": 4.05034919691382
19+
"x": 7.784130647736067,
20+
"y": 4.056060371517029
2121
},
2222
"prevControl": {
23-
"x": 8.392903605395963,
24-
"y": -0.012071586277174085
23+
"x": 8.008371961788862,
24+
"y": -0.006360411673965061
2525
},
2626
"nextControl": null,
2727
"isLocked": false,

‎src/main/deploy/pathplanner/paths/2 Center back depot.path‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -3,13 +3,13 @@
33
"waypoints": [
44
{
55
"anchor": {
6-
"x": 8.277989587383358,
7-
"y": 3.9630386129471233
6+
"x": 7.657274816176471,
7+
"y": 3.9826781395123847
88
},
99
"prevControl": null,
1010
"nextControl": {
11-
"x": 9.866484861003109,
12-
"y": 8.05084212188958
11+
"x": 8.096910071594428,
12+
"y": 8.151205737229102
1313
},
1414
"isLocked": false,
1515
"linkedName": null

‎src/main/deploy/pathplanner/paths/2 center to trench outpost.path‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -3,13 +3,13 @@
33
"waypoints": [
44
{
55
"anchor": {
6-
"x": 8.281460925349922,
7-
"y": 3.940337164657855
6+
"x": 7.823070397155572,
7+
"y": 3.9245975836880813
88
},
99
"prevControl": null,
1010
"nextControl": {
11-
"x": 8.616913394245723,
12-
"y": 1.0917462091757395
11+
"x": 8.158522866051372,
12+
"y": 1.076006628205966
1313
},
1414
"isLocked": false,
1515
"linkedName": null

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

Lines changed: 44 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -7,10 +7,9 @@
77
import com.ctre.phoenix6.hardware.Pigeon2;
88
import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
99
import com.pathplanner.lib.auto.AutoBuilder;
10+
import com.pathplanner.lib.auto.NamedCommands;
1011
import com.pathplanner.lib.commands.FollowPathCommand;
11-
1212
import edu.wpi.first.epilogue.Logged;
13-
import edu.wpi.first.util.sendable.Sendable;
1413
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
1514
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1615
import edu.wpi.first.wpilibj2.command.Command;
@@ -40,21 +39,54 @@ public class RobotContainer {
4039
Commands.parallel(
4140
new KickerCommand(kicker, KickerCommand.Position.INTAKE),
4241
Commands.runEnd(
43-
() -> hopper.slowMove(HopperConfig.HOPPER_RETRACT_ROTATION),
44-
() -> hopper.stop(),
45-
hopper).withDeadline(Commands.waitSeconds(2)),
42+
() -> hopper.slowMove(HopperConfig.HOPPER_RETRACT_ROTATION),
43+
() -> hopper.stop(),
44+
hopper)
45+
.withDeadline(Commands.waitSeconds(2)),
4646
new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE))
4747
.finallyDo(
4848
interrupt ->
4949
CommandScheduler.getInstance()
5050
.schedule(
5151
Commands.runEnd(
52-
() -> hopper.move(HopperConfig.HOPPER_EXTEND_ROTATION),
53-
() -> hopper.stop(),
54-
hopper).withDeadline(Commands.waitSeconds(2))));
52+
() -> hopper.move(HopperConfig.HOPPER_EXTEND_ROTATION),
53+
() -> hopper.stop(),
54+
hopper)
55+
.withDeadline(Commands.waitSeconds(2))));
5556

5657
public RobotContainer() {
57-
autoChooser = AutoBuilder.buildAutoChooser("Test");
58+
NamedCommands.registerCommand(
59+
"idle",
60+
Commands.runOnce(() -> hopper.stop(), hopper)
61+
.alongWith(Commands.runOnce(() -> intake.stop(), intake))
62+
.alongWith(Commands.runOnce(() -> kicker.stop(), kicker)));
63+
64+
NamedCommands.registerCommand(
65+
"hopper deploy",
66+
Commands.runEnd(
67+
() -> hopper.move(HopperConfig.HOPPER_EXTEND_ROTATION), () -> hopper.stop(), hopper)
68+
.alongWith(Commands.run(() -> System.out.println("Hopper Deployed")))
69+
.withDeadline(Commands.waitSeconds(2)));
70+
71+
NamedCommands.registerCommand(
72+
"intake",
73+
new IntakeCommand(intake, IntakeCommand.Position.INTAKE)
74+
.withDeadline(Commands.waitSeconds(8)));
75+
76+
NamedCommands.registerCommand(
77+
"shoot long",
78+
Commands.parallel(
79+
new KickerCommand(kicker, KickerCommand.Position.INTAKE),
80+
Commands.runEnd(
81+
() -> hopper.slowMove(HopperConfig.HOPPER_RETRACT_ROTATION),
82+
() -> hopper.stop(),
83+
hopper)
84+
.withDeadline(Commands.waitSeconds(2)),
85+
new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE),
86+
new FlywheelCommand(flywheel, FlywheelCommand.Position.TRENCH_SHOT, drivetrain))
87+
.withDeadline(Commands.waitSeconds(8)));
88+
89+
autoChooser = AutoBuilder.buildAutoChooser("Test");
5890
SmartDashboard.putData("Auto Mode", autoChooser);
5991

6092
configureBindings();
@@ -86,9 +118,9 @@ private void configureBindings() {
86118
.toggleOnTrue(new IntakeCommand(intake, IntakeCommand.Position.INTAKE));
87119

88120
controller
89-
.povRight()
90-
.and(() -> !shootGroup.isScheduled())
91-
.toggleOnTrue(new IntakeCommand(intake, IntakeCommand.Position.OUTTAKE));
121+
.povRight()
122+
.and(() -> !shootGroup.isScheduled())
123+
.toggleOnTrue(new IntakeCommand(intake, IntakeCommand.Position.OUTTAKE));
92124
// Right Stick Down = Extend/Retract Hopper
93125
controller
94126
.rightStick()

‎src/main/java/frc/robot/commands/FlywheelCommand.java‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ public class FlywheelCommand extends Command {
1212
public static enum Position {
1313
HUB_SHOT,
1414
TOWER_SHOT,
15+
TRENCH_SHOT,
1516
CALCULATED_SHOT,
1617
DEFAULT_SHOT,
1718
OUTTAKE
@@ -44,6 +45,11 @@ public void execute() {
4445
// System.out.println("Flywheel: Tower Shot");
4546
break;
4647

48+
case TRENCH_SHOT:
49+
subsystem.shoot(FlywheelConfig.FLYWHEEL_TRENCH_SHOT_SPEED);
50+
System.out.println("Flywheel: Trench Shot");
51+
break;
52+
4753
// Spins flywheels at estimated speed given distance from hub
4854
case CALCULATED_SHOT:
4955
subsystem.shoot(

‎src/main/java/frc/robot/config/FlywheelConfig.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@ public class FlywheelConfig {
3333
public static final double FLYWHEEL_OUTTAKE_SPEED = 0;
3434
public static final double FLYWHEEL_HUB_SHOT_SPEED = 40;
3535
public static final double FLYWHEEL_TOWER_SHOT_SPEED = 0;
36+
public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 40; // to be set later w/ testing
3637
public static final double FLYWHEEL_DEFAULT_SHOT_SPEED = 40;
3738
public static final double FLYWHEEL_TOLERANCE = 5;
3839
}

0 commit comments

Comments
 (0)