From 230e6d171ea04a8159adc089caa0b4c6da832d17 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Fri, 23 Feb 2024 20:43:58 -0500 Subject: [PATCH] new autos --- simgui-ds.json | 5 - .../pathplanner/paths/4AndPickupNoteAuto.path | 170 ++++++++++++++++++ .../pathplanner/paths/Example Path.path | 50 +++++- .../com/team4099/robot2023/RobotContainer.kt | 4 +- .../auto/mode/FiveNoteCenterlineAutoPath.kt | 81 +++++++++ .../auto/mode/SixNoteCenterlineAutoPath.kt | 80 +++++++++ .../SixNoteCenterlineWithPickupAutoPath.kt | 95 ++++++++++ .../config/constants/DrivetrainConstants.kt | 8 +- 8 files changed, 477 insertions(+), 16 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/4AndPickupNoteAuto.path create mode 100644 src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteCenterlineAutoPath.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineAutoPath.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineWithPickupAutoPath.kt diff --git a/simgui-ds.json b/simgui-ds.json index ba7e2e64..5ea3819d 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "Keyboard 0 Settings": { - "window": { - "visible": true - } - }, "System Joysticks": { "window": { "visible": false diff --git a/src/main/deploy/pathplanner/paths/4AndPickupNoteAuto.path b/src/main/deploy/pathplanner/paths/4AndPickupNoteAuto.path new file mode 100644 index 00000000..4b8a0e53 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4AndPickupNoteAuto.path @@ -0,0 +1,170 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.741887381054855, + "y": 6.695925443427855 + }, + "prevControl": null, + "nextControl": { + "x": 1.3249004714516954, + "y": 6.550174838486252 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.511115021072635, + "y": 6.9832349513151 + }, + "prevControl": { + "x": 2.212410438221992, + "y": 6.990643205161632 + }, + "nextControl": { + "x": 2.824296168340601, + "y": 6.975467660245216 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5082724716954066, + "y": 5.488743105018038 + }, + "prevControl": { + "x": 2.125198127424631, + "y": 5.366730902594161 + }, + "nextControl": { + "x": 1.4444384407793702, + "y": 5.501367852634432 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.388440350339902, + "y": 5.488743105018038 + }, + "prevControl": { + "x": 1.6754803456796465, + "y": 5.471219489931063 + }, + "nextControl": { + "x": 2.7106112717723123, + "y": 5.496661640675712 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5082724716954066, + "y": 5.488743105018038 + }, + "prevControl": { + "x": 2.7225892863967442, + "y": 5.521583593948438 + }, + "nextControl": { + "x": 1.4220056676836053, + "y": 5.486410069694874 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.786221898253982, + "y": 7.42479630901102 + }, + "prevControl": { + "x": 7.15588177827108, + "y": 7.266533230042734 + }, + "nextControl": { + "x": 8.416562018236883, + "y": 7.583059387979306 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5082724716954066, + "y": 5.488743105018038 + }, + "prevControl": { + "x": 2.672514522173267, + "y": 5.695384322269494 + }, + "nextControl": { + "x": 1.3904431950573972, + "y": 5.467829597262204 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.786221898253982, + "y": 5.7846202012571855 + }, + "prevControl": { + "x": 6.538274967970654, + "y": 7.594698262132346 + }, + "nextControl": { + "x": 9.03416882853731, + "y": 3.9745421403820247 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.834244879861288, + "y": 4.06537564992086 + }, + "prevControl": { + "x": 6.9342068541992985, + "y": 4.043525214419497 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 600.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 60.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 67490b70..691c318d 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 1.4706390139415118, + "y": 6.328270263890066 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 6.5 + "x": 2.4706390139415104, + "y": 5.828270263890066 }, "isLocked": false, "linkedName": null @@ -55,12 +55,50 @@ "x": 5.380848911075716, "y": 2.0910493199386275 }, + "nextControl": { + "x": 1.4118781121676838, + "y": 1.1917246647355006 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.819079116601521, + "y": 6.328270263890066 + }, + "prevControl": { + "x": 4.117083609492741, + "y": 4.035665082434044 + }, + "nextControl": { + "x": 7.521074623710301, + "y": 8.620875445346087 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.2230432314597275, + "y": 4.91798685745809 + }, + "prevControl": { + "x": 6.628203460598891, + "y": 7.094390818899554 + }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0, + "rotationDegrees": 30.0, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -71,7 +109,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 173.99592794454193, + "rotation": 127.90464256381038, "rotateFast": false }, "reversed": false, diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index d94d9cee..a01d677c 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -1,7 +1,9 @@ package com.team4099.robot2023 +import com.team4099.robot2023.auto.mode.FiveNoteCenterlineAutoPath import com.team4099.robot2023.auto.mode.FourNoteAutoPath -import com.team4099.robot2023.auto.mode.TestAutoPath +import com.team4099.robot2023.auto.mode.SixNoteCenterlineAutoPath +import com.team4099.robot2023.auto.mode.SixNoteCenterlineWithPickupAutoPath import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteCenterlineAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteCenterlineAutoPath.kt new file mode 100644 index 00000000..09896384 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FiveNoteCenterlineAutoPath.kt @@ -0,0 +1,81 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class FiveNoteCenterlineAutoPath(val drivetrain: Drivetrain): SequentialCommandGroup() { + init { + addRequirements(drivetrain) + + addCommands( + ResetPoseCommand(drivetrain, Pose2d(Translation2d(1.51.meters, 5.49.meters), 180.degrees)), + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(1.51.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Middle wing note + FieldWaypoint( + Translation2d(2.51.meters, 6.98.meters).translation2d, + null, + 235.degrees.inRotation2ds + ), // Leftmost wing note + FieldWaypoint( + Translation2d(3.meters, 6.98.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Right in front of the leftmost wing note + FieldWaypoint( + Translation2d(7.79.meters, 7.42.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost center line note + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(5.82.meters, 6.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(7.79.meters, 5.78.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Second leftmost wing note + FieldWaypoint( + Translation2d(5.82.meters, 6.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + ) + }, + resetPose = true + ) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineAutoPath.kt new file mode 100644 index 00000000..035eb52a --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineAutoPath.kt @@ -0,0 +1,80 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class SixNoteCenterlineAutoPath(val drivetrain: Drivetrain): SequentialCommandGroup() { + init { + addRequirements(drivetrain) + + addCommands( + ResetPoseCommand(drivetrain, Pose2d(Translation2d(1.51.meters, 5.49.meters), 180.degrees)), + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(1.51.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(2.41.meters, 4.14.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Rightmost wing note + FieldWaypoint( + Translation2d(2.41.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Middle wing note + FieldWaypoint( + Translation2d(2.41.meters, 7.03.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost wing note + FieldWaypoint( + Translation2d(7.79.meters, 7.42.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost center line note + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(5.82.meters, 6.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(7.79.meters, 5.78.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Second leftmost wing note + FieldWaypoint( + Translation2d(5.82.meters, 6.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) // Subwoofer + ) + }, + resetPose = true + ) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineWithPickupAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineWithPickupAutoPath.kt new file mode 100644 index 00000000..f56246c4 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/SixNoteCenterlineWithPickupAutoPath.kt @@ -0,0 +1,95 @@ +package com.team4099.robot2023.auto.mode + +import com.team4099.lib.trajectory.FieldWaypoint +import com.team4099.robot2023.commands.drivetrain.DrivePathCommand +import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotation2ds + +class SixNoteCenterlineWithPickupAutoPath(val drivetrain: Drivetrain): SequentialCommandGroup() { + init { + addRequirements(drivetrain) + + addCommands( + ResetPoseCommand(drivetrain, Pose2d(Translation2d(1.51.meters, 5.49.meters), 180.degrees)), + DrivePathCommand.createPathInFieldFrame( + drivetrain, + { + listOf( + FieldWaypoint( + Translation2d(1.51.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(2.41.meters, 4.14.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Rightmost wing note + FieldWaypoint( + Translation2d(2.41.meters, 5.49.meters).translation2d, + null, + 235.degrees.inRotation2ds + ), // Middle wing note + FieldWaypoint( + Translation2d(2.41.meters, 7.03.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost wing note + FieldWaypoint( + Translation2d((2.41.meters + 7.79.meters) / 2, 7.21.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost wing note + FieldWaypoint( + Translation2d(7.79.meters, 7.21.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Leftmost center line note + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Subwoofer + FieldWaypoint( + Translation2d(5.82.meters, 6.5.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(7.79.meters, 5.78.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // Second leftmost wing note + FieldWaypoint( + Translation2d(4.83.meters, 4.07.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), // In order to avoid stage + FieldWaypoint( + Translation2d(2.39.meters, 5.49.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(4.83.meters, 4.07.meters).translation2d, + null, + 180.degrees.inRotation2ds + ), + FieldWaypoint( + Translation2d(7.77.meters, 4.03.meters).translation2d, + null, + 180.degrees.inRotation2ds + ) + ) + }, + resetPose = true + ) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 98a7366d..8e4c1f50 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -50,16 +50,16 @@ object DrivetrainConstants { .perSecond // 648 // cruise velocity and accel for steering motor - val STEERING_VEL_MAX = 270.degrees.perSecond - val STEERING_ACCEL_MAX = 600.degrees.perSecond.perSecond + val STEERING_VEL_MAX = 151.degrees.perSecond + val STEERING_ACCEL_MAX = 302.degrees.perSecond.perSecond const val GYRO_RATE_COEFFICIENT = 0.0 // TODO: Change this value val SLOW_AUTO_VEL = 2.meters.perSecond val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond - val MAX_AUTO_VEL = 0.5.meters.perSecond // 4 - val MAX_AUTO_ACCEL = 0.5.meters.perSecond.perSecond // 3 + val MAX_AUTO_VEL = 4.meters.perSecond // 4 + val MAX_AUTO_ACCEL = 3.meters.perSecond.perSecond // 3 val MAX_AUTO_BRAKE_VEL = 0.5.meters.perSecond // 4 val MAX_AUTO_BRAKE_ACCEL = 0.5.meters.perSecond.perSecond // 3