From 68d9da6e3dd2ec3a9fafe498f96c0f64dd213f21 Mon Sep 17 00:00:00 2001 From: MyaTaheri <91988696+MyaTaheri@users.noreply.github.com> Date: Fri, 1 Mar 2024 16:31:57 -0500 Subject: [PATCH 1/6] more auto basework --- .../pathplanner/autos/simple back test.auto | 19 ++++ .../subsystems/SmartDashboardUpdater.java | 88 +++++++++++++++++++ 2 files changed, 107 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/simple back test.auto create mode 100644 src/main/java/frc/robot/subsystems/SmartDashboardUpdater.java diff --git a/src/main/deploy/pathplanner/autos/simple back test.auto b/src/main/deploy/pathplanner/autos/simple back test.auto new file mode 100644 index 0000000..c019127 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/simple back test.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Simple Back" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SmartDashboardUpdater.java b/src/main/java/frc/robot/subsystems/SmartDashboardUpdater.java new file mode 100644 index 0000000..0d9a910 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SmartDashboardUpdater.java @@ -0,0 +1,88 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SmartDashboardUpdater +{ + private final SendableChooser autoChooser = new SendableChooser<>(); + + public static final String kDefaultAuto = "Default Path"; + public static final String kAuto1 = "Red Path"; + public static final String kAuto2 = "Orange Path"; + public static final String kAuto3 = "Yellow Path"; + public static final String kAuto4 = "Green Path"; + public static final String kAuto5 = "Blue Path"; + public static final String kAuto6 = "Purple Path"; + public static final String kAuto7 = "Teal Path"; + + private final SendableChooser testChooser = new SendableChooser<>(); + + public static final String kDefaultTest = "Test Default"; + public static final String kTest1 = "Test Index"; + public static final String kTest2 = "Test Shooter"; + public static final String kTest3 = "Test Drivetrain"; + public static final String kTest4 = "Test Intake"; + public static final String kTest5 = "Test Climb"; + + /** + * Constructor + */ + public SmartDashboardUpdater () { + + } + + /** + * Populates the smartdashboard + */ + public void setupSmartDashboard() { + setAutonomousOptions(); + setTestOptions(); + } + + /** + * Sets up the dropdown menu for auto paths + */ + private void setAutonomousOptions() { + autoChooser.setDefaultOption("Default Path", kDefaultAuto); + autoChooser.addOption ("Red Path", kAuto1); + autoChooser.addOption ("Orange Path", kAuto2); + autoChooser.addOption ("Yellow Path", kAuto3); + autoChooser.addOption ("Green Path", kAuto4); + autoChooser.addOption ("Blue Path", kAuto5); + autoChooser.addOption ("Purple Path", kAuto6); + autoChooser.addOption ("Teal Path", kAuto7); + + SmartDashboard.putData("Autonomuos Choices", autoChooser); + } + + /** + * Returns the option selected on the smartdashboard + * @return String of the auto path selected + */ + public String getAutoSelected() { + return autoChooser.getSelected(); + } + + /** + * Sets up the dropdown menu for which subsystem to test + */ + private void setTestOptions() { + testChooser.setDefaultOption("Test Default", kDefaultTest); + testChooser.addOption ("Test Index", kTest1); + testChooser.addOption ("Test Outtake", kTest2); + testChooser.addOption ("Test DriveTrain", kTest3); + testChooser.addOption ("Test Intake", kTest4); + testChooser.addOption ("Test Climb", kTest5); + + SmartDashboard.putData("Test Choices", testChooser); + } + + /** + * Returns the option selected on the smartdashboard + * @return String of the test function selected + */ + public String getTestSelected() { + return testChooser.getSelected(); + } +} From 29a3120e39d1ad5ed7baea0efd33d6e8e66eb0a4 Mon Sep 17 00:00:00 2001 From: BenBerol Date: Thu, 21 Mar 2024 10:17:55 -0400 Subject: [PATCH 2/6] Revert "more auto basework" This reverts commit 68d9da6e3dd2ec3a9fafe498f96c0f64dd213f21. --- .../pathplanner/autos/simple back test.auto | 19 ---- .../subsystems/SmartDashboardUpdater.java | 88 ------------------- 2 files changed, 107 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/simple back test.auto delete mode 100644 src/main/java/frc/robot/subsystems/SmartDashboardUpdater.java diff --git a/src/main/deploy/pathplanner/autos/simple back test.auto b/src/main/deploy/pathplanner/autos/simple back test.auto deleted file mode 100644 index c019127..0000000 --- a/src/main/deploy/pathplanner/autos/simple back test.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Simple Back" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SmartDashboardUpdater.java b/src/main/java/frc/robot/subsystems/SmartDashboardUpdater.java deleted file mode 100644 index 0d9a910..0000000 --- a/src/main/java/frc/robot/subsystems/SmartDashboardUpdater.java +++ /dev/null @@ -1,88 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class SmartDashboardUpdater -{ - private final SendableChooser autoChooser = new SendableChooser<>(); - - public static final String kDefaultAuto = "Default Path"; - public static final String kAuto1 = "Red Path"; - public static final String kAuto2 = "Orange Path"; - public static final String kAuto3 = "Yellow Path"; - public static final String kAuto4 = "Green Path"; - public static final String kAuto5 = "Blue Path"; - public static final String kAuto6 = "Purple Path"; - public static final String kAuto7 = "Teal Path"; - - private final SendableChooser testChooser = new SendableChooser<>(); - - public static final String kDefaultTest = "Test Default"; - public static final String kTest1 = "Test Index"; - public static final String kTest2 = "Test Shooter"; - public static final String kTest3 = "Test Drivetrain"; - public static final String kTest4 = "Test Intake"; - public static final String kTest5 = "Test Climb"; - - /** - * Constructor - */ - public SmartDashboardUpdater () { - - } - - /** - * Populates the smartdashboard - */ - public void setupSmartDashboard() { - setAutonomousOptions(); - setTestOptions(); - } - - /** - * Sets up the dropdown menu for auto paths - */ - private void setAutonomousOptions() { - autoChooser.setDefaultOption("Default Path", kDefaultAuto); - autoChooser.addOption ("Red Path", kAuto1); - autoChooser.addOption ("Orange Path", kAuto2); - autoChooser.addOption ("Yellow Path", kAuto3); - autoChooser.addOption ("Green Path", kAuto4); - autoChooser.addOption ("Blue Path", kAuto5); - autoChooser.addOption ("Purple Path", kAuto6); - autoChooser.addOption ("Teal Path", kAuto7); - - SmartDashboard.putData("Autonomuos Choices", autoChooser); - } - - /** - * Returns the option selected on the smartdashboard - * @return String of the auto path selected - */ - public String getAutoSelected() { - return autoChooser.getSelected(); - } - - /** - * Sets up the dropdown menu for which subsystem to test - */ - private void setTestOptions() { - testChooser.setDefaultOption("Test Default", kDefaultTest); - testChooser.addOption ("Test Index", kTest1); - testChooser.addOption ("Test Outtake", kTest2); - testChooser.addOption ("Test DriveTrain", kTest3); - testChooser.addOption ("Test Intake", kTest4); - testChooser.addOption ("Test Climb", kTest5); - - SmartDashboard.putData("Test Choices", testChooser); - } - - /** - * Returns the option selected on the smartdashboard - * @return String of the test function selected - */ - public String getTestSelected() { - return testChooser.getSelected(); - } -} From 6b39d3a9eec945747a915b306803d52813613472 Mon Sep 17 00:00:00 2001 From: BenBerol Date: Thu, 21 Mar 2024 10:19:23 -0400 Subject: [PATCH 3/6] Auto work --- .../deploy/pathplanner/paths/ChargeCopy.path | 52 +++++++++++++++++++ .../deploy/pathplanner/paths/New Charge.path | 52 +++++++++++++++++++ .../frc/robot/commands/IntakeNoteCommand.java | 29 +++++++++++ .../frc/robot/commands/ShootOnceCommand.java | 27 ++++++++++ .../frc/robot/commands/ShootTwiceCommand.java | 39 ++++++++++++++ 5 files changed, 199 insertions(+) create mode 100644 src/main/deploy/pathplanner/paths/ChargeCopy.path create mode 100644 src/main/deploy/pathplanner/paths/New Charge.path create mode 100644 src/main/java/frc/robot/commands/IntakeNoteCommand.java create mode 100644 src/main/java/frc/robot/commands/ShootOnceCommand.java create mode 100644 src/main/java/frc/robot/commands/ShootTwiceCommand.java diff --git a/src/main/deploy/pathplanner/paths/ChargeCopy.path b/src/main/deploy/pathplanner/paths/ChargeCopy.path new file mode 100644 index 0000000..2c60a29 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ChargeCopy.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.406016284254791, + "y": 5.547688595377191 + }, + "prevControl": null, + "nextControl": { + "x": 2.4060162842547927, + "y": 5.547688595377191 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.296907191036604, + "y": 5.547688595377191 + }, + "prevControl": { + "x": 3.2969071910366043, + "y": 5.547688595377191 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 90.0, + "maxAngularAcceleration": 90.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Charge.path b/src/main/deploy/pathplanner/paths/New Charge.path new file mode 100644 index 0000000..2f85b10 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Charge.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4538800929453712, + "y": 5.58391306597428 + }, + "prevControl": null, + "nextControl": { + "x": 2.4538800929453757, + "y": 5.58391306597428 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.081104361282797, + "y": 5.58391306597428 + }, + "prevControl": { + "x": 3.0811043612827973, + "y": 5.58391306597428 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 90.0, + "maxAngularAcceleration": 90.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/IntakeNoteCommand.java b/src/main/java/frc/robot/commands/IntakeNoteCommand.java new file mode 100644 index 0000000..8d4d4e3 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeNoteCommand.java @@ -0,0 +1,29 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.FeedSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class IntakeNoteCommand extends SequentialCommandGroup{ + private IntakeSubsystem intake; + private FeedSubsystem feeder; + + public IntakeNoteCommand(IntakeSubsystem intake, FeedSubsystem feeder) { + + this.intake = intake; + this.feeder = feeder; + + addRequirements(this.intake); + + addCommands( + new RunCommand(()-> { + intake.intake(0.5); + feeder.feed(0.25); + System.out.println("intake"); + }, intake, feeder).withTimeout(4) + ); + } +} diff --git a/src/main/java/frc/robot/commands/ShootOnceCommand.java b/src/main/java/frc/robot/commands/ShootOnceCommand.java new file mode 100644 index 0000000..a27896d --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootOnceCommand.java @@ -0,0 +1,27 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.FeedSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class ShootOnceCommand extends SequentialCommandGroup{ + private ShooterSubsystem shooter; + private FeedSubsystem feeder; + + public ShootOnceCommand(ShooterSubsystem shooter, FeedSubsystem feeder) { + + this.shooter = shooter; + this.feeder = feeder; + + addRequirements(this.shooter); + + addCommands( + new RunCommand(()-> { + shooter.shoot(0.75); + feeder.feed(0.25); + }, shooter, feeder) + ); + } +} diff --git a/src/main/java/frc/robot/commands/ShootTwiceCommand.java b/src/main/java/frc/robot/commands/ShootTwiceCommand.java new file mode 100644 index 0000000..5caed37 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootTwiceCommand.java @@ -0,0 +1,39 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.FeedSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class ShootTwiceCommand extends SequentialCommandGroup{ + private ShooterSubsystem shooter; + private FeedSubsystem feeder; + private IntakeSubsystem intake; + + public ShootTwiceCommand(ShooterSubsystem shooter, FeedSubsystem feeder, IntakeSubsystem intake) { + + this.shooter = shooter; + this.feeder = feeder; + this.intake = intake; + + addRequirements(this.shooter, this.feeder, this.intake); + + addCommands( + new RunCommand(()-> { + feeder.feed(0.25); + shooter.shoot(0.75); + }, feeder, shooter).withTimeout(2), + new RunCommand(()-> { + feeder.feed(0.15); + intake.intake(0.5); + shooter.shoot(-0.05); + }, feeder, intake, shooter).withTimeout(6), + new RunCommand(()-> { + feeder.feed(0.25); + shooter.shoot(0.75); + },feeder, shooter).withTimeout(2) + ); + } +} From 9b1e716950fe4def649fda69839c45b728738538 Mon Sep 17 00:00:00 2001 From: BenBerol Date: Thu, 21 Mar 2024 10:19:48 -0400 Subject: [PATCH 4/6] Update more paths --- src/main/deploy/pathplanner/paths/Charge.path | 30 ++-- .../deploy/pathplanner/paths/Middle2Note.path | 133 +++--------------- src/main/deploy/pathplanner/paths/Shoot.path | 4 +- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 81 +++++------ .../commands/autonomous/ChargeCommand.java | 2 + .../autonomous/MiddleTwoNoteCommand.java | 20 ++- .../frc/robot/subsystems/MaxWheelModule.java | 5 +- .../subsystems/SwerveDriveSubsystem.java | 29 +++- vendordeps/PathplannerLib.json | 6 +- 10 files changed, 120 insertions(+), 192 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Charge.path b/src/main/deploy/pathplanner/paths/Charge.path index 8b5d5ed..140fcca 100644 --- a/src/main/deploy/pathplanner/paths/Charge.path +++ b/src/main/deploy/pathplanner/paths/Charge.path @@ -3,32 +3,38 @@ "waypoints": [ { "anchor": { - "x": 3.3990747796310408, - "y": 6.867981472074056 + "x": 0.689957228128742, + "y": 6.625068886298487 }, "prevControl": null, "nextControl": { - "x": 4.399074779631045, - "y": 6.867981472074056 + "x": 1.689957228128744, + "y": 6.625068886298487 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.1486443898813725, - "y": 6.867981472074056 + "x": 3.9095172563485012, + "y": 2.9457296684310634 }, "prevControl": { - "x": 0.14864438988137252, - "y": 6.867981472074056 + "x": 2.9095172563485012, + "y": 2.9457296684310634 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.45, + "rotationDegrees": -45.0, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 45.0, + "rotateFast": true }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 0, + "rotation": 57.28864935553704, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Middle2Note.path b/src/main/deploy/pathplanner/paths/Middle2Note.path index 1983fbc..b7e52cb 100644 --- a/src/main/deploy/pathplanner/paths/Middle2Note.path +++ b/src/main/deploy/pathplanner/paths/Middle2Note.path @@ -3,28 +3,28 @@ "waypoints": [ { "anchor": { - "x": 1.2689287141904957, - "y": 5.562302316231041 + "x": 1.14, + "y": 5.56 }, "prevControl": null, "nextControl": { - "x": 1.4912622761563639, - "y": 5.562302316231041 + "x": 1.3623335619658687, + "y": 5.56 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.7536497285957524, + "x": 0.45, "y": 5.562302316231041 }, "prevControl": { - "x": 1.807320789581015, + "x": -0.4963289390147374, "y": 5.562302316231041 }, "nextControl": { - "x": 3.6999786676104898, + "x": 1.3963289390147366, "y": 5.562302316231041 }, "isLocked": false, @@ -32,15 +32,15 @@ }, { "anchor": { - "x": 3.309867566957356, + "x": 3.015245771004935, "y": 5.562302316231041 }, "prevControl": { - "x": 3.668077374669185, + "x": 3.3734555787167637, "y": 5.558666804615571 }, "nextControl": { - "x": 2.594190796714926, + "x": 2.299569000762505, "y": 5.569565800242712 }, "isLocked": false, @@ -48,28 +48,12 @@ }, { "anchor": { - "x": 1.7, - "y": 5.56 - }, - "prevControl": { - "x": 1.7100475418595091, - "y": 5.56 - }, - "nextControl": { - "x": 1.6899524581404908, - "y": 5.56 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.7694608255799635, - "y": 5.56 + "x": 0.28911669244469446, + "y": 5.320438224581806 }, "prevControl": { - "x": 4.124902635835381, - "y": 5.556261972707489 + "x": 0.2991642343042038, + "y": 5.320438224581806 }, "nextControl": null, "isLocked": false, @@ -80,24 +64,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "intake", - "waypointRelativePos": 0.8999999999999999, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "intake" - } - } - ] - } - } - }, - { - "name": "shoot", + "name": "shootTwice", "waypointRelativePos": 0.5499999999999999, "command": { "type": "sequential", @@ -106,58 +73,7 @@ { "type": "named", "data": { - "name": "shoot" - } - } - ] - } - } - }, - { - "name": "printEnd", - "waypointRelativePos": 4.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "printAfter" - } - } - ] - } - } - }, - { - "name": "printRebound", - "waypointRelativePos": 1.7, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "printRebound" - } - } - ] - } - } - }, - { - "name": "wait", - "waypointRelativePos": 0.2, - "command": { - "type": "deadline", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 5.0 + "name": "shootTwice" } } ] @@ -180,23 +96,6 @@ ] } } - }, - { - "name": "backFeed", - "waypointRelativePos": 3.75, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "backFeed" - } - } - ] - } - } } ], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/Shoot.path b/src/main/deploy/pathplanner/paths/Shoot.path index c2d22e1..25c804a 100644 --- a/src/main/deploy/pathplanner/paths/Shoot.path +++ b/src/main/deploy/pathplanner/paths/Shoot.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "shoot", + "name": "shootOnce", "waypointRelativePos": 0.9, "command": { "type": "parallel", @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "shoot" + "name": "shootOnce" } } ] diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e238a41..4c6d82b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -72,6 +72,7 @@ public void autonomousInit() { System.out.println(m_autonomousCommand); m_robotContainer.pivotSubsystem.setLockPos(0.85); System.out.println("AUTO GOT!"); + m_robotContainer.setAuto(true); // schedule the autonomous command (example) @@ -90,6 +91,7 @@ public void teleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. + m_robotContainer.setAuto(false); m_robotContainer.getOdometry().zeroHeading(); m_robotContainer.getSwerveDriveSubsystem().resetLockRot(); m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(0))); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a68f6c9..a971143 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,9 @@ import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.commands.AlignByAprilTag; +import frc.robot.commands.IntakeNoteCommand; +import frc.robot.commands.ShootOnceCommand; +import frc.robot.commands.ShootTwiceCommand; import frc.robot.commands.autonomous.BlueLeftOneCommand; import frc.robot.commands.autonomous.BlueRightOneCommand; import frc.robot.commands.autonomous.ChargeCommand; @@ -27,7 +30,7 @@ import frc.robot.Constants.ClimbConstants; import frc.robot.subsystems.ClimberSubsystem; - +import com.fasterxml.jackson.annotation.JsonCreator.Mode; import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.IdleMode; @@ -61,6 +64,8 @@ public class RobotContainer { private final RobotBase robot; + private boolean inAuto = false; + //Constructing the swerve wheel modules private CANSparkMax backRightAngleMotor = new CANSparkMax(DrivetrainConstants.BACK_RIGHT_ANGLE_ID, MotorType.kBrushless); @@ -128,21 +133,17 @@ public class RobotContainer { private SwerveDriveSubsystem swerveDrive = new SwerveDriveSubsystem( backRightWheel, backLeftWheel, frontRightWheel, frontLeftWheel, - DrivetrainConstants.SWERVE_KINEMATICS, odometry); + DrivetrainConstants.SWERVE_KINEMATICS, odometry, this); private final AlignByAprilTag alignAtAmpCenter = new AlignByAprilTag(swerveDrive, limelight, odometry, 0.03, -0.57, 0.9, 0.07, 0.1, 90, 0); private final AlignByAprilTag alignAtSpeakerCenter = new AlignByAprilTag(swerveDrive, limelight, odometry, 0.03, -1.33, 1, 0.04, 0.1, 0, 0); // private final AlignByAprilTag alignRightOfSpeaker = new AlignByAprilTag(swerveDrive, limelight, odometry, 1.08, -0.96, 1, 0.04, 0.1, 44, 44); private final AlignByAprilTag alignFarFromSpeakerCenter = new AlignByAprilTag(swerveDrive, limelight, odometry, -0.06, -2.40, 1, 0.04, 0.1, 0, 0); - private final ChargeCommand chargeCommand; - private final MiddleOneNoteCommand middle1Command; - private final MiddleTwoNoteCommand middle2Command; - private final BlueLeftOneCommand blueLeftCommand; - private final BlueRightOneCommand blueRightCommand; - private final ShootCommand shootCommand; - - + //Auto Commands: + private final ShootOnceCommand shootOnceCommand = new ShootOnceCommand(shooter, feedSubsystem); + private final ShootTwiceCommand shootTwiceCommand = new ShootTwiceCommand(shooter, feedSubsystem, intakeSubsystem); + private final IntakeNoteCommand intakeNoteCommand = new IntakeNoteCommand(intakeSubsystem, feedSubsystem); SlewRateLimiter limitX = new SlewRateLimiter(6); SlewRateLimiter limitY = new SlewRateLimiter(6); @@ -157,20 +158,20 @@ public RobotContainer(RobotBase robot) { CameraServer.startAutomaticCapture(); - //Auto Test commands: - NamedCommands.registerCommand("print", new PrintCommand("print!")); - NamedCommands.registerCommand("printRight2NoteRed", new PrintCommand("righ2notered path!")); - NamedCommands.registerCommand("printLeft2Note", new PrintCommand("left2note path!")); - NamedCommands.registerCommand("printAfter", new PrintCommand("print at end!")); - NamedCommands.registerCommand("printAfterShoot", new PrintCommand("print after SHOOTER!")); - NamedCommands.registerCommand("printRebound", new PrintCommand("print after REBOUND!")); - - NamedCommands.registerCommand("intake", intakeNoteCommand()); - NamedCommands.registerCommand("shoot", shootNoteCommand()); + //Auto Named Commands: + NamedCommands.registerCommand("intake", intakeNoteCommand); + NamedCommands.registerCommand("shootOnce", shootOnceCommand); + NamedCommands.registerCommand("shootTwice", shootTwiceCommand); NamedCommands.registerCommand("armPos", setArmCommand()); NamedCommands.registerCommand("backFeed", backFeedCommand()); - + //Auto Paths: + final ChargeCommand chargeCommand; + final MiddleOneNoteCommand middle1Command; + final MiddleTwoNoteCommand middle2Command; + final BlueLeftOneCommand blueLeftCommand; + final BlueRightOneCommand blueRightCommand; + final ShootCommand shootCommand; // Configure the trigger bindings configureBindings(); @@ -179,7 +180,7 @@ public RobotContainer(RobotBase robot) { chargeCommand = new ChargeCommand(swerveDrive, feedSubsystem); middle1Command = new MiddleOneNoteCommand(swerveDrive, feedSubsystem); - middle2Command = new MiddleTwoNoteCommand(swerveDrive, feedSubsystem); + middle2Command = new MiddleTwoNoteCommand(swerveDrive, feedSubsystem, shooter); blueLeftCommand = new BlueLeftOneCommand(swerveDrive, feedSubsystem); blueRightCommand = new BlueRightOneCommand(swerveDrive, feedSubsystem); shootCommand = new ShootCommand(swerveDrive, feedSubsystem); @@ -258,8 +259,8 @@ private void configureBindings() { // JoystickButton alignRightOfSpeakerButton = new JoystickButton(rightJoystick, 3); JoystickButton alignFarFromSpeakerButton = new JoystickButton(rightJoystick, 4); - JoystickButton climbUpButton = new JoystickButton(leftJoystick, 5); - JoystickButton climbDownButton = new JoystickButton(leftJoystick, 6); + JoystickButton climbUpButton = new JoystickButton(leftJoystick, 3); + JoystickButton climbDownButton = new JoystickButton(leftJoystick, 4); //Constructs commands and binds them for swerve drive swerveDrive.setDefaultCommand(new RunCommand(() -> { @@ -412,32 +413,19 @@ public SwerveDriveSubsystem getSwerveDriveSubsystem() { return swerveDrive; } + public void setAuto(boolean auto) { + inAuto = auto; + } + + public boolean getAuto() { + return inAuto; + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous */ - - public Command intakeNoteCommand() - { - return (new RunCommand(() -> { - intakeSubsystem.intake(0.5); - feedSubsystem.feed(0.25); - shooter.shoot(0); - // pivotSubsystem.setLockPos(0.82); - System.out.println("intake"); - }, intakeSubsystem, feedSubsystem)); - } - - public Command shootNoteCommand() - { - return (new RunCommand(() -> { - shooter.shoot(0.75); - feedSubsystem.feed(0.25); - System.out.println("shooter"); - // pivotSubsystem.setLockPos(0.85); - }, shooter)); - } public Command backFeedCommand() { @@ -450,8 +438,7 @@ public Command backFeedCommand() public Command setArmCommand() { return (new RunCommand(() -> { - pivotSubsystem.setLockPos(0.84); - System.out.println("set arm"); + pivotSubsystem.setLockPos(0.855); })); } diff --git a/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java b/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java index 127095b..fd432ff 100644 --- a/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java +++ b/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java @@ -21,6 +21,8 @@ public class ChargeCommand extends SequentialCommandGroup { //SET UP WITH SHOOTER TOWARDS US + // private String TRAJECTORY_NAME = "Charge"; + private String TRAJECTORY_NAME = "Charge"; SwerveDriveSubsystem swerveDrive; FeedSubsystem feedSubsystem; diff --git a/src/main/java/frc/robot/commands/autonomous/MiddleTwoNoteCommand.java b/src/main/java/frc/robot/commands/autonomous/MiddleTwoNoteCommand.java index 51ea547..d88fb8a 100644 --- a/src/main/java/frc/robot/commands/autonomous/MiddleTwoNoteCommand.java +++ b/src/main/java/frc/robot/commands/autonomous/MiddleTwoNoteCommand.java @@ -3,6 +3,7 @@ import frc.robot.commands.FeedCommand; import frc.robot.common.Odometry; import frc.robot.subsystems.FeedSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; import frc.robot.commands.FeedCommand; @@ -15,7 +16,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; /** An example command that uses an example subsystem. */ public class MiddleTwoNoteCommand extends SequentialCommandGroup { @@ -23,15 +26,28 @@ public class MiddleTwoNoteCommand extends SequentialCommandGroup { private String TRAJECTORY_NAME = "Middle2Note"; SwerveDriveSubsystem swerveDrive; FeedSubsystem feedSubsystem; + ShooterSubsystem shooter; - public MiddleTwoNoteCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem) { + public MiddleTwoNoteCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem, ShooterSubsystem shooter) { this.swerveDrive = swerveDrive; this.feedSubsystem = feedSubsystem; + this.shooter = shooter; addRequirements(swerveDrive, feedSubsystem); addCommands( - swerveDrive.followPath(PathPlannerPath.fromPathFile(TRAJECTORY_NAME)) + swerveDrive.followPath(PathPlannerPath.fromPathFile(TRAJECTORY_NAME)), + new RunCommand(() -> { + feedSubsystem.feed(-0.1); + }, feedSubsystem).withTimeout(0.5), + new RunCommand(() -> { + feedSubsystem.feed(0); + }, feedSubsystem).withTimeout(1), + new RunCommand(() -> { + feedSubsystem.feed(0.25); + shooter.shoot(0.75); + }, feedSubsystem, shooter).withTimeout(2) + ); } diff --git a/src/main/java/frc/robot/subsystems/MaxWheelModule.java b/src/main/java/frc/robot/subsystems/MaxWheelModule.java index bf5ac79..bea4926 100644 --- a/src/main/java/frc/robot/subsystems/MaxWheelModule.java +++ b/src/main/java/frc/robot/subsystems/MaxWheelModule.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.robot.RobotContainer; import frc.robot.Constants.WheelConstants; public class MaxWheelModule { @@ -59,12 +60,12 @@ public SwerveModulePosition getSwerveModulePosition() { } //Final drive method, passes each wheel's states into the PID controllers and optimizes rotation - public void drive(SwerveModuleState state) + public void drive(SwerveModuleState state, boolean inAuto) { SwerveModuleState optimizedState = SwerveModuleState.optimize(state, Rotation2d.fromRotations( getEncoderPosition() )); - + targetSpeed = optimizedState.speedMetersPerSecond; speedPIDController.setReference(targetSpeed, ControlType.kVelocity); diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index e7baa12..5198799 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; import frc.robot.Constants.DriverConstants; import frc.robot.common.Odometry; @@ -53,7 +54,7 @@ public class SwerveDriveSubsystem extends SubsystemBase { private final NetworkTable odometryTable = ntInstance.getTable("/common/Odometry"); private final NetworkTableEntry ntOdometryPose = odometryTable.getEntry("odometryPose"); - private PIDController rotationController = new PIDController(0.04, 0, 0); + private PIDController rotationController = new PIDController(0.06, 0, 0); private SwerveDriveKinematics kinematics; private Odometry odometry; @@ -63,8 +64,10 @@ public class SwerveDriveSubsystem extends SubsystemBase { private double lockedRot = 0; private ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 0); + private RobotContainer robotContainer; + public SwerveDriveSubsystem(MaxWheelModule backRight, MaxWheelModule backLeft, MaxWheelModule frontRight, MaxWheelModule frontLeft, - SwerveDriveKinematics kinematics, Odometry odometry) { + SwerveDriveKinematics kinematics, Odometry odometry, RobotContainer robotContainer) { this.backRight = backRight; this.backLeft = backLeft; @@ -74,6 +77,8 @@ public SwerveDriveSubsystem(MaxWheelModule backRight, MaxWheelModule backLeft, M this.kinematics = kinematics; this.odometry = odometry; + this.robotContainer = robotContainer; + this.ntIsFieldCentric.setBoolean(fieldCentric); AutoBuilder.configureHolonomic( @@ -131,7 +136,17 @@ public void drive(double x, double y, double rot) { // Second method passing to moduleStates public void drive(ChassisSpeeds speeds){ - SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(speeds); + ChassisSpeeds newSpeeds = speeds; + if (robotContainer.getAuto()) { + newSpeeds = speeds.minus(new ChassisSpeeds(speeds.vxMetersPerSecond*2, 0, 0)); + } + // ChassisSpeeds newSpeeds = speeds.times(0); + if (newSpeeds.omegaRadiansPerSecond != 0) { + System.out.println("rot: " + newSpeeds.omegaRadiansPerSecond); + } + + SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(newSpeeds); + drive(moduleStates); } @@ -143,10 +158,10 @@ public void drive (SwerveModuleState[] moduleStates){ SwerveModuleState backLeftState = moduleStates[2]; SwerveModuleState backRightState = moduleStates[3]; - frontLeft.drive(frontLeftState); - frontRight.drive(frontRightState); - backLeft.drive(backLeftState); - backRight.drive(backRightState); + frontLeft.drive(frontLeftState, robotContainer.getAuto()); + frontRight.drive(frontRightState, robotContainer.getAuto()); + backLeft.drive(backLeftState, robotContainer.getAuto()); + backRight.drive(backRightState, robotContainer.getAuto()); } //Locks the robot's angles to prevent movement diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 787450f..a019706 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.4", + "version": "2024.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.4" + "version": "2024.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.4", + "version": "2024.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From e2422e7d4d035bd70319892e79dfb9c56c969a53 Mon Sep 17 00:00:00 2001 From: BenBerol Date: Sat, 23 Mar 2024 09:11:31 -0400 Subject: [PATCH 5/6] Middle 2 --- .../paths/{New Charge.path => BlueLeft1.path} | 20 ++++---- .../{ChargeCopy.path => BlueRight1.path} | 20 ++++---- src/main/deploy/pathplanner/paths/Charge.path | 28 +++++------ .../deploy/pathplanner/paths/Middle2Note.path | 49 +++---------------- src/main/java/frc/robot/RobotContainer.java | 10 ++-- .../frc/robot/commands/IntakeNoteCommand.java | 1 - .../frc/robot/commands/ShootOnceCommand.java | 11 ++++- .../frc/robot/commands/ShootTwiceCommand.java | 7 +-- .../autonomous/BlueLeftOneCommand.java | 18 +++++-- .../autonomous/BlueRightOneCommand.java | 17 ++++++- .../commands/autonomous/ChargeCommand.java | 12 ----- .../autonomous/MiddleTwoNoteCommand.java | 46 ++++++++++++++--- .../commands/autonomous/ShootCommand.java | 18 +++++-- .../subsystems/SwerveDriveSubsystem.java | 20 +++++--- 14 files changed, 153 insertions(+), 124 deletions(-) rename src/main/deploy/pathplanner/paths/{New Charge.path => BlueLeft1.path} (71%) rename src/main/deploy/pathplanner/paths/{ChargeCopy.path => BlueRight1.path} (71%) diff --git a/src/main/deploy/pathplanner/paths/New Charge.path b/src/main/deploy/pathplanner/paths/BlueLeft1.path similarity index 71% rename from src/main/deploy/pathplanner/paths/New Charge.path rename to src/main/deploy/pathplanner/paths/BlueLeft1.path index 2f85b10..293daab 100644 --- a/src/main/deploy/pathplanner/paths/New Charge.path +++ b/src/main/deploy/pathplanner/paths/BlueLeft1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.4538800929453712, - "y": 5.58391306597428 + "x": 0.6389027008015726, + "y": 6.5860328446899 }, "prevControl": null, "nextControl": { - "x": 2.4538800929453757, - "y": 5.58391306597428 + "x": 1.376356984489902, + "y": 6.8226047175732285 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.081104361282797, - "y": 5.58391306597428 + "x": 3.283642594070903, + "y": 6.5860328446899 }, "prevControl": { - "x": 3.0811043612827973, - "y": 5.58391306597428 + "x": 2.385813636959755, + "y": 6.33537530654963 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0, + "rotation": -120.0, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 180.0, + "rotation": -120.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/ChargeCopy.path b/src/main/deploy/pathplanner/paths/BlueRight1.path similarity index 71% rename from src/main/deploy/pathplanner/paths/ChargeCopy.path rename to src/main/deploy/pathplanner/paths/BlueRight1.path index 2c60a29..3bb1ab5 100644 --- a/src/main/deploy/pathplanner/paths/ChargeCopy.path +++ b/src/main/deploy/pathplanner/paths/BlueRight1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.406016284254791, - "y": 5.547688595377191 + "x": 0.765577540269664, + "y": 4.5617833320860095 }, "prevControl": null, "nextControl": { - "x": 2.4060162842547927, - "y": 5.547688595377191 + "x": 1.7655775402696618, + "y": 4.5617833320860095 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.296907191036604, - "y": 5.547688595377191 + "x": 3.432094929666608, + "y": 2.0115087365632673 }, "prevControl": { - "x": 3.2969071910366043, - "y": 5.547688595377191 + "x": 1.7205183415214291, + "y": 1.896900456755597 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0, + "rotation": 120.0, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 180.0, + "rotation": 120.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Charge.path b/src/main/deploy/pathplanner/paths/Charge.path index 140fcca..58515d3 100644 --- a/src/main/deploy/pathplanner/paths/Charge.path +++ b/src/main/deploy/pathplanner/paths/Charge.path @@ -3,38 +3,32 @@ "waypoints": [ { "anchor": { - "x": 0.689957228128742, - "y": 6.625068886298487 + "x": 0.8269679653198594, + "y": 6.9748548946502655 }, "prevControl": null, "nextControl": { - "x": 1.689957228128744, - "y": 6.625068886298487 + "x": 1.8269679653198612, + "y": 6.9748548946502655 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.9095172563485012, - "y": 2.9457296684310634 + "x": 4.002876853409957, + "y": 6.9748548946502655 }, "prevControl": { - "x": 2.9095172563485012, - "y": 2.9457296684310634 + "x": 3.0028768534099566, + "y": 6.9748548946502655 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.45, - "rotationDegrees": -45.0, - "rotateFast": true - } - ], + "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -45,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 45.0, + "rotation": 180.0, "rotateFast": true }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 57.28864935553704, + "rotation": 180.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Middle2Note.path b/src/main/deploy/pathplanner/paths/Middle2Note.path index b7e52cb..4e5210f 100644 --- a/src/main/deploy/pathplanner/paths/Middle2Note.path +++ b/src/main/deploy/pathplanner/paths/Middle2Note.path @@ -16,31 +16,15 @@ }, { "anchor": { - "x": 0.45, + "x": 3.18, "y": 5.562302316231041 }, "prevControl": { - "x": -0.4963289390147374, - "y": 5.562302316231041 - }, - "nextControl": { - "x": 1.3963289390147366, - "y": 5.562302316231041 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.015245771004935, - "y": 5.562302316231041 - }, - "prevControl": { - "x": 3.3734555787167637, + "x": 3.5382098077118287, "y": 5.558666804615571 }, "nextControl": { - "x": 2.299569000762505, + "x": 2.46432322975757, "y": 5.569565800242712 }, "isLocked": false, @@ -48,12 +32,12 @@ }, { "anchor": { - "x": 0.28911669244469446, - "y": 5.320438224581806 + "x": 0.5217945759552234, + "y": 5.56 }, "prevControl": { - "x": 0.2991642343042038, - "y": 5.320438224581806 + "x": 0.5318421178147328, + "y": 5.56 }, "nextControl": null, "isLocked": false, @@ -65,7 +49,7 @@ "eventMarkers": [ { "name": "shootTwice", - "waypointRelativePos": 0.5499999999999999, + "waypointRelativePos": 0.3, "command": { "type": "sequential", "data": { @@ -79,23 +63,6 @@ ] } } - }, - { - "name": "arm", - "waypointRelativePos": 0.0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "armPos" - } - } - ] - } - } } ], "globalConstraints": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a971143..6f6d70b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -141,7 +141,7 @@ public class RobotContainer { private final AlignByAprilTag alignFarFromSpeakerCenter = new AlignByAprilTag(swerveDrive, limelight, odometry, -0.06, -2.40, 1, 0.04, 0.1, 0, 0); //Auto Commands: - private final ShootOnceCommand shootOnceCommand = new ShootOnceCommand(shooter, feedSubsystem); + private final ShootOnceCommand shootOnceCommand = new ShootOnceCommand(shooter, feedSubsystem, pivotSubsystem); private final ShootTwiceCommand shootTwiceCommand = new ShootTwiceCommand(shooter, feedSubsystem, intakeSubsystem); private final IntakeNoteCommand intakeNoteCommand = new IntakeNoteCommand(intakeSubsystem, feedSubsystem); @@ -180,10 +180,10 @@ public RobotContainer(RobotBase robot) { chargeCommand = new ChargeCommand(swerveDrive, feedSubsystem); middle1Command = new MiddleOneNoteCommand(swerveDrive, feedSubsystem); - middle2Command = new MiddleTwoNoteCommand(swerveDrive, feedSubsystem, shooter); - blueLeftCommand = new BlueLeftOneCommand(swerveDrive, feedSubsystem); - blueRightCommand = new BlueRightOneCommand(swerveDrive, feedSubsystem); - shootCommand = new ShootCommand(swerveDrive, feedSubsystem); + middle2Command = new MiddleTwoNoteCommand(swerveDrive, feedSubsystem, shooter, pivotSubsystem); + blueLeftCommand = new BlueLeftOneCommand(swerveDrive, feedSubsystem, shooter, pivotSubsystem); + blueRightCommand = new BlueRightOneCommand(swerveDrive, feedSubsystem, shooter, pivotSubsystem); + shootCommand = new ShootCommand(swerveDrive, feedSubsystem, shooter, pivotSubsystem); // Build an auto chooser. This will use Commands.none() as the default option. autoChooser = AutoBuilder.buildAutoChooser(); diff --git a/src/main/java/frc/robot/commands/IntakeNoteCommand.java b/src/main/java/frc/robot/commands/IntakeNoteCommand.java index 8d4d4e3..6530593 100644 --- a/src/main/java/frc/robot/commands/IntakeNoteCommand.java +++ b/src/main/java/frc/robot/commands/IntakeNoteCommand.java @@ -22,7 +22,6 @@ public IntakeNoteCommand(IntakeSubsystem intake, FeedSubsystem feeder) { new RunCommand(()-> { intake.intake(0.5); feeder.feed(0.25); - System.out.println("intake"); }, intake, feeder).withTimeout(4) ); } diff --git a/src/main/java/frc/robot/commands/ShootOnceCommand.java b/src/main/java/frc/robot/commands/ShootOnceCommand.java index a27896d..b5d149e 100644 --- a/src/main/java/frc/robot/commands/ShootOnceCommand.java +++ b/src/main/java/frc/robot/commands/ShootOnceCommand.java @@ -4,24 +4,31 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.FeedSubsystem; +import frc.robot.subsystems.PivotSubsystem; import frc.robot.subsystems.ShooterSubsystem; public class ShootOnceCommand extends SequentialCommandGroup{ private ShooterSubsystem shooter; private FeedSubsystem feeder; + private PivotSubsystem pivot; - public ShootOnceCommand(ShooterSubsystem shooter, FeedSubsystem feeder) { + public ShootOnceCommand(ShooterSubsystem shooter, FeedSubsystem feeder, PivotSubsystem pivot) { this.shooter = shooter; this.feeder = feeder; + this.pivot = pivot; addRequirements(this.shooter); addCommands( + new RunCommand(() -> { + pivot.setLockPos(0.86); + }).withTimeout(4), + new RunCommand(()-> { shooter.shoot(0.75); feeder.feed(0.25); - }, shooter, feeder) + }, shooter, feeder).withTimeout(1) ); } } diff --git a/src/main/java/frc/robot/commands/ShootTwiceCommand.java b/src/main/java/frc/robot/commands/ShootTwiceCommand.java index 5caed37..5058a1e 100644 --- a/src/main/java/frc/robot/commands/ShootTwiceCommand.java +++ b/src/main/java/frc/robot/commands/ShootTwiceCommand.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.FeedSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -21,15 +20,11 @@ public ShootTwiceCommand(ShooterSubsystem shooter, FeedSubsystem feeder, IntakeS addRequirements(this.shooter, this.feeder, this.intake); addCommands( - new RunCommand(()-> { - feeder.feed(0.25); - shooter.shoot(0.75); - }, feeder, shooter).withTimeout(2), new RunCommand(()-> { feeder.feed(0.15); intake.intake(0.5); shooter.shoot(-0.05); - }, feeder, intake, shooter).withTimeout(6), + }, feeder, intake, shooter).withTimeout(7), new RunCommand(()-> { feeder.feed(0.25); shooter.shoot(0.75); diff --git a/src/main/java/frc/robot/commands/autonomous/BlueLeftOneCommand.java b/src/main/java/frc/robot/commands/autonomous/BlueLeftOneCommand.java index 2c9e02a..6109d63 100644 --- a/src/main/java/frc/robot/commands/autonomous/BlueLeftOneCommand.java +++ b/src/main/java/frc/robot/commands/autonomous/BlueLeftOneCommand.java @@ -1,8 +1,11 @@ package frc.robot.commands.autonomous; import frc.robot.commands.FeedCommand; +import frc.robot.commands.ShootOnceCommand; import frc.robot.common.Odometry; import frc.robot.subsystems.FeedSubsystem; +import frc.robot.subsystems.PivotSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; import frc.robot.commands.FeedCommand; @@ -15,24 +18,33 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; /** An example command that uses an example subsystem. */ public class BlueLeftOneCommand extends SequentialCommandGroup { - private String TRAJECTORY_NAME = "BlueLeft1Note"; + private String TRAJECTORY_NAME = "BlueLeft1"; SwerveDriveSubsystem swerveDrive; FeedSubsystem feedSubsystem; + ShooterSubsystem shooter; + PivotSubsystem pivot; - public BlueLeftOneCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem) { + public BlueLeftOneCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem, ShooterSubsystem shooter, PivotSubsystem pivot) { this.swerveDrive = swerveDrive; this.feedSubsystem = feedSubsystem; + this.shooter = shooter; + this.pivot = pivot; addRequirements(swerveDrive, feedSubsystem); addCommands( - swerveDrive.followPath(PathPlannerPath.fromPathFile(TRAJECTORY_NAME)) + new ShootOnceCommand(shooter, feedSubsystem, pivot).deadlineWith( + new RunCommand(() -> { + swerveDrive.turtle(); + }, swerveDrive) + ) ); } diff --git a/src/main/java/frc/robot/commands/autonomous/BlueRightOneCommand.java b/src/main/java/frc/robot/commands/autonomous/BlueRightOneCommand.java index 9a09cc7..54c9597 100644 --- a/src/main/java/frc/robot/commands/autonomous/BlueRightOneCommand.java +++ b/src/main/java/frc/robot/commands/autonomous/BlueRightOneCommand.java @@ -1,8 +1,11 @@ package frc.robot.commands.autonomous; import frc.robot.commands.FeedCommand; +import frc.robot.commands.ShootOnceCommand; import frc.robot.common.Odometry; import frc.robot.subsystems.FeedSubsystem; +import frc.robot.subsystems.PivotSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; import frc.robot.commands.FeedCommand; @@ -15,23 +18,33 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; /** An example command that uses an example subsystem. */ public class BlueRightOneCommand extends SequentialCommandGroup { - private String TRAJECTORY_NAME = "BlueLeft1Note"; + private String TRAJECTORY_NAME = "BlueRight1"; SwerveDriveSubsystem swerveDrive; FeedSubsystem feedSubsystem; + ShooterSubsystem shooter; + PivotSubsystem pivot; - public BlueRightOneCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem) { + public BlueRightOneCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem, ShooterSubsystem shooter, PivotSubsystem pivot) { this.swerveDrive = swerveDrive; this.feedSubsystem = feedSubsystem; + this.shooter = shooter; + this.pivot = pivot; addRequirements(swerveDrive, feedSubsystem); addCommands( + new ShootOnceCommand(shooter, feedSubsystem, pivot).deadlineWith( + new RunCommand(() -> { + swerveDrive.turtle(); + }, swerveDrive) + ), swerveDrive.followPath(PathPlannerPath.fromPathFile(TRAJECTORY_NAME)) ); } diff --git a/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java b/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java index fd432ff..f9d679b 100644 --- a/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java +++ b/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java @@ -1,27 +1,15 @@ package frc.robot.commands.autonomous; -import frc.robot.commands.FeedCommand; -import frc.robot.common.Odometry; import frc.robot.subsystems.FeedSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; -import frc.robot.commands.FeedCommand; - -import java.nio.file.Path; -import java.util.HashMap; - -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; /** An example command that uses an example subsystem. */ public class ChargeCommand extends SequentialCommandGroup { //SET UP WITH SHOOTER TOWARDS US - // private String TRAJECTORY_NAME = "Charge"; private String TRAJECTORY_NAME = "Charge"; SwerveDriveSubsystem swerveDrive; diff --git a/src/main/java/frc/robot/commands/autonomous/MiddleTwoNoteCommand.java b/src/main/java/frc/robot/commands/autonomous/MiddleTwoNoteCommand.java index d88fb8a..c45b69f 100644 --- a/src/main/java/frc/robot/commands/autonomous/MiddleTwoNoteCommand.java +++ b/src/main/java/frc/robot/commands/autonomous/MiddleTwoNoteCommand.java @@ -3,6 +3,7 @@ import frc.robot.commands.FeedCommand; import frc.robot.common.Odometry; import frc.robot.subsystems.FeedSubsystem; +import frc.robot.subsystems.PivotSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; import frc.robot.commands.FeedCommand; @@ -27,27 +28,60 @@ public class MiddleTwoNoteCommand extends SequentialCommandGroup { SwerveDriveSubsystem swerveDrive; FeedSubsystem feedSubsystem; ShooterSubsystem shooter; + PivotSubsystem pivot; - public MiddleTwoNoteCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem, ShooterSubsystem shooter) { + public MiddleTwoNoteCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem, ShooterSubsystem shooter, PivotSubsystem pivot) { this.swerveDrive = swerveDrive; this.feedSubsystem = feedSubsystem; this.shooter = shooter; + this.pivot = pivot; - addRequirements(swerveDrive, feedSubsystem); + addRequirements(swerveDrive, feedSubsystem, shooter, pivot); addCommands( + new RunCommand(() -> { + pivot.setLockPos(0.86); + }).withTimeout(2).deadlineWith( + new RunCommand(() -> { + swerveDrive.turtle(); + }, swerveDrive) + ), + + new RunCommand(()-> { + feedSubsystem.feed(0.25); + shooter.shoot(0.75); + }, feedSubsystem, shooter).withTimeout(1).deadlineWith( + new RunCommand(() -> { + swerveDrive.turtle(); + }, swerveDrive) + ), + swerveDrive.followPath(PathPlannerPath.fromPathFile(TRAJECTORY_NAME)), + new RunCommand(() -> { feedSubsystem.feed(-0.1); - }, feedSubsystem).withTimeout(0.5), + }, feedSubsystem).withTimeout(0.5).deadlineWith( + new RunCommand(() -> { + swerveDrive.turtle(); + }, swerveDrive) + ), + new RunCommand(() -> { feedSubsystem.feed(0); - }, feedSubsystem).withTimeout(1), + }, feedSubsystem).withTimeout(2).deadlineWith( + new RunCommand(() -> { + swerveDrive.turtle(); + }, swerveDrive) + ), + new RunCommand(() -> { feedSubsystem.feed(0.25); shooter.shoot(0.75); - }, feedSubsystem, shooter).withTimeout(2) - + }, feedSubsystem, shooter).withTimeout(2).deadlineWith( + new RunCommand(() -> { + swerveDrive.turtle(); + }, swerveDrive) + ) ); } diff --git a/src/main/java/frc/robot/commands/autonomous/ShootCommand.java b/src/main/java/frc/robot/commands/autonomous/ShootCommand.java index c4551cb..fc3416d 100644 --- a/src/main/java/frc/robot/commands/autonomous/ShootCommand.java +++ b/src/main/java/frc/robot/commands/autonomous/ShootCommand.java @@ -1,8 +1,11 @@ package frc.robot.commands.autonomous; import frc.robot.commands.FeedCommand; +import frc.robot.commands.ShootOnceCommand; import frc.robot.common.Odometry; import frc.robot.subsystems.FeedSubsystem; +import frc.robot.subsystems.PivotSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; import frc.robot.commands.FeedCommand; @@ -15,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; /** An example command that uses an example subsystem. */ @@ -23,16 +27,24 @@ public class ShootCommand extends SequentialCommandGroup { private String TRAJECTORY_NAME = "Shoot"; SwerveDriveSubsystem swerveDrive; FeedSubsystem feedSubsystem; + ShooterSubsystem shooter; + PivotSubsystem pivot; - public ShootCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem) { + public ShootCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem, ShooterSubsystem shooter, PivotSubsystem pivot) { this.swerveDrive = swerveDrive; this.feedSubsystem = feedSubsystem; + this.shooter = shooter; + this.pivot = pivot; - addRequirements(swerveDrive, feedSubsystem); + addRequirements(swerveDrive, feedSubsystem, shooter, pivot); addCommands( - swerveDrive.followPath(PathPlannerPath.fromPathFile(TRAJECTORY_NAME)) + new ShootOnceCommand(shooter, feedSubsystem, pivot).deadlineWith( + new RunCommand(() -> { + swerveDrive.turtle(); + }, swerveDrive) + ) ); } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 5198799..bf858c3 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -54,7 +54,8 @@ public class SwerveDriveSubsystem extends SubsystemBase { private final NetworkTable odometryTable = ntInstance.getTable("/common/Odometry"); private final NetworkTableEntry ntOdometryPose = odometryTable.getEntry("odometryPose"); - private PIDController rotationController = new PIDController(0.06, 0, 0); + private PIDController rotationController = new PIDController(0.04, 0, 0); + private PIDController autoController = new PIDController(0.12, 0, 0); private SwerveDriveKinematics kinematics; private Odometry odometry; @@ -113,7 +114,6 @@ public SwerveDriveSubsystem(MaxWheelModule backRight, MaxWheelModule backLeft, M //Initial drive method, maintains rotation and passes into ChassisSpeeds public void drive(double x, double y, double rot) { - if(rot == 0){ rot = rotationController.calculate(odometry.getHeading(), lockedRot); } @@ -137,13 +137,21 @@ public void drive(double x, double y, double rot) { // Second method passing to moduleStates public void drive(ChassisSpeeds speeds){ ChassisSpeeds newSpeeds = speeds; + + double ogRot = newSpeeds.omegaRadiansPerSecond; + + if (newSpeeds.omegaRadiansPerSecond != 0) { + System.out.println("rot: " + ogRot); + } if (robotContainer.getAuto()) { newSpeeds = speeds.minus(new ChassisSpeeds(speeds.vxMetersPerSecond*2, 0, 0)); + newSpeeds = newSpeeds.plus(new ChassisSpeeds(0, 0, autoController.calculate(odometry.getHeading(), lockedRot))); + System.out.println("Adjustment rot: " + (newSpeeds.omegaRadiansPerSecond-ogRot)); } - // ChassisSpeeds newSpeeds = speeds.times(0); - if (newSpeeds.omegaRadiansPerSecond != 0) { - System.out.println("rot: " + newSpeeds.omegaRadiansPerSecond); - } + + System.out.println("heading: " + odometry.getHeading()); + + System.out.println("locked rot: " + lockedRot); SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(newSpeeds); From 42342f8c56c70087b9871e5d4ff9a8e3dcc97fd3 Mon Sep 17 00:00:00 2001 From: MyaTaheri <91988696+MyaTaheri@users.noreply.github.com> Date: Sat, 23 Mar 2024 14:17:34 -0400 Subject: [PATCH 6/6] Update encoder offset --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1fa0987..73f9aa2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,7 +39,7 @@ public final static class DrivetrainConstants{ public static final int BACK_LEFT_SPEED_ID = 2; public static final AnalogInput BACK_LEFT_ENCODER = new AnalogInput(2); public static final Translation2d BACK_LEFT_LOC = new Translation2d(-0.238125, 0.238125); - public static final double BACK_LEFT_ENCODER_OFFSET = 0.654; + public static final double BACK_LEFT_ENCODER_OFFSET = 0.844; public static final int FRONT_LEFT_ANGLE_ID = 3; public static final int FRONT_LEFT_SPEED_ID = 4;