From 1e884035ccc319d07f6dabfcd12cdb1f02a55747 Mon Sep 17 00:00:00 2001 From: joelzoto <118407587+joelzoto@users.noreply.github.com> Date: Tue, 8 Apr 2025 21:08:57 -0400 Subject: [PATCH] auton tuning --- src/main/deploy/pathplanner/paths/A_LGr.path | 14 +++--- src/main/deploy/pathplanner/paths/B_MGr.path | 18 ++++---- src/main/deploy/pathplanner/paths/B_RGr.path | 4 +- src/main/deploy/pathplanner/paths/C_Load.path | 24 +++++----- src/main/deploy/pathplanner/paths/C_RGr.path | 14 +++--- src/main/deploy/pathplanner/paths/D_Load.path | 24 +++++----- src/main/deploy/pathplanner/paths/F_Load.path | 8 ++-- src/main/deploy/pathplanner/paths/Load_C.path | 16 +++---- src/main/deploy/pathplanner/paths/Load_D.path | 16 +++---- src/main/deploy/pathplanner/paths/MGr_A.path | 8 ++-- src/main/deploy/pathplanner/paths/MGr_B.path | 4 +- src/main/deploy/pathplanner/paths/RGr_B.path | 12 ++--- .../commands/DrivetrainCommands.java | 45 +++++++++++++++++++ .../java/frc/robot/operator/OperatorXbox.java | 2 + .../frc/robot/vision/commands/AutoAlign.java | 2 +- 15 files changed, 129 insertions(+), 82 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A_LGr.path b/src/main/deploy/pathplanner/paths/A_LGr.path index c9bd928..19ad298 100644 --- a/src/main/deploy/pathplanner/paths/A_LGr.path +++ b/src/main/deploy/pathplanner/paths/A_LGr.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.4235000000000002, - "y": 5.507000000000001 + "x": 1.8221311475409834, + "y": 6.020952868852459 }, "prevControl": { - "x": 1.7536630434782607, - "y": 5.470315217391304 + "x": 2.1522941910192444, + "y": 5.9842680862437625 }, "nextControl": null, "isLocked": false, @@ -38,8 +38,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 2.5, + "maxAcceleration": 2.5, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, @@ -55,5 +55,5 @@ "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B_MGr.path b/src/main/deploy/pathplanner/paths/B_MGr.path index ff194aa..2b5a921 100644 --- a/src/main/deploy/pathplanner/paths/B_MGr.path +++ b/src/main/deploy/pathplanner/paths/B_MGr.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.54475, + "x": 2.7811475409836066, "y": 3.9762499999999994 }, "prevControl": null, "nextControl": { - "x": 2.1265434782608694, + "x": 2.362941019244476, "y": 4.012934782608695 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.5405000000000002, - "y": 3.8787499999999993 + "x": 1.7382172131147542, + "y": 4.076 }, "prevControl": { - "x": 1.8706630434782612, - "y": 3.842065217391303 + "x": 2.0683802565930147, + "y": 4.039315217391303 }, "nextControl": null, "isLocked": false, @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 2.5, + "maxAcceleration": 2.5, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, @@ -62,5 +62,5 @@ "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B_RGr.path b/src/main/deploy/pathplanner/paths/B_RGr.path index 2dded8f..8a8d8c7 100644 --- a/src/main/deploy/pathplanner/paths/B_RGr.path +++ b/src/main/deploy/pathplanner/paths/B_RGr.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.54475, + "x": 2.7811475409836066, "y": 3.9762499999999994 }, "prevControl": null, "nextControl": { - "x": 1.9357826086956522, + "x": 2.1721801496792588, "y": 3.9469021739130428 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C_Load.path b/src/main/deploy/pathplanner/paths/C_Load.path index 1ae7225..22b1d98 100644 --- a/src/main/deploy/pathplanner/paths/C_Load.path +++ b/src/main/deploy/pathplanner/paths/C_Load.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.50025, - "y": 2.2602499999999996 + "x": 3.560348360655738, + "y": 2.4845799180327868 }, "prevControl": null, "nextControl": { - "x": 2.211763421851226, - "y": 2.071946746391003 + "x": 2.0338307237824553, + "y": 2.3513180869673818 }, "isLocked": false, "linkedName": "C" }, { "anchor": { - "x": 1.693421052631579, - "y": 0.7151315789473688 + "x": 1.7142418032786886, + "y": 0.7823258196721316 }, "prevControl": { - "x": 2.1175970874680523, - "y": 1.5674789841066974 + "x": 2.352751531894421, + "y": 1.6728229721921297 }, "nextControl": null, "isLocked": false, @@ -33,12 +33,12 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 4.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -120.80144597613695 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C_RGr.path b/src/main/deploy/pathplanner/paths/C_RGr.path index d6fbd04..92accb0 100644 --- a/src/main/deploy/pathplanner/paths/C_RGr.path +++ b/src/main/deploy/pathplanner/paths/C_RGr.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.5014999999999998, - "y": 2.5625 + "x": 1.474487704918033, + "y": 2.9640881147540985 }, "prevControl": { - "x": 1.8610108695652177, - "y": 2.789945652173914 + "x": 1.833998574483251, + "y": 3.1915337669280124 }, "nextControl": null, "isLocked": false, @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 2.5, + "maxAcceleration": 2.5, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, @@ -62,5 +62,5 @@ "velocity": 0, "rotation": -120.80144597613695 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D_Load.path b/src/main/deploy/pathplanner/paths/D_Load.path index e16323e..4b265cd 100644 --- a/src/main/deploy/pathplanner/paths/D_Load.path +++ b/src/main/deploy/pathplanner/paths/D_Load.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.4417500000000003, - "y": 2.230999999999999 + "x": 3.5483606557377048, + "y": 2.41265368852459 }, "prevControl": null, "nextControl": { - "x": 1.8386970625610948, - "y": 1.9971132171373727 + "x": 1.9453077182987992, + "y": 2.1787669056619636 }, "isLocked": false, "linkedName": "D" }, { "anchor": { - "x": 1.693421052631579, - "y": 0.7151315789473688 + "x": 1.7142418032786886, + "y": 0.7823258196721316 }, "prevControl": { - "x": 2.148635892642519, - "y": 0.9903496842500739 + "x": 2.1694566432896285, + "y": 1.0575439249748366 }, "nextControl": null, "isLocked": false, @@ -40,12 +40,12 @@ } ], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 4.5, + "maxAcceleration": 4.5, "maxAngularVelocity": 400.0, "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, - "unlimited": true + "unlimited": false }, "goalEndState": { "velocity": 0, @@ -57,5 +57,5 @@ "velocity": 0, "rotation": -120.80144597613695 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F_Load.path b/src/main/deploy/pathplanner/paths/F_Load.path index 24f5b16..c34c584 100644 --- a/src/main/deploy/pathplanner/paths/F_Load.path +++ b/src/main/deploy/pathplanner/paths/F_Load.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.693421052631579, - "y": 0.7151315789473688 + "x": 1.7142418032786886, + "y": 0.7823258196721316 }, "prevControl": { - "x": 3.069619172428834, - "y": 1.3063477775858439 + "x": 3.090439923075944, + "y": 1.3735420183106068 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Load_C.path b/src/main/deploy/pathplanner/paths/Load_C.path index 46fb0ad..7465810 100644 --- a/src/main/deploy/pathplanner/paths/Load_C.path +++ b/src/main/deploy/pathplanner/paths/Load_C.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.693421052631579, - "y": 0.7151315789473688 + "x": 1.7142418032786886, + "y": 0.7823258196721316 }, "prevControl": null, "nextControl": { - "x": 2.334830844183318, - "y": 1.1949404730700535 + "x": 2.355651594830428, + "y": 1.2621347137948162 }, "isLocked": false, "linkedName": "RightLoad" }, { "anchor": { - "x": 3.50025, - "y": 2.2602499999999996 + "x": 3.560348360655738, + "y": 2.4845799180327868 }, "prevControl": { - "x": 1.8696567014550511, - "y": 1.909669423953661 + "x": 1.9297550621107895, + "y": 2.1339993419864483 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Load_D.path b/src/main/deploy/pathplanner/paths/Load_D.path index 9074eb1..3f3b842 100644 --- a/src/main/deploy/pathplanner/paths/Load_D.path +++ b/src/main/deploy/pathplanner/paths/Load_D.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.693421052631579, - "y": 0.7151315789473688 + "x": 1.7142418032786886, + "y": 0.7823258196721316 }, "prevControl": null, "nextControl": { - "x": 2.190805151537417, - "y": 0.8630032432566129 + "x": 2.1875152957158983, + "y": 0.8957055224604326 }, "isLocked": false, "linkedName": "RightLoad" }, { "anchor": { - "x": 3.4417500000000003, - "y": 2.230999999999999 + "x": 3.5483606557377048, + "y": 2.41265368852459 }, "prevControl": { - "x": 1.7686736833661747, - "y": 2.0110759481020244 + "x": 1.9207271832518775, + "y": 2.1678426822096943 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/MGr_A.path b/src/main/deploy/pathplanner/paths/MGr_A.path index 9f82f9e..9cde661 100644 --- a/src/main/deploy/pathplanner/paths/MGr_A.path +++ b/src/main/deploy/pathplanner/paths/MGr_A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.5405000000000002, - "y": 3.8787499999999993 + "x": 1.7382172131147542, + "y": 4.076 }, "prevControl": null, "nextControl": { - "x": 1.9587065217391295, - "y": 4.113532608695653 + "x": 2.1564237348538837, + "y": 4.3107826086956536 }, "isLocked": false, "linkedName": "Rmidalg" diff --git a/src/main/deploy/pathplanner/paths/MGr_B.path b/src/main/deploy/pathplanner/paths/MGr_B.path index 94be291..e629273 100644 --- a/src/main/deploy/pathplanner/paths/MGr_B.path +++ b/src/main/deploy/pathplanner/paths/MGr_B.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.54475, + "x": 2.7811475409836066, "y": 3.9762499999999994 }, "prevControl": { - "x": 1.9651304347826084, + "x": 2.201527975766215, "y": 3.9762499999999994 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/RGr_B.path b/src/main/deploy/pathplanner/paths/RGr_B.path index 0f70a6a..27e601f 100644 --- a/src/main/deploy/pathplanner/paths/RGr_B.path +++ b/src/main/deploy/pathplanner/paths/RGr_B.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 1.5014999999999998, - "y": 2.5625 + "x": 1.474487704918033, + "y": 2.9640881147540985 }, "prevControl": null, "nextControl": { - "x": 1.9343804347826086, - "y": 3.303532608695653 + "x": 1.9073681397006417, + "y": 3.7051207234497516 }, "isLocked": false, "linkedName": "Lrightalg" }, { "anchor": { - "x": 2.54475, + "x": 2.7811475409836066, "y": 3.9762499999999994 }, "prevControl": { - "x": 1.811054347826087, + "x": 2.0474518888096935, "y": 3.902880434782608 }, "nextControl": null, diff --git a/src/main/java/frc/robot/drivetrain/commands/DrivetrainCommands.java b/src/main/java/frc/robot/drivetrain/commands/DrivetrainCommands.java index c403769..fc74efc 100644 --- a/src/main/java/frc/robot/drivetrain/commands/DrivetrainCommands.java +++ b/src/main/java/frc/robot/drivetrain/commands/DrivetrainCommands.java @@ -1,10 +1,55 @@ package frc.robot.drivetrain.commands; +import com.ctre.phoenix6.swerve.SwerveModule; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.drivetrain.CommandSwerveDrivetrain; public class DrivetrainCommands { public static Command drive(double velocityX, double velocityY, double velocityRotational) { return new TeleopDrive(velocityX, velocityY, velocityRotational); } + + public static Command turn180InPlaceCommand() { + return new Command() { + private final PIDController thetaController = new PIDController(0.01, 0.0, 0); + private double targetAngle; + + { + addRequirements(CommandSwerveDrivetrain.getInstance()); + thetaController.enableContinuousInput(-180.0, 180.0); + } + + @Override + public void initialize() { + double currentAngle = CommandSwerveDrivetrain.getInstance().getPigeon2().getAngle(); + targetAngle = Math.IEEEremainder(currentAngle + 180.0, 360.0); + thetaController.setSetpoint(targetAngle); + } + + @Override + public void execute() { + double currentAngle = CommandSwerveDrivetrain.getInstance().getPigeon2().getAngle(); + double rotationRate = thetaController.calculate(currentAngle); + ChassisSpeeds speeds = new ChassisSpeeds(0.0, 0.0, rotationRate); + SwerveRequest.ApplyFieldSpeeds applyFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds() + .withDriveRequestType(SwerveModule.DriveRequestType.Velocity); + CommandSwerveDrivetrain.getInstance().setControl(applyFieldSpeeds.withSpeeds(speeds)); + } + + @Override + public boolean isFinished() { + return thetaController.atSetpoint(); + } + + @Override + public void end(boolean interrupted) { + CommandSwerveDrivetrain.getInstance().stopSwerve();; + } + }; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/operator/OperatorXbox.java b/src/main/java/frc/robot/operator/OperatorXbox.java index 121c241..3edc18f 100644 --- a/src/main/java/frc/robot/operator/OperatorXbox.java +++ b/src/main/java/frc/robot/operator/OperatorXbox.java @@ -23,6 +23,7 @@ import frc.robot.climber.commands.SetClimberAngle; import frc.robot.commands.RobotCommands; import frc.robot.driver.DriverXbox; +import frc.robot.drivetrain.commands.DrivetrainCommands; import frc.robot.elevator.ElevatorSubsystem; import frc.robot.elevator.ElevatorSubsystem.State; import frc.robot.elevator.commands.SetElevatorState; @@ -231,6 +232,7 @@ public void setupTeleopButtons() { /*TEsting Bindings */ + // controller.rightStick().whileTrue(DrivetrainCommands.turn180InPlaceCommand()); // controller.leftBumper().whileTrue(IndexerCommands.setOutput(() -> -1.0)); // // controller.leftTrigger().whileTrue(RobotCommands.primeShoot()); diff --git a/src/main/java/frc/robot/vision/commands/AutoAlign.java b/src/main/java/frc/robot/vision/commands/AutoAlign.java index e829f94..0642150 100644 --- a/src/main/java/frc/robot/vision/commands/AutoAlign.java +++ b/src/main/java/frc/robot/vision/commands/AutoAlign.java @@ -246,7 +246,7 @@ public void execute() { SmartDashboard.putBoolean("requesting lineup left branch - AutoAlign", isLeftAlign); SmartDashboard.putString("nearest ReefFace accessed - AutoAlign", nearestReefFace.name()); if(isElevatorL4 == true) { - System.out.println("REACHED ELEVATOR L4 IF STATEMENT IN AUTOALIGN"); + // System.out.println("REACHED ELEVATOR L4 IF STATEMENT IN AUTOALIGN"); ReefFace newReefFace = updateReefFace(nearestReefFace); SmartDashboard.putString("updated ReefFace - AutoAlign", newReefFace.name()); goalPose2d = new Pose2d(newReefFace.aprilTagX, newReefFace.aprilTagY, Rotation2d.fromDegrees(newReefFace.aprilTagTheta));