Skip to content

Commit 1e0c44e

Browse files
dabeycorndabeycorn
committed
asdf
1 parent da956b2 commit 1e0c44e

File tree

3 files changed

+80
-8
lines changed

3 files changed

+80
-8
lines changed

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

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
{
2+
"name":"move",
3+
"version":1,
4+
"snapshot":{
5+
"waypoints":[
6+
{"x":7.363391399383545, "y":1.3668529987335205, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
7+
{"x":5.502589225769043, "y":1.3668529987335205, "heading":0.0, "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+
"targetDt":0.05
13+
},
14+
"params":{
15+
"waypoints":[
16+
{"x":{"exp":"7.363391399383545 m", "val":7.363391399383545}, "y":{"exp":"1.3668529987335205 m", "val":1.3668529987335205}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
17+
{"x":{"exp":"5.502589225769043 m", "val":5.502589225769043}, "y":{"exp":"1.3668529987335205 m", "val":1.3668529987335205}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
18+
"constraints":[
19+
{"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
20+
{"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
21+
{"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}],
22+
"targetDt":{
23+
"exp":"0.05 s",
24+
"val":0.05
25+
}
26+
},
27+
"trajectory":{
28+
"sampleType":"Swerve",
29+
"waypoints":[0.0,1.24536],
30+
"samples":[
31+
{"t":0.0, "x":7.36339, "y":1.36685, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.81028, "ay":0.0, "alpha":0.0, "fx":[-81.82151,-81.82151,-81.82151,-81.82151], "fy":[0.0,0.0,0.0,0.0]},
32+
{"t":0.04981, "x":7.35742, "y":1.36685, "heading":0.0, "vx":-0.23962, "vy":0.0, "omega":0.0, "ax":-4.80992, "ay":0.0, "alpha":0.0, "fx":[-81.81531,-81.81531,-81.81531,-81.81531], "fy":[0.0,0.0,0.0,0.0]},
33+
{"t":0.09963, "x":7.33952, "y":1.36685, "heading":0.0, "vx":-0.47923, "vy":0.0, "omega":0.0, "ax":-4.80948, "ay":0.0, "alpha":0.0, "fx":[-81.80787,-81.80787,-81.80787,-81.80787], "fy":[0.0,0.0,0.0,0.0]},
34+
{"t":0.14944, "x":7.30968, "y":1.36685, "heading":0.0, "vx":-0.71881, "vy":0.0, "omega":0.0, "ax":-4.80894, "ay":0.0, "alpha":0.0, "fx":[-81.79877,-81.79877,-81.79877,-81.79877], "fy":[0.0,0.0,0.0,0.0]},
35+
{"t":0.19926, "x":7.26791, "y":1.36685, "heading":0.0, "vx":-0.95836, "vy":0.0, "omega":0.0, "ax":-4.80828, "ay":0.0, "alpha":0.0, "fx":[-81.7874,-81.7874,-81.7874,-81.7874], "fy":[0.0,0.0,0.0,0.0]},
36+
{"t":0.24907, "x":7.2142, "y":1.36685, "heading":0.0, "vx":-1.19788, "vy":0.0, "omega":0.0, "ax":-4.80742, "ay":0.0, "alpha":0.0, "fx":[-81.77279,-81.77279,-81.77279,-81.77279], "fy":[0.0,0.0,0.0,0.0]},
37+
{"t":0.29889, "x":7.14856, "y":1.36685, "heading":0.0, "vx":-1.43736, "vy":0.0, "omega":0.0, "ax":-4.80627, "ay":0.0, "alpha":0.0, "fx":[-81.75331,-81.75331,-81.75331,-81.75331], "fy":[0.0,0.0,0.0,0.0]},
38+
{"t":0.3487, "x":7.071, "y":1.36685, "heading":0.0, "vx":-1.67678, "vy":0.0, "omega":0.0, "ax":-4.80467, "ay":0.0, "alpha":0.0, "fx":[-81.72604,-81.72604,-81.72604,-81.72604], "fy":[0.0,0.0,0.0,0.0]},
39+
{"t":0.39852, "x":6.98151, "y":1.36685, "heading":0.0, "vx":-1.91613, "vy":0.0, "omega":0.0, "ax":-4.80227, "ay":0.0, "alpha":0.0, "fx":[-81.68516,-81.68516,-81.68516,-81.68516], "fy":[0.0,0.0,0.0,0.0]},
40+
{"t":0.44833, "x":6.8801, "y":1.36685, "heading":0.0, "vx":-2.15535, "vy":0.0, "omega":0.0, "ax":-4.79826, "ay":0.0, "alpha":0.0, "fx":[-81.61707,-81.61707,-81.61707,-81.61707], "fy":[0.0,0.0,0.0,0.0]},
41+
{"t":0.49814, "x":6.76678, "y":1.36685, "heading":0.0, "vx":-2.39437, "vy":0.0, "omega":0.0, "ax":-4.79027, "ay":0.0, "alpha":0.0, "fx":[-81.48107,-81.48107,-81.48107,-81.48107], "fy":[0.0,0.0,0.0,0.0]},
42+
{"t":0.54796, "x":6.64156, "y":1.36685, "heading":0.0, "vx":-2.633, "vy":0.0, "omega":0.0, "ax":-4.76636, "ay":0.0, "alpha":0.0, "fx":[-81.07441,-81.07441,-81.07441,-81.07441], "fy":[0.0,0.0,0.0,0.0]},
43+
{"t":0.59777, "x":6.50448, "y":1.36685, "heading":0.0, "vx":-2.87043, "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]},
44+
{"t":0.64759, "x":6.3615, "y":1.36685, "heading":0.0, "vx":-2.87043, "vy":0.0, "omega":0.0, "ax":4.76636, "ay":0.0, "alpha":0.0, "fx":[81.07441,81.07441,81.07441,81.07441], "fy":[0.0,0.0,0.0,0.0]},
45+
{"t":0.6974, "x":6.22442, "y":1.36685, "heading":0.0, "vx":-2.633, "vy":0.0, "omega":0.0, "ax":4.79027, "ay":0.0, "alpha":0.0, "fx":[81.48107,81.48107,81.48107,81.48107], "fy":[0.0,0.0,0.0,0.0]},
46+
{"t":0.74722, "x":6.0992, "y":1.36685, "heading":0.0, "vx":-2.39437, "vy":0.0, "omega":0.0, "ax":4.79826, "ay":0.0, "alpha":0.0, "fx":[81.61707,81.61707,81.61707,81.61707], "fy":[0.0,0.0,0.0,0.0]},
47+
{"t":0.79703, "x":5.98588, "y":1.36685, "heading":0.0, "vx":-2.15535, "vy":0.0, "omega":0.0, "ax":4.80227, "ay":0.0, "alpha":0.0, "fx":[81.68516,81.68516,81.68516,81.68516], "fy":[0.0,0.0,0.0,0.0]},
48+
{"t":0.84685, "x":5.88447, "y":1.36685, "heading":0.0, "vx":-1.91613, "vy":0.0, "omega":0.0, "ax":4.80467, "ay":0.0, "alpha":0.0, "fx":[81.72604,81.72604,81.72604,81.72604], "fy":[0.0,0.0,0.0,0.0]},
49+
{"t":0.89666, "x":5.79498, "y":1.36685, "heading":0.0, "vx":-1.67678, "vy":0.0, "omega":0.0, "ax":4.80627, "ay":0.0, "alpha":0.0, "fx":[81.75331,81.75331,81.75331,81.75331], "fy":[0.0,0.0,0.0,0.0]},
50+
{"t":0.94648, "x":5.71742, "y":1.36685, "heading":0.0, "vx":-1.43736, "vy":0.0, "omega":0.0, "ax":4.80742, "ay":0.0, "alpha":0.0, "fx":[81.77279,81.77279,81.77279,81.77279], "fy":[0.0,0.0,0.0,0.0]},
51+
{"t":0.99629, "x":5.65178, "y":1.36685, "heading":0.0, "vx":-1.19788, "vy":0.0, "omega":0.0, "ax":4.80828, "ay":0.0, "alpha":0.0, "fx":[81.7874,81.7874,81.7874,81.7874], "fy":[0.0,0.0,0.0,0.0]},
52+
{"t":1.0461, "x":5.59808, "y":1.36685, "heading":0.0, "vx":-0.95836, "vy":0.0, "omega":0.0, "ax":4.80894, "ay":0.0, "alpha":0.0, "fx":[81.79877,81.79877,81.79877,81.79877], "fy":[0.0,0.0,0.0,0.0]},
53+
{"t":1.09592, "x":5.5563, "y":1.36685, "heading":0.0, "vx":-0.71881, "vy":0.0, "omega":0.0, "ax":4.80948, "ay":0.0, "alpha":0.0, "fx":[81.80787,81.80787,81.80787,81.80787], "fy":[0.0,0.0,0.0,0.0]},
54+
{"t":1.14573, "x":5.52646, "y":1.36685, "heading":0.0, "vx":-0.47923, "vy":0.0, "omega":0.0, "ax":4.80992, "ay":0.0, "alpha":0.0, "fx":[81.81531,81.81531,81.81531,81.81531], "fy":[0.0,0.0,0.0,0.0]},
55+
{"t":1.19555, "x":5.50856, "y":1.36685, "heading":0.0, "vx":-0.23962, "vy":0.0, "omega":0.0, "ax":4.81028, "ay":0.0, "alpha":0.0, "fx":[81.82151,81.82151,81.82151,81.82151], "fy":[0.0,0.0,0.0,0.0]},
56+
{"t":1.24536, "x":5.50259, "y":1.36685, "heading":0.0, "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]}],
57+
"splits":[0]
58+
},
59+
"events":[]
60+
}

‎src/main/java/wmironpatriots/Robot.java‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -11,17 +11,19 @@
1111
import static edu.wpi.first.units.Units.RotationsPerSecond;
1212
import static wmironpatriots.Constants.SWERVE_SIM_CONFIG;
1313

14+
import choreo.auto.AutoChooser;
1415
import com.ctre.phoenix6.SignalLogger;
1516
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
1617
import com.ctre.phoenix6.swerve.SwerveRequest;
1718
import edu.wpi.first.math.geometry.Pose2d;
1819
import edu.wpi.first.math.geometry.Rotation2d;
1920
import edu.wpi.first.wpilibj.DriverStation;
2021
import edu.wpi.first.wpilibj.TimedRobot;
22+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2123
import edu.wpi.first.wpilibj2.command.Command;
2224
import edu.wpi.first.wpilibj2.command.CommandScheduler;
23-
import edu.wpi.first.wpilibj2.command.Commands;
2425
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
26+
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
2527
import edu.wpi.first.wpilibj2.command.button.Trigger;
2628
import java.util.HashMap;
2729
import java.util.Map;
@@ -82,6 +84,8 @@ public class Robot extends TimedRobot implements Logged {
8284

8385
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
8486

87+
private final AutoChooser chooser;
88+
8589
public Robot() {
8690
// * MONOLOGUE SETUP
8791
DriverStation.silenceJoystickConnectionWarning(true);
@@ -283,6 +287,9 @@ public Robot() {
283287
*/
284288

285289
factory = new ChoreoFactory(drivetrain);
290+
chooser = factory.getChooser();
291+
SmartDashboard.putData("sdf", chooser);
292+
RobotModeTriggers.autonomous().whileTrue(chooser.selectedCommandScheduler());
286293
}
287294

288295
private void configureBindings() {
@@ -337,11 +344,10 @@ public void robotPeriodic() {
337344
@Override
338345
public void autonomousInit() {
339346
Command auton =
340-
factory.getMove().withDeadline(Commands.waitUntil(() -> DriverStation.isEnabled()));
341-
342-
if (auton != null) {
343-
auton.schedule();
344-
}
347+
factory
348+
.getMove(); // .withDeadline(Commands.waitUntil(() -> DriverStation.isTeleopEnabled()));
349+
// if (auton != null) {
350+
auton.schedule();
345351
}
346352

347353
@Override

‎src/main/java/wmironpatriots/commands/ChoreoFactory.java‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66

77
package wmironpatriots.commands;
88

9+
import choreo.auto.AutoChooser;
910
import choreo.auto.AutoFactory;
1011
import choreo.trajectory.SwerveSample;
1112
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
@@ -31,8 +32,7 @@ public ChoreoFactory(CommandSwerveDrivetrain drivetrain) {
3132
this.drivetrain = drivetrain;
3233
controller =
3334
new SwerveRequest.RobotCentric()
34-
.withDriveRequestType(DriveRequestType.OpenLoopVoltage)
35-
.withSteerRequestType(SteerRequestType.Position);
35+
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
3636
factory =
3737
new AutoFactory(
3838
drivetrain::getPose, drivetrain::resetPose, this::followTraj, true, drivetrain);
@@ -66,4 +66,10 @@ public AutoFactory getFactor() {
6666
public Command getMove() {
6767
return Commands.sequence(factory.resetOdometry("move"), factory.trajectoryCmd("move"));
6868
}
69+
70+
public AutoChooser getChooser() {
71+
AutoChooser chooser = new AutoChooser();
72+
chooser.addCmd("move", this::getMove);
73+
return chooser;
74+
}
6975
}

0 commit comments

Comments
 (0)