diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote1.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote1.toml new file mode 100644 index 00000000..2c5009f3 --- /dev/null +++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote1.toml @@ -0,0 +1,16 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 0.0 #degrees + +[start_pose] + dataPoint = "MI1" + +[end_pose] + dataPoint = "M1" + +[[internal_points]] + x = 5.82 + y = 7.45 diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote2.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote2.toml new file mode 100644 index 00000000..9e11f9b1 --- /dev/null +++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote2.toml @@ -0,0 +1,16 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 0.0 #degrees + +[start_pose] + dataPoint = "MI1" + +[end_pose] + dataPoint = "M2" + +[[internal_points]] + x = 5.80 + y = 7.45 diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml index e472b1cc..b6b3886b 100644 --- a/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml +++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml @@ -17,4 +17,4 @@ target_yaw = 0.0 #degrees [[internal_points]] x = 4.71 - y = 4.105 \ No newline at end of file + y = 4.11 diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote4.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote4.toml new file mode 100644 index 00000000..5366f31d --- /dev/null +++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote4.toml @@ -0,0 +1,20 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 0.0 #degrees + +[start_pose] + dataPoint = "MI1" + +[end_pose] + dataPoint = "M4" + +[[internal_points]] + x = 4.41 + y = 4.83 + +[[internal_points]] + x = 4.71 + y = 4.05 diff --git a/src/main/deploy/paths/MiddleInitial1_MiddleNote5.toml b/src/main/deploy/paths/MiddleInitial1_MiddleNote5.toml new file mode 100644 index 00000000..51b28901 --- /dev/null +++ b/src/main/deploy/paths/MiddleInitial1_MiddleNote5.toml @@ -0,0 +1,20 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 0.0 #degrees + +[start_pose] + dataPoint = "MI1" + +[end_pose] + dataPoint = "M5" + +[[internal_points]] + x = 4.41 + y = 4.83 + +[[internal_points]] + x = 5.00 + y = 4.05 diff --git a/src/main/deploy/paths/MiddleNote1_MiddleNote2.toml b/src/main/deploy/paths/MiddleNote1_MiddleNote2.toml new file mode 100644 index 00000000..1b26b19e --- /dev/null +++ b/src/main/deploy/paths/MiddleNote1_MiddleNote2.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = -90.0 #degrees + +[start_pose] + dataPoint = "M1" + angle = -90.0 + +[end_pose] + dataPoint = "M2" + angle = -90.0 + +[[internal_points]] + x = 8.27 + y = 1.59 diff --git a/src/main/deploy/paths/MiddleNote1_MiddleNote3.toml b/src/main/deploy/paths/MiddleNote1_MiddleNote3.toml new file mode 100644 index 00000000..7ed07b88 --- /dev/null +++ b/src/main/deploy/paths/MiddleNote1_MiddleNote3.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = -90.0 #degrees + +[start_pose] + dataPoint = "M1" + angle = -90.0 + +[end_pose] + dataPoint = "M3" + angle = -90.0 + +[[internal_points]] + x = 7.60 + y = 2.43 diff --git a/src/main/deploy/paths/MiddleNote1_MiddleNote4.toml b/src/main/deploy/paths/MiddleNote1_MiddleNote4.toml new file mode 100644 index 00000000..60c52423 --- /dev/null +++ b/src/main/deploy/paths/MiddleNote1_MiddleNote4.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = -90.0 #degrees + +[start_pose] + dataPoint = "M1" + angle = -90.0 + +[end_pose] + dataPoint = "M4" + angle = -90.0 + +[[internal_points]] + x = 7.60 + y = 3.27 diff --git a/src/main/deploy/paths/MiddleNote1_MiddleNote5.toml b/src/main/deploy/paths/MiddleNote1_MiddleNote5.toml new file mode 100644 index 00000000..54e128c4 --- /dev/null +++ b/src/main/deploy/paths/MiddleNote1_MiddleNote5.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = -90.0 #degrees + +[start_pose] + dataPoint = "M1" + angle = -90.0 + +[end_pose] + dataPoint = "M4" + angle = -90.0 + +[[internal_points]] + x = 7.60 + y = 4.11 diff --git a/src/main/deploy/paths/MiddleNote2_MiddleNote1.toml b/src/main/deploy/paths/MiddleNote2_MiddleNote1.toml new file mode 100644 index 00000000..03af096a --- /dev/null +++ b/src/main/deploy/paths/MiddleNote2_MiddleNote1.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 90.0 #degrees + +[start_pose] + dataPoint = "M2" + angle = 90.0 + +[end_pose] + dataPoint = "M1" + angle = 90.0 + +[[internal_points]] + x = 8.27 + y = 1.59 diff --git a/src/main/deploy/paths/MiddleNote2_MiddleNote3.toml b/src/main/deploy/paths/MiddleNote2_MiddleNote3.toml new file mode 100644 index 00000000..d9d6e5f2 --- /dev/null +++ b/src/main/deploy/paths/MiddleNote2_MiddleNote3.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = -90.0 #degrees + +[start_pose] + dataPoint = "M2" + angle = -90.0 + +[end_pose] + dataPoint = "M3" + angle = -90.0 + +[[internal_points]] + x = 8.27 + y = 3.27 diff --git a/src/main/deploy/paths/MiddleNote2_MiddleNote4.toml b/src/main/deploy/paths/MiddleNote2_MiddleNote4.toml new file mode 100644 index 00000000..293b4b1f --- /dev/null +++ b/src/main/deploy/paths/MiddleNote2_MiddleNote4.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = -90.0 #degrees + +[start_pose] + dataPoint = "M2" + angle = -90.0 + +[end_pose] + dataPoint = "M4" + angle = -90.0 + +[[internal_points]] + x = 7.60 + y = 4.11 diff --git a/src/main/deploy/paths/MiddleNote2_MiddleNote5.toml b/src/main/deploy/paths/MiddleNote2_MiddleNote5.toml new file mode 100644 index 00000000..1d864122 --- /dev/null +++ b/src/main/deploy/paths/MiddleNote2_MiddleNote5.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = -90.0 #degrees + +[start_pose] + dataPoint = "M2" + angle = -90.0 + +[end_pose] + dataPoint = "M5" + angle = -90.0 + +[[internal_points]] + x = 7.60 + y = 4.94 diff --git a/src/main/deploy/paths/MiddleNote3_MiddleNote1.toml b/src/main/deploy/paths/MiddleNote3_MiddleNote1.toml new file mode 100644 index 00000000..ebb3894b --- /dev/null +++ b/src/main/deploy/paths/MiddleNote3_MiddleNote1.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 90.0 #degrees + +[start_pose] + dataPoint = "M3" + angle = 90.0 + +[end_pose] + dataPoint = "M1" + angle = 90.0 + +[[internal_points]] + x = 8.27 + y = 2.43 diff --git a/src/main/deploy/paths/MiddleNote3_MiddleNote2.toml b/src/main/deploy/paths/MiddleNote3_MiddleNote2.toml new file mode 100644 index 00000000..33c6d52f --- /dev/null +++ b/src/main/deploy/paths/MiddleNote3_MiddleNote2.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 90.0 #degrees + +[start_pose] + dataPoint = "M3" + angle = 90.0 + +[end_pose] + dataPoint = "M2" + angle = 90.0 + +[[internal_points]] + x = 8.27 + y = 3.27 diff --git a/src/main/deploy/paths/MiddleNote3_MiddleNote4.toml b/src/main/deploy/paths/MiddleNote3_MiddleNote4.toml new file mode 100644 index 00000000..42a4561b --- /dev/null +++ b/src/main/deploy/paths/MiddleNote3_MiddleNote4.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = -90.0 #degrees + +[start_pose] + dataPoint = "M3" + angle = -90.0 + +[end_pose] + dataPoint = "M4" + angle = -90.0 + +[[internal_points]] + x = 8.27 + y = 4.94 diff --git a/src/main/deploy/paths/MiddleNote3_MiddleNote5.toml b/src/main/deploy/paths/MiddleNote3_MiddleNote5.toml new file mode 100644 index 00000000..03f51953 --- /dev/null +++ b/src/main/deploy/paths/MiddleNote3_MiddleNote5.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = -90.0 #degrees + +[start_pose] + dataPoint = "M3" + angle = -90.0 + +[end_pose] + dataPoint = "M5" + angle = -90.0 + +[[internal_points]] + x = 8.27 + y = 5.78 diff --git a/src/main/deploy/paths/MiddleNote4_MiddleNote1.toml b/src/main/deploy/paths/MiddleNote4_MiddleNote1.toml new file mode 100644 index 00000000..8df061a1 --- /dev/null +++ b/src/main/deploy/paths/MiddleNote4_MiddleNote1.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 90.0 #degrees + +[start_pose] + dataPoint = "M4" + angle = 90.0 + +[end_pose] + dataPoint = "M1" + angle = 90.0 + +[[internal_points]] + x = 7.60 + y = 3.27 diff --git a/src/main/deploy/paths/MiddleNote4_MiddleNote2.toml b/src/main/deploy/paths/MiddleNote4_MiddleNote2.toml new file mode 100644 index 00000000..1e897eac --- /dev/null +++ b/src/main/deploy/paths/MiddleNote4_MiddleNote2.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 90.0 #degrees + +[start_pose] + dataPoint = "M4" + angle = 90.0 + +[end_pose] + dataPoint = "M2" + angle = 90.0 + +[[internal_points]] + x = 7.60 + y = 4.11 diff --git a/src/main/deploy/paths/MiddleNote4_MiddleNote3.toml b/src/main/deploy/paths/MiddleNote4_MiddleNote3.toml new file mode 100644 index 00000000..b036de74 --- /dev/null +++ b/src/main/deploy/paths/MiddleNote4_MiddleNote3.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 90.0 #degrees + +[start_pose] + dataPoint = "M4" + angle = 90.0 + +[end_pose] + dataPoint = "M3" + angle = 90.0 + +[[internal_points]] + x = 8.27 + y = 4.94 diff --git a/src/main/deploy/paths/MiddleNote4_MiddleNote5.toml b/src/main/deploy/paths/MiddleNote4_MiddleNote5.toml new file mode 100644 index 00000000..3506813f --- /dev/null +++ b/src/main/deploy/paths/MiddleNote4_MiddleNote5.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = -90.0 #degrees + +[start_pose] + dataPoint = "M4" + angle = -90.0 + +[end_pose] + dataPoint = "M5" + angle = -90.0 + +[[internal_points]] + x = 8.27 + y = 6.62 diff --git a/src/main/deploy/paths/MiddleNote5_MiddleNote1.toml b/src/main/deploy/paths/MiddleNote5_MiddleNote1.toml new file mode 100644 index 00000000..3d148507 --- /dev/null +++ b/src/main/deploy/paths/MiddleNote5_MiddleNote1.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 90.0 #degrees + +[start_pose] + dataPoint = "M5" + angle = 90.0 + +[end_pose] + dataPoint = "M1" + angle = 90.0 + +[[internal_points]] + x = 8.27 + y = 4.11 diff --git a/src/main/deploy/paths/MiddleNote5_MiddleNote2.toml b/src/main/deploy/paths/MiddleNote5_MiddleNote2.toml new file mode 100644 index 00000000..3a053103 --- /dev/null +++ b/src/main/deploy/paths/MiddleNote5_MiddleNote2.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 90.0 #degrees + +[start_pose] + dataPoint = "M5" + angle = 90.0 + +[end_pose] + dataPoint = "M2" + angle = 90.0 + +[[internal_points]] + x = 8.27 + y = 4.94 diff --git a/src/main/deploy/paths/MiddleNote5_MiddleNote3.toml b/src/main/deploy/paths/MiddleNote5_MiddleNote3.toml new file mode 100644 index 00000000..1d333ccb --- /dev/null +++ b/src/main/deploy/paths/MiddleNote5_MiddleNote3.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 90.0 #degrees + +[start_pose] + dataPoint = "M5" + angle = 90.0 + +[end_pose] + dataPoint = "M3" + angle = 90.0 + +[[internal_points]] + x = 8.27 + y = 5.78 diff --git a/src/main/deploy/paths/MiddleNote5_MiddleNote4.toml b/src/main/deploy/paths/MiddleNote5_MiddleNote4.toml new file mode 100644 index 00000000..d19ebdf6 --- /dev/null +++ b/src/main/deploy/paths/MiddleNote5_MiddleNote4.toml @@ -0,0 +1,18 @@ +max_velocity = 4.0 #m/s +max_acceleration = 1.0 #m/s +start_velocity = 0.0 #m/s +end_velocity = 0.0 #m/s +is_reversed = false +target_yaw = 90.0 #degrees + +[start_pose] + dataPoint = "M5" + angle = 90.0 + +[end_pose] + dataPoint = "M4" + angle = 90.0 + +[[internal_points]] + x = 8.27 + y = 6.62 diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java index 58c94b37..f113dc7c 100644 --- a/src/main/java/frc/robot/constants/AutonConstants.java +++ b/src/main/java/frc/robot/constants/AutonConstants.java @@ -4,6 +4,9 @@ import edu.wpi.first.math.geometry.Rotation2d; public final class AutonConstants { + // field width (x) = 16.540988 m; + // field height (y) = 8.21055 m; + public final class Setpoints { // Starting Positions public static final Pose2d MI1 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)); @@ -14,11 +17,11 @@ public final class Setpoints { public static final Pose2d W3 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)); // Middle Notes - public static final Pose2d M1 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)); - public static final Pose2d M2 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)); - public static final Pose2d M3 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)); - public static final Pose2d M4 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)); - public static final Pose2d M5 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)); + public static final Pose2d M1 = new Pose2d(8.270494, 0.752856, Rotation2d.fromDegrees(0.0)); + public static final Pose2d M2 = new Pose2d(8.270494, 2.429256, Rotation2d.fromDegrees(0.0)); + public static final Pose2d M3 = new Pose2d(8.270494, 4.105275, Rotation2d.fromDegrees(0.0)); + public static final Pose2d M4 = new Pose2d(8.270494, 5.781294, Rotation2d.fromDegrees(0.0)); + public static final Pose2d M5 = new Pose2d(8.270494, 7.457694, Rotation2d.fromDegrees(0.0)); // Shooting Positions public static final Pose2d AS1 = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index f1a73157..644511e4 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -355,47 +355,67 @@ public PathData generateTrajectory(String trajectoryName) { } } - private Pose2d parseEndPoint(TomlParseResult parseResult, String pose) { - TomlTable table = parseResult.getTable(pose); + private Pose2d parseEndPoint(TomlParseResult parseResult, String poseName) { + TomlTable table = parseResult.getTable(poseName); if (table.contains("dataPoint")) { + Pose2d pose; + switch (table.getString("dataPoint")) { // Starting Positions case "MI1": - return Setpoints.MI1; + pose = Setpoints.MI1; + break; // Wing Notes case "W1": - return Setpoints.W1; + pose = Setpoints.W1; + break; case "W2": - return Setpoints.W2; + pose = Setpoints.W2; + break; case "W3": - return Setpoints.W3; + pose = Setpoints.W3; + break; // Middle Notes case "M1": - return Setpoints.M1; + pose = Setpoints.M1; + break; case "M2": - return Setpoints.M2; + pose = Setpoints.M2; + break; case "M3": - return Setpoints.M3; + pose = Setpoints.M3; + break; case "M4": - return Setpoints.M4; + pose = Setpoints.M4; + break; case "M5": - return Setpoints.M5; + pose = Setpoints.M5; + break; // Shooting Positions case "AS1": - return Setpoints.AS1; + pose = Setpoints.AS1; + break; case "MS1": - return Setpoints.MS1; + pose = Setpoints.MS1; + break; case "NAS1": - return Setpoints.NAS1; + pose = Setpoints.NAS1; + break; default: logger.warn("Bad data point {}", table.getString("dataPoint")); return new Pose2d(); } + + if (table.contains("angle")) { + pose = new Pose2d(pose.getX(), pose.getX(), Rotation2d.fromDegrees(table.getDouble("angle"))); + } + + return pose; } else { return new Pose2d( table.getDouble("x"), @@ -404,13 +424,6 @@ private Pose2d parseEndPoint(TomlParseResult parseResult, String pose) { } } - private Pose2d parsePose2d(TomlParseResult parseResult, String pose) { - return new Pose2d( - parseResult.getTable(pose).getDouble("x"), - parseResult.getTable(pose).getDouble("y"), - Rotation2d.fromDegrees(parseResult.getTable(pose).getDouble("angle"))); - } - // Control Methods public void lockZero() { SwerveModule[] swerveModules = io.getSwerveModules();