From f538076438f125d5e62f4f42008ecc1f63b56c64 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Tue, 12 Mar 2024 18:16:08 -0400 Subject: [PATCH] two and three peice from left sub works --- .../deploy/pathplanner/autos/Left Sub 3s.auto | 38 +++++++++++- .../autos/Left Subwoofer Center 2S.auto | 17 +++++- .../pathplanner/paths/L Sub to Note 1.path | 17 ++---- .../pathplanner/paths/L Sub to Note 2.path | 56 ++++-------------- .../pathplanner/paths/Left Subwoofer.path | 10 ++-- .../pathplanner/paths/Note 1 to Speaker.path | 10 ++-- .../pathplanner/paths/Note 2 to L Sub.path | 58 +++++++++++++++++++ .../java/frc/robot/Commands/autoIntaking.java | 1 + src/main/java/frc/robot/RobotContainer.java | 42 ++++++++++++-- .../frc/robot/constants/AutoConstants.java | 6 +- .../frc/robot/constants/IntakeConstants.java | 2 +- 11 files changed, 176 insertions(+), 81 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Note 2 to L Sub.path diff --git a/src/main/deploy/pathplanner/autos/Left Sub 3s.auto b/src/main/deploy/pathplanner/autos/Left Sub 3s.auto index 273e7d0..fc0729f 100644 --- a/src/main/deploy/pathplanner/autos/Left Sub 3s.auto +++ b/src/main/deploy/pathplanner/autos/Left Sub 3s.auto @@ -30,9 +30,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "L Sub to Note 1" + "commands": [ + { + "type": "path", + "data": { + "pathName": "L Sub to Note 1" + } + }, + { + "type": "named", + "data": { + "name": "Auto Intake" + } + } + ] } }, { @@ -53,10 +66,29 @@ "name": "Speaker Note Shot" } }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "L Sub to Note 2" + } + }, + { + "type": "named", + "data": { + "name": "Auto Intake" + } + } + ] + } + }, { "type": "path", "data": { - "pathName": "L Sub to Note 2" + "pathName": "Note 2 to L Sub" } }, { diff --git a/src/main/deploy/pathplanner/autos/Left Subwoofer Center 2S.auto b/src/main/deploy/pathplanner/autos/Left Subwoofer Center 2S.auto index f77b1dc..17c4525 100644 --- a/src/main/deploy/pathplanner/autos/Left Subwoofer Center 2S.auto +++ b/src/main/deploy/pathplanner/autos/Left Subwoofer Center 2S.auto @@ -30,9 +30,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "L Sub to Note 1" + "commands": [ + { + "type": "path", + "data": { + "pathName": "L Sub to Note 1" + } + }, + { + "type": "named", + "data": { + "name": "Auto Intake" + } + } + ] } }, { diff --git a/src/main/deploy/pathplanner/paths/L Sub to Note 1.path b/src/main/deploy/pathplanner/paths/L Sub to Note 1.path index 0d2d88a..34e2db4 100644 --- a/src/main/deploy/pathplanner/paths/L Sub to Note 1.path +++ b/src/main/deploy/pathplanner/paths/L Sub to Note 1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.0900457128549583, - "y": 6.560420642208806 + "x": 1.4408703461281565, + "y": 6.583808951093687 }, "prevControl": null, "nextControl": { - "x": 1.5344235816676761, - "y": 7.098351746561042 + "x": 1.8852482149408742, + "y": 7.121740055445923 }, "isLocked": false, "linkedName": null @@ -43,14 +43,7 @@ "command": { "type": "parallel", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Auto Intake" - } - } - ] + "commands": [] } } } diff --git a/src/main/deploy/pathplanner/paths/L Sub to Note 2.path b/src/main/deploy/pathplanner/paths/L Sub to Note 2.path index ef033f2..9b21b52 100644 --- a/src/main/deploy/pathplanner/paths/L Sub to Note 2.path +++ b/src/main/deploy/pathplanner/paths/L Sub to Note 2.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.0900457128549583, - "y": 6.548726487766366 + "x": 1.5578118905525558, + "y": 6.4083966344570875 }, "prevControl": null, "nextControl": { - "x": 1.4993411183403562, - "y": 6.490255715554167 + "x": 1.9671072960379536, + "y": 6.349925862244889 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.247767002656512, - "y": 5.87046553010485 + "x": 2.5284267092750707, + "y": 5.671664904583371 }, "prevControl": { - "x": 1.8852482149408738, - "y": 6.256372626705368 - }, - "nextControl": { - "x": 2.61028579037215, - "y": 5.484558433504332 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0900457128549583, - "y": 6.548726487766366 - }, - "prevControl": { - "x": 2.095742994904793, - "y": 6.805997885500045 + "x": 2.1659079215594326, + "y": 6.057572001183889 }, "nextControl": null, "isLocked": false, @@ -46,31 +30,13 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.1, + "waypointRelativePos": 0.7, "rotationDegrees": 157.35591349901955, "rotateFast": true } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0.1, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Auto Intake" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, @@ -79,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -153.43494882292208, + "rotation": 159.0754982550787, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Left Subwoofer.path b/src/main/deploy/pathplanner/paths/Left Subwoofer.path index 6191819..54c42ef 100644 --- a/src/main/deploy/pathplanner/paths/Left Subwoofer.path +++ b/src/main/deploy/pathplanner/paths/Left Subwoofer.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.148516485067158, - "y": 6.560420642208806 + "x": 1.2771521839339972, + "y": 6.268066781147808 }, "prevControl": { - "x": 1.5812001994374358, - "y": 6.864468657712245 + "x": 1.709835898304275, + "y": 6.572114796651247 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -128.39665211123375, + "rotation": -141.11550356628527, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Note 1 to Speaker.path b/src/main/deploy/pathplanner/paths/Note 1 to Speaker.path index a6227f0..873c260 100644 --- a/src/main/deploy/pathplanner/paths/Note 1 to Speaker.path +++ b/src/main/deploy/pathplanner/paths/Note 1 to Speaker.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.136822330624718, - "y": 6.478561561111727 + "x": 1.3122346472613169, + "y": 6.232984317820488 }, "prevControl": { - "x": 1.581200199437436, - "y": 6.712444649960526 + "x": 1.756612516074035, + "y": 6.466867406669287 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -145.88552705465875, + "rotation": -141.34019174590983, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Note 2 to L Sub.path b/src/main/deploy/pathplanner/paths/Note 2 to L Sub.path new file mode 100644 index 0000000..3a58631 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Note 2 to L Sub.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.1776020760018726, + "y": 5.964018765644369 + }, + "prevControl": null, + "nextControl": { + "x": 1.9671072960379539, + "y": 6.209596008935608 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2888463383764373, + "y": 6.232984317820488 + }, + "prevControl": { + "x": 1.6630592805345157, + "y": 6.373314171129768 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -164.3011806576524, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -144.293308599397, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 150.25511870305772, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Commands/autoIntaking.java b/src/main/java/frc/robot/Commands/autoIntaking.java index 11ff662..bcdd402 100644 --- a/src/main/java/frc/robot/Commands/autoIntaking.java +++ b/src/main/java/frc/robot/Commands/autoIntaking.java @@ -28,4 +28,5 @@ public class autoIntaking extends Command{ return true; }else {return false;} } + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index df9dcf1..521a56d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -101,9 +102,40 @@ public class RobotContainer { climber = new Climber(shooter::getShooterAngle); - NamedCommands.registerCommand("Charge Shooter 2 Sec", shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0).withTimeout(2.0)); - NamedCommands.registerCommand("Speaker Note Shot", Commands.parallel(shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0), indexer.shootNote(() -> 1.0)).withTimeout(2.0)); - NamedCommands.registerCommand("Auto Intake", new autoIntaking(intake, indexer::getBeamBreak).andThen(intake.intakeUpCommand())); + NamedCommands.registerCommand( + "Charge Shooter 2 Sec", + shooter.angleSpeedsSetpoints( + () -> ShooterConstants.kShooterLoadAngle, + 1.0, + 1.0 + ).withTimeout(2.0) + ); + + NamedCommands.registerCommand( + "Speaker Note Shot", + Commands.parallel( + shooter.angleSpeedsSetpoints( + () -> ShooterConstants.kShooterLoadAngle, + 1.0, + 1.0 + ), + indexer.shootNote(() -> 1.0) + ).withTimeout(2.0) + ); + + + NamedCommands.registerCommand( + "Auto Intake", + Commands.parallel( + intake.intakeControl( + () -> IntakeConstants.kDownAngle, + () -> 1.0 + ), + indexer.advanceNote() + ).withTimeout(3.0)//.until(indexer::getBeamBreak) + ); + + //NamedCommands.registerCommand("Auto Intake", new PrintCommand("Intake Note")); // An example Named Command, doesn't need to remain once we start actually adding real things // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration @@ -186,7 +218,7 @@ public class RobotContainer { ) ); - driver.leftTrigger().onFalse( + driver.leftTrigger().whileFalse( new InstantCommand( () -> { intakeDown = false; @@ -194,7 +226,7 @@ public class RobotContainer { ) ); - driver.leftTrigger().onTrue(Commands.parallel( + driver.leftTrigger().whileTrue(Commands.parallel( new InstantCommand( () -> { intakeDown = true; diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 04a59f6..1e091ae 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -13,9 +13,9 @@ public final class AutoConstants { public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - public static final double kPXController = 1.05;//5; - public static final double kPYController = 1.05;//5; - public static final double kPThetaController = 0.95;//0.5; // needs to be separate from heading control + public static final double kPXController = 5.5;//5; + public static final double kPYController = 5.5;//5; + public static final double kPThetaController = 5;//0.5; // needs to be separate from heading control public static final double kPHeadingController = 0.02; // for heading control NOT PATHING public static final double kDHeadingController = 0.0025; diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 658db17..5d6f7ce 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -16,7 +16,7 @@ public class IntakeConstants { public static final double kGFeedForward = 1;//1.11; public static final double kVFeedForward = .5;//0.73; - public static final double kStartingAngle = Math.toRadians(105.0); + public static final double kStartingAngle = Math.toRadians(95.0); public static final double kUpAngle = Math.toRadians(90.0); public static final double kDownAngle = Math.toRadians(-34.0);