From b7a2b8a3a565447694f3d2aa597a1e8d5ca82b61 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Mon, 10 Mar 2025 20:01:01 -0600 Subject: [PATCH 01/17] align: Add dual PIDs --- .../blackknights/commands/AlignCommand.java | 83 +++++++++++++++---- 1 file changed, 67 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 08eb67b..55070d2 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -20,8 +20,6 @@ public class AlignCommand extends Command { private static final Logger LOGGER = LogManager.getLogger(); private final SwerveSubsystem swerveSubsystem; - // private final Controller controller; - private final ProfiledPIDController xAxisPid = new ProfiledPIDController( AlignConstants.X_AXIS_P, @@ -43,6 +41,21 @@ public class AlignCommand extends Command { AlignConstants.ROTATION_D, AlignConstants.ROTATION_CONSTRAINTS); + /// Infinite acceleration PIDs + private final ProfiledPIDController xAxisInfPid = + new ProfiledPIDController( + AlignConstants.X_AXIS_P, + AlignConstants.X_AXIS_I, + AlignConstants.X_AXIS_D, + AlignConstants.X_AXIS_CONSTRAINTS); + + private final ProfiledPIDController yAxisInfPid = + new ProfiledPIDController( + AlignConstants.Y_AXIS_P, + AlignConstants.Y_AXIS_I, + AlignConstants.Y_AXIS_D, + AlignConstants.Y_AXIS_CONSTRAINTS); + private final Odometry odometry = Odometry.getInstance(); private final ConfigManager configManager = ConfigManager.getInstance(); @@ -72,12 +85,10 @@ public class AlignCommand extends Command { */ public AlignCommand( SwerveSubsystem swerveSubsystem, - // Controller controller, Supplier poseSupplier, boolean stopWhenFinished, String profile) { this.swerveSubsystem = swerveSubsystem; - // this.controller = controller; this.pose2dSupplier = poseSupplier; this.stopWhenFinished = stopWhenFinished; this.profile = profile; @@ -112,14 +123,23 @@ public void initialize() { this.yAxisPid.setP(configManager.get("align_y_axis_p", 3)); this.rotationPid.setP(configManager.get("align_rot_p", 7.3)); + this.xAxisInfPid.setP(configManager.get("align_x_axis_p", 3)); + this.yAxisInfPid.setP(configManager.get("align_y_axis_p", 3)); + this.xAxisPid.setD(configManager.get("align_x_axis_d", .25)); this.yAxisPid.setD(configManager.get("align_y_axis_d", .25)); this.rotationPid.setD(configManager.get("align_rot_d", 0.5)); + this.xAxisInfPid.setD(configManager.get("align_x_axis_d", .25)); + this.yAxisInfPid.setD(configManager.get("align_y_axis_d", .25)); + this.xAxisPid.setI(configManager.get("align_x_axis_i", 0.0)); this.yAxisPid.setI(configManager.get("align_y_axis_i", 0.0)); this.rotationPid.setI(configManager.get("align_rot_i", 0.0)); + this.xAxisInfPid.setI(configManager.get("align_x_axis_i", 0.0)); + this.yAxisInfPid.setI(configManager.get("align_y_axis_i", 0.0)); + this.xAxisPid.setConstraints( new TrapezoidProfile.Constraints( configManager.get(String.format("align_%s_x_max_vel_m", this.profile), 3.0), @@ -141,6 +161,16 @@ public void initialize() { String.format("align_%s_rot_max_accel_degps", this.profile), 360)))); + this.xAxisInfPid.setConstraints( + new TrapezoidProfile.Constraints( + configManager.get(String.format("align_%s_x_max_vel_m", this.profile), 3.0), + Double.MAX_VALUE)); + this.yAxisInfPid.setConstraints( + new TrapezoidProfile.Constraints( + configManager.get(String.format("align_%s_y_max_vel_m", this.profile), 3.0), + Double.MAX_VALUE)); + + this.xAxisPid.setTolerance( configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); this.yAxisPid.setTolerance( @@ -150,6 +180,10 @@ public void initialize() { configManager.get( String.format("align_%s_rotation_tolerance", this.profile), 1))); + this.xAxisInfPid.setTolerance(0.0); + this.yAxisInfPid.setTolerance(0.0); + + // Reset All pids this.xAxisPid.reset( robotPose.getX(), swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); @@ -160,29 +194,51 @@ public void initialize() { robotPose.getRotation().getZ(), swerveSubsystem.getFieldRelativeChassisSpeeds().omegaRadiansPerSecond); + this.xAxisInfPid.reset( + robotPose.getX(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); + this.yAxisInfPid.reset( + robotPose.getY(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond); + this.xAxisPid.setGoal(robotPose.getX()); this.yAxisPid.setGoal(robotPose.getY()); this.rotationPid.setGoal(robotPose.getRotation().getZ()); + this.xAxisInfPid.setGoal(robotPose.getX()); + this.yAxisInfPid.setGoal(robotPose.getY()); + this.xAxisPid.calculate(robotPose.getX()); this.yAxisPid.calculate(robotPose.getY()); this.rotationPid.calculate(robotPose.getRotation().getZ()); + this.xAxisInfPid.calculate(robotPose.getX()); + this.yAxisInfPid.calculate(robotPose.getY()); + this.xAxisPid.setGoal(targetPos.getX()); this.yAxisPid.setGoal(targetPos.getY()); this.rotationPid.setGoal(targetPos.getRotation().getRadians()); + + this.xAxisInfPid.setGoal(targetPos.getX()); + this.yAxisInfPid.setGoal(targetPos.getY()); } @Override public void execute() { - // Set PIDs to target - Pose3d robotPose = odometry.getRobotPose(); + double distToTarget = Math.sqrt( + Math.pow(xAxisPid.getPositionError(), 2) + Math.pow(yAxisPid.getPositionError(), 2) + ); + double xAxisCalc = this.xAxisPid.calculate(robotPose.getX()); double yAxisCalc = this.yAxisPid.calculate(robotPose.getY()); double rotationPidCalc = this.rotationPid.calculate(robotPose.getRotation().getZ()); + double infX = xAxisInfPid.calculate(robotPose.getX()); + double infY = yAxisInfPid.calculate(robotPose.getY()); + + debug.setEntry("X Pid Error", this.xAxisPid.getPositionError()); debug.setEntry("Y Pid Error", this.yAxisPid.getPositionError()); debug.setEntry("Rot Pid Error", this.rotationPid.getPositionError()); @@ -209,6 +265,11 @@ public void execute() { ? 0 : Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1)); + if (distToTarget < ConfigManager.getInstance().get("inf_switch_dist", 1.0) && !stopWhenFinished) { + finalX = infX; + finalY = infY; + } + debug.setEntry("Xms", finalX); debug.setEntry("Yms", finalY); debug.setEntry("Rrads", rotationPidCalc); @@ -221,17 +282,7 @@ public void execute() { this.targetPos.getRotation().getRadians() }); - // if (controller.hasStickInput(0.03) || !odometry.hasSeenTarget()) { - // swerveSubsystem.drive( - // controller.getLeftX(), - // controller.getLeftY(), - // controller.getRightX(), - // true, - // true, - // false); - // } else { swerveSubsystem.drive(finalX, finalY, rotationPidCalc, true, true, true); - // } if (xAxisPid.atGoal() && yAxisPid.atGoal() && rotationPid.atGoal() && this.doUpdate) { LOGGER.debug("Hit goal, waiting for time to expire"); From 0ad042cd66adb8a72e292e0800c2129f126433b6 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Mon, 10 Mar 2025 20:08:18 -0600 Subject: [PATCH 02/17] misc: Add tuning backup --- tuning/backups/tuning_2025_03_Mon:20:05:39.json | 1 + tuning/fetch_tuning.sh | 3 +++ 2 files changed, 4 insertions(+) create mode 100755 tuning/backups/tuning_2025_03_Mon:20:05:39.json create mode 100755 tuning/fetch_tuning.sh diff --git a/tuning/backups/tuning_2025_03_Mon:20:05:39.json b/tuning/backups/tuning_2025_03_Mon:20:05:39.json new file mode 100755 index 0000000..65d058f --- /dev/null +++ b/tuning/backups/tuning_2025_03_Mon:20:05:39.json @@ -0,0 +1 @@ +{"align_enable_slow_mode":false,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.3,"align_rough_rotation_tolerance":10,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":3.0,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":4.0,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":3.5,"arm_intake":0.64,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_dist_back":0.72,"swerve_drive_p":0.04,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.5,"arm_max_vel_degs":90.0,"scoring_left_y":0.21,"align_fine_pos_tolerance":0.05,"l4_elevator_position":0.0,"align_x_d":0.25,"controller_deadband":0.2,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.2,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":3.5,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":25.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.875,"elevator_l4":1.64,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.0,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.82,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"arm_ks":0.0,"align_fine_finish_time":400.0,"elevator_max_vel":5.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.0,"placeholder":0.0,"arm_kg":0.1,"scoring_right_x":0.5,"scoring_right_y":-0.13,"arm_encoder_offset":1.679,"elevator_intake":0.14,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":0.6,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.636,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":200.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":360,"align_rough_x_max_vel_m":4.0} diff --git a/tuning/fetch_tuning.sh b/tuning/fetch_tuning.sh new file mode 100755 index 0000000..514820f --- /dev/null +++ b/tuning/fetch_tuning.sh @@ -0,0 +1,3 @@ +#!/usr/bin/env bash + +scp admin@10.20.36.2:/home/lvuser/deploy/tuning.json backups/tuning_$(date +%Y_%m_%a:%T).json From e02087a27971fe644a444d206c0f8ef80ca09d1b Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Thu, 13 Mar 2025 17:30:52 -0600 Subject: [PATCH 03/17] vision: tune offsets --- .../blackknights/commands/AlignCommand.java | 17 ++++++++------- .../constants/VisionConstants.java | 21 +++++++++++++++++-- .../java/org/blackknights/utils/Camera.java | 4 ++-- 3 files changed, 31 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 55070d2..89cda98 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -170,7 +170,6 @@ public void initialize() { configManager.get(String.format("align_%s_y_max_vel_m", this.profile), 3.0), Double.MAX_VALUE)); - this.xAxisPid.setTolerance( configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); this.yAxisPid.setTolerance( @@ -227,9 +226,10 @@ public void initialize() { public void execute() { Pose3d robotPose = odometry.getRobotPose(); - double distToTarget = Math.sqrt( - Math.pow(xAxisPid.getPositionError(), 2) + Math.pow(yAxisPid.getPositionError(), 2) - ); + double distToTarget = + Math.sqrt( + Math.pow(xAxisPid.getPositionError(), 2) + + Math.pow(yAxisPid.getPositionError(), 2)); double xAxisCalc = this.xAxisPid.calculate(robotPose.getX()); double yAxisCalc = this.yAxisPid.calculate(robotPose.getY()); @@ -238,6 +238,7 @@ public void execute() { double infX = xAxisInfPid.calculate(robotPose.getX()); double infY = yAxisInfPid.calculate(robotPose.getY()); + debug.setEntry("Dist to target (Error)", distToTarget); debug.setEntry("X Pid Error", this.xAxisPid.getPositionError()); debug.setEntry("Y Pid Error", this.yAxisPid.getPositionError()); @@ -265,9 +266,11 @@ public void execute() { ? 0 : Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1)); - if (distToTarget < ConfigManager.getInstance().get("inf_switch_dist", 1.0) && !stopWhenFinished) { - finalX = infX; - finalY = infY; + if (distToTarget < ConfigManager.getInstance().get("inf_switch_dist", 1.0) + && !stopWhenFinished) { + LOGGER.info("Using inf pid"); + finalX = infX; + finalY = infY; } debug.setEntry("Xms", finalX); diff --git a/src/main/java/org/blackknights/constants/VisionConstants.java b/src/main/java/org/blackknights/constants/VisionConstants.java index 4e94d5c..204e7ef 100644 --- a/src/main/java/org/blackknights/constants/VisionConstants.java +++ b/src/main/java/org/blackknights/constants/VisionConstants.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import org.blackknights.utils.ConfigManager; /** Vision related constants */ public class VisionConstants { @@ -10,8 +11,24 @@ public class VisionConstants { public static double VISION_TRUST = 0.5; public static final Transform3d LEFT_CAM_TRANSFORM = - new Transform3d(0.235, 0.36, 0.16, new Rotation3d(0.0, 0.0, Math.toRadians(-13))); + new Transform3d( + 0.253, + 0.336, + 0.229, + new Rotation3d( + 0.0, + 0.0, + Math.toRadians( + ConfigManager.getInstance().get("left_cam_angle", -10.0)))); public static final Transform3d RIGHT_CAM_TRANSFORM = - new Transform3d(0.235, -0.36, 0.16, new Rotation3d(0.0, 0.0, Math.toRadians(10))); + new Transform3d( + 0.253, + -0.3995, + 0.229, + new Rotation3d( + 0.0, + 0.0, + Math.toRadians( + ConfigManager.getInstance().get("right_cam_angle", 10.0)))); } diff --git a/src/main/java/org/blackknights/utils/Camera.java b/src/main/java/org/blackknights/utils/Camera.java index 4ff5c7c..05212c4 100644 --- a/src/main/java/org/blackknights/utils/Camera.java +++ b/src/main/java/org/blackknights/utils/Camera.java @@ -22,10 +22,10 @@ public class Camera { private Optional photonCamera; private Optional photonPoseEstimator; - private Transform3d camOffset; + private final Transform3d camOffset; private double photonTimestamp; private Pose3d targetPose; - private String name; + private final String name; private final CameraType camType; From b905e814f950d3fdeb6e05939c54778065b21908 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sat, 15 Mar 2025 12:44:56 -0600 Subject: [PATCH 04/17] misc: random amp stuff idk --- src/main/deploy/pathplanner/navgrid.json | 1 - src/main/deploy/pathplanner/settings.json | 32 ------------------- .../blackknights/commands/AlignCommand.java | 11 ++++--- .../backups/tuning_2025_03_Fri:21:16:57.json | 1 + 4 files changed, 7 insertions(+), 38 deletions(-) delete mode 100644 src/main/deploy/pathplanner/navgrid.json delete mode 100644 src/main/deploy/pathplanner/settings.json create mode 100755 tuning/backups/tuning_2025_03_Fri:21:16:57.json diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json deleted file mode 100644 index 23e0db9..0000000 --- a/src/main/deploy/pathplanner/navgrid.json +++ /dev/null @@ -1 +0,0 @@ -{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json deleted file mode 100644 index 8e6437a..0000000 --- a/src/main/deploy/pathplanner/settings.json +++ /dev/null @@ -1,32 +0,0 @@ -{ - "robotWidth": 0.837, - "robotLength": 0.837, - "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 4.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, - "defaultNominalVoltage": 12.0, - "robotMass": 50.0, - "robotMOI": 4.839, - "robotTrackwidth": 0.546, - "driveWheelRadius": 0.038, - "driveGearing": 5.08, - "maxDriveSpeed": 5.0, - "driveMotorType": "vortex", - "driveCurrentLimit": 40.0, - "wheelCOF": 0.5, - "flModuleX": 0.32, - "flModuleY": 0.32, - "frModuleX": 0.32, - "frModuleY": -0.32, - "blModuleX": -0.32, - "blModuleY": 0.32, - "brModuleX": -0.32, - "brModuleY": -0.32, - "bumperOffsetX": 0.0, - "bumperOffsetY": 0.0, - "robotFeatures": [] -} \ No newline at end of file diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 89cda98..eda61b5 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -227,9 +227,8 @@ public void execute() { Pose3d robotPose = odometry.getRobotPose(); double distToTarget = - Math.sqrt( - Math.pow(xAxisPid.getPositionError(), 2) - + Math.pow(yAxisPid.getPositionError(), 2)); + Math.pow(robotPose.getX() - targetPos.getX(), 2) + + Math.pow(robotPose.getY() - targetPos.getY(), 2); double xAxisCalc = this.xAxisPid.calculate(robotPose.getX()); double yAxisCalc = this.yAxisPid.calculate(robotPose.getY()); @@ -244,6 +243,9 @@ public void execute() { debug.setEntry("Y Pid Error", this.yAxisPid.getPositionError()); debug.setEntry("Rot Pid Error", this.rotationPid.getPositionError()); + debug.setEntry("X Pid Error (inf)", this.xAxisInfPid.getPositionError()); + debug.setEntry("Y Pid Error (inf)", this.yAxisInfPid.getPositionError()); + debug.setEntry("X Pid setpoint", this.xAxisPid.atSetpoint()); debug.setEntry("X Pid goal", this.xAxisPid.atGoal()); @@ -266,9 +268,8 @@ public void execute() { ? 0 : Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1)); - if (distToTarget < ConfigManager.getInstance().get("inf_switch_dist", 1.0) + if (distToTarget < Math.pow(ConfigManager.getInstance().get("inf_switch_dist", 1.0), 2) && !stopWhenFinished) { - LOGGER.info("Using inf pid"); finalX = infX; finalY = infY; } diff --git a/tuning/backups/tuning_2025_03_Fri:21:16:57.json b/tuning/backups/tuning_2025_03_Fri:21:16:57.json new file mode 100755 index 0000000..7faffa1 --- /dev/null +++ b/tuning/backups/tuning_2025_03_Fri:21:16:57.json @@ -0,0 +1 @@ +{"align_enable_slow_mode":false,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.1,"driver_max_speed":3.5,"align_rough_rotation_tolerance":10,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.3,"inf_switch_dist":2.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.5,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":5.0,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.0,"arm_intake":0.64,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_dist_back":0.72,"swerve_drive_p":0.04,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.5,"arm_max_vel_degs":90.0,"scoring_left_y":0.21,"align_fine_pos_tolerance":0.03,"l4_elevator_position":0.0,"align_x_d":0.25,"controller_deadband":0.2,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.25,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":4.0,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"driver_max_speed_rot":360,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":65.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.95,"elevator_l4":1.64,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.5,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.82,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"arm_ks":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":5.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.5,"placeholder":0.0,"arm_kg":0.1,"scoring_right_x":0.5,"scoring_right_y":-0.16,"arm_encoder_offset":1.679,"elevator_intake":0.14,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"right_cam_angle":7.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":0.6,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.636,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":1.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":520,"align_rough_x_max_vel_m":5.0} From a7a9edecb7a018a685a26557f5f489fe1bc719dc Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sat, 15 Mar 2025 17:52:34 -0600 Subject: [PATCH 05/17] align: Alignment tuning --- .../blackknights/commands/AlignCommand.java | 64 +++++++++++++++---- 1 file changed, 50 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index eda61b5..7b44847 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -71,6 +71,12 @@ public class AlignCommand extends Command { private double timeSenseFinished = -1; private boolean doUpdate = true; + private double distToTarget = Double.MAX_VALUE; + + private double finalX = 0.0; + private double finalY = 0.0; + private boolean log = true; + /** * Align to a fieldspace position with odometry * @@ -114,6 +120,10 @@ public void initialize() { this.targetPos = pose2dSupplier.get(); this.timeSenseFinished = -1; this.doUpdate = true; + this.distToTarget = Double.MAX_VALUE; + this.finalX = 0.0; + this.finalY = 0.0; + this.log = true; LOGGER.info("Initializing AlignCommand"); Pose3d robotPose = odometry.getRobotPose(); @@ -164,11 +174,11 @@ public void initialize() { this.xAxisInfPid.setConstraints( new TrapezoidProfile.Constraints( configManager.get(String.format("align_%s_x_max_vel_m", this.profile), 3.0), - Double.MAX_VALUE)); + 5000)); this.yAxisInfPid.setConstraints( new TrapezoidProfile.Constraints( configManager.get(String.format("align_%s_y_max_vel_m", this.profile), 3.0), - Double.MAX_VALUE)); + 5000)); this.xAxisPid.setTolerance( configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); @@ -179,8 +189,10 @@ public void initialize() { configManager.get( String.format("align_%s_rotation_tolerance", this.profile), 1))); - this.xAxisInfPid.setTolerance(0.0); - this.yAxisInfPid.setTolerance(0.0); + this.xAxisPid.setTolerance( + configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); + this.yAxisPid.setTolerance( + configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); // Reset All pids this.xAxisPid.reset( @@ -226,7 +238,7 @@ public void initialize() { public void execute() { Pose3d robotPose = odometry.getRobotPose(); - double distToTarget = + this.distToTarget = Math.pow(robotPose.getX() - targetPos.getX(), 2) + Math.pow(robotPose.getY() - targetPos.getY(), 2); @@ -257,12 +269,18 @@ public void execute() { debug.setEntry("Robot rotation: ", robotPose.getRotation().getZ()); debug.setEntry("Rot setpoint", this.rotationPid.getSetpoint().position); - double finalX = + debug.setEntry( + "Rot diff", + Math.abs( + Math.abs(this.targetPos.getRotation().getRadians()) + - Math.abs(odometry.getRobotPose().getRotation().getZ()))); + + this.finalX = xAxisCalc + (xAxisPid.atGoal() || xAxisPid.atSetpoint() ? 0 : Math.signum(xAxisCalc) * configManager.get("align_ff", 0.1)); - double finalY = + this.finalY = yAxisCalc + (yAxisPid.atGoal() || yAxisPid.atSetpoint() ? 0 @@ -270,8 +288,13 @@ public void execute() { if (distToTarget < Math.pow(ConfigManager.getInstance().get("inf_switch_dist", 1.0), 2) && !stopWhenFinished) { - finalX = infX; - finalY = infY; + this.finalX = infX; + this.finalY = infY; + } + + if (log) { + log = false; + LOGGER.info("First commanded speeds: {} {}", xAxisCalc, xAxisCalc); } debug.setEntry("Xms", finalX); @@ -288,8 +311,8 @@ public void execute() { swerveSubsystem.drive(finalX, finalY, rotationPidCalc, true, true, true); - if (xAxisPid.atGoal() && yAxisPid.atGoal() && rotationPid.atGoal() && this.doUpdate) { - LOGGER.debug("Hit goal, waiting for time to expire"); + if (checkAtGoal() && doUpdate) { + LOGGER.info("Hit goal, waiting for time to expire"); this.timeSenseFinished = Timer.getFPGATimestamp() * 1000; this.doUpdate = false; } @@ -297,9 +320,7 @@ public void execute() { @Override public boolean isFinished() { - return xAxisPid.atGoal() - && yAxisPid.atGoal() - && rotationPid.atGoal() + return checkAtGoal() && Timer.getFPGATimestamp() * 1000 - this.timeSenseFinished > configManager.get( String.format("align_%s_finish_time", this.profile), 200.0); @@ -308,5 +329,20 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { if (stopWhenFinished) swerveSubsystem.zeroVoltage(); + else swerveSubsystem.drive(this.finalX, this.finalY, 0.0, true, true, true); + LOGGER.info("Final commanded speeds: {} {}", this.finalX, this.finalY); + } + + private boolean checkAtGoal() { + return (stopWhenFinished && xAxisPid.atGoal() && yAxisPid.atGoal() && rotationPid.atGoal()) + || (!stopWhenFinished + && this.distToTarget + <= Math.pow( + configManager.get( + String.format( + "align_%s_pos_dist_tol", this.profile), + 0.05), + 2) + && rotationPid.atGoal()); } } From 04578648c19b72bdc753234b5fc06f4952ffd37f Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sun, 16 Mar 2025 16:31:46 -0600 Subject: [PATCH 06/17] misc: All code after 1619 --- .../java/org/blackknights/RobotContainer.java | 103 +++++++++++++----- .../blackknights/commands/AlignCommand.java | 50 ++++++--- .../blackknights/commands/BaseCommand.java | 4 +- .../blackknights/commands/IntakeCommand.java | 14 ++- .../constants/VisionConstants.java | 11 ++ 5 files changed, 136 insertions(+), 46 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index c9c5d57..2ba4d84 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.*; @@ -47,6 +48,12 @@ public class RobotContainer { Camera.CameraType.PHOTONVISION, VisionConstants.RIGHT_CAM_TRANSFORM); + private final Camera centerCam = + new Camera( + "centerCam", + Camera.CameraType.PHOTONVISION, + VisionConstants.CENTER_CAM_TRANSFORM); + private final Odometry odometry = Odometry.getInstance(); // Auto Chooser SendableChooser superSecretMissileTech = new SendableChooser<>(); @@ -125,20 +132,35 @@ private void configureBindings() { primaryController .rightBumper() .whileTrue( - new ParallelCommandGroup( - new DriveCommands( - swerveSubsystem, - primaryController::getLeftY, - primaryController::getLeftX, - () -> -primaryController.getRightX() * Math.PI, - true, - true), - new ElevatorArmCommand( - elevatorSubsystem, - armSubsystem, - () -> ScoringConstants.ScoringHeights.INTAKE), - new IntakeCommand( - intakeSubsystem, IntakeCommand.IntakeMode.INTAKE))); + new SequentialCommandGroup( + new ParallelRaceGroup( + new DriveCommands( + swerveSubsystem, + primaryController::getLeftY, + primaryController::getLeftX, + () -> -primaryController.getRightX() * Math.PI, + true, + true), + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.INTAKE), + new IntakeCommand( + intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)), + new RunCommand( + () -> + swerveSubsystem.drive( + ConfigManager.getInstance() + .get("back_mps", -1.0), + 0.0, + 0.0, + false, + false, + false), + swerveSubsystem) + .withTimeout( + ConfigManager.getInstance() + .get("back_time_sec", 0.2)))); elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem)); @@ -206,6 +228,7 @@ private void configureBindings() { public void robotInit() { odometry.addCamera(leftCam); odometry.addCamera(rightCam); + odometry.addCamera(centerCam); } /** Runs every 20ms while the robot is on */ @@ -253,6 +276,7 @@ public Command getAutonomousCommand() { private Command getPlaceCommand( Supplier currentSupplier, Supplier nextSupplier) { + return new SequentialCommandGroup( new ParallelRaceGroup( new AlignCommand( @@ -290,7 +314,23 @@ private Command getPlaceCommand( armSubsystem, () -> nextSupplier.get().getHeight()), new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE) - .withTimeout(1))); + .withTimeout(1)), + new ParallelRaceGroup( + new AutoEndCommand(), + new BaseCommand(elevatorSubsystem, armSubsystem), + new RunCommand( + () -> + swerveSubsystem.drive( + ConfigManager.getInstance() + .get("back_mps", -1.0), + 0.0, + 0.0, + false, + false, + false), + swerveSubsystem) + .withTimeout( + ConfigManager.getInstance().get("back_time_sec", 0.2)))); } /** @@ -315,22 +355,13 @@ private Command getAutoIntakeCommand(IntakeSides side) { intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI))); return new ParallelRaceGroup( - new SequentialCommandGroup( - new AlignCommand( - swerveSubsystem, - () -> - AlignUtils.getXDistBack( - intakePoseFinal, - ConfigManager.getInstance() - .get("autointake_first_back", 1.0)), - false, - "auto"), - new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "fine")), + new ParallelCommandGroup( + new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "rough"), + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)), new ElevatorArmCommand( elevatorSubsystem, armSubsystem, - () -> ScoringConstants.ScoringHeights.INTAKE), - new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)); + () -> ScoringConstants.ScoringHeights.INTAKE)); } private static Pose2d getPose2d(IntakeSides side) { @@ -356,4 +387,20 @@ private enum IntakeSides { LEFT, RIGHT } + + private class AutoEndCommand extends Command { + private double currTime = Timer.getFPGATimestamp() * 1000; + + @Override + public void initialize() { + this.currTime = Timer.getFPGATimestamp() * 1000; + } + + @Override + public boolean isFinished() { + return DriverStation.isAutonomous() + && Timer.getFPGATimestamp() * 1000 - this.currTime + > ConfigManager.getInstance().get("auto_place_backup_time_ms", 0.2); + } + } } diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 7b44847..10ae692 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -110,8 +110,41 @@ public AlignCommand( configManager.get( String.format("align_%s_rotation_tolerance", this.profile), 1))); + Pose3d robotPose = Odometry.getInstance().getRobotPose(); + this.rotationPid.enableContinuousInput(-Math.PI, Math.PI); + this.xAxisPid.reset( + robotPose.getX(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); + this.yAxisPid.reset( + robotPose.getY(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond); + this.rotationPid.reset( + robotPose.getRotation().getZ(), + swerveSubsystem.getFieldRelativeChassisSpeeds().omegaRadiansPerSecond); + + this.xAxisInfPid.reset( + robotPose.getX(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); + this.yAxisInfPid.reset( + robotPose.getY(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond); + + this.xAxisPid.setGoal(robotPose.getX()); + this.yAxisPid.setGoal(robotPose.getY()); + this.rotationPid.setGoal(robotPose.getRotation().getZ()); + + this.xAxisInfPid.setGoal(robotPose.getX()); + this.yAxisInfPid.setGoal(robotPose.getY()); + + this.xAxisPid.calculate(robotPose.getX()); + this.yAxisPid.calculate(robotPose.getY()); + this.rotationPid.calculate(robotPose.getRotation().getZ()); + + this.xAxisInfPid.calculate(robotPose.getX()); + this.yAxisInfPid.calculate(robotPose.getY()); + addRequirements(swerveSubsystem); } @@ -212,20 +245,6 @@ public void initialize() { robotPose.getY(), swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond); - this.xAxisPid.setGoal(robotPose.getX()); - this.yAxisPid.setGoal(robotPose.getY()); - this.rotationPid.setGoal(robotPose.getRotation().getZ()); - - this.xAxisInfPid.setGoal(robotPose.getX()); - this.yAxisInfPid.setGoal(robotPose.getY()); - - this.xAxisPid.calculate(robotPose.getX()); - this.yAxisPid.calculate(robotPose.getY()); - this.rotationPid.calculate(robotPose.getRotation().getZ()); - - this.xAxisInfPid.calculate(robotPose.getX()); - this.yAxisInfPid.calculate(robotPose.getY()); - this.xAxisPid.setGoal(targetPos.getX()); this.yAxisPid.setGoal(targetPos.getY()); this.rotationPid.setGoal(targetPos.getRotation().getRadians()); @@ -286,8 +305,7 @@ public void execute() { ? 0 : Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1)); - if (distToTarget < Math.pow(ConfigManager.getInstance().get("inf_switch_dist", 1.0), 2) - && !stopWhenFinished) { + if (!stopWhenFinished) { this.finalX = infX; this.finalY = infY; } diff --git a/src/main/java/org/blackknights/commands/BaseCommand.java b/src/main/java/org/blackknights/commands/BaseCommand.java index 6c6c191..b87985e 100644 --- a/src/main/java/org/blackknights/commands/BaseCommand.java +++ b/src/main/java/org/blackknights/commands/BaseCommand.java @@ -32,7 +32,9 @@ public void initialize() { @Override public void execute() { armSubsystem.setPivotAngle(ConfigManager.getInstance().get("arm_base_angle", 0.1)); - if (armSubsystem.getPivotAngle() <= -Math.PI / 4 || armSubsystem.getPivotAngle() >= 0.2) { + if (armSubsystem.getPivotAngle() <= -Math.PI / 4 + || armSubsystem.getPivotAngle() + >= ConfigManager.getInstance().get("arm_movement_max", 0.2)) { elevatorSubsystem.holdPosition(); } else { elevatorSubsystem.zeroElevator(); diff --git a/src/main/java/org/blackknights/commands/IntakeCommand.java b/src/main/java/org/blackknights/commands/IntakeCommand.java index d9114a5..d509ad4 100644 --- a/src/main/java/org/blackknights/commands/IntakeCommand.java +++ b/src/main/java/org/blackknights/commands/IntakeCommand.java @@ -1,6 +1,7 @@ /* Black Knights Robotics (C) 2025 */ package org.blackknights.commands; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import org.blackknights.subsystems.IntakeSubsystem; import org.blackknights.utils.ConfigManager; @@ -10,6 +11,8 @@ public class IntakeCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final IntakeMode mode; + private double currentTime; + /** * Create a new intake command * @@ -22,6 +25,11 @@ public IntakeCommand(IntakeSubsystem intakeSubsystem, IntakeMode mode) { addRequirements(intakeSubsystem); } + @Override + public void initialize() { + this.currentTime = Timer.getFPGATimestamp() * 1000; + } + @Override public void execute() { switch (mode) { @@ -47,7 +55,11 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak(); + return (mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak()) + || (mode.equals(IntakeMode.OUTTAKE) + && !intakeSubsystem.getLinebreak() + && Timer.getFPGATimestamp() * 1000 - this.currentTime + > ConfigManager.getInstance().get("outtake_wait_time_ms", 40)); } /** Enum of the different intake modes */ diff --git a/src/main/java/org/blackknights/constants/VisionConstants.java b/src/main/java/org/blackknights/constants/VisionConstants.java index 204e7ef..828ca8e 100644 --- a/src/main/java/org/blackknights/constants/VisionConstants.java +++ b/src/main/java/org/blackknights/constants/VisionConstants.java @@ -31,4 +31,15 @@ public class VisionConstants { 0.0, Math.toRadians( ConfigManager.getInstance().get("right_cam_angle", 10.0)))); + + public static final Transform3d CENTER_CAM_TRANSFORM = + new Transform3d( + 0.1, // 0.341122 0.3832 + 0.0, + 0.2040382, + new Rotation3d( + 0.0, + Math.toRadians( + ConfigManager.getInstance().get("center_cam_pitch", 45.0)), + 0.0)); } From e48c30919c25f21c4ba5e777f73443f490cbf104 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Fri, 21 Mar 2025 16:40:24 -0600 Subject: [PATCH 07/17] colorado: Good code --- .../java/org/blackknights/RobotContainer.java | 20 +++++++++---------- .../blackknights/commands/AlignCommand.java | 10 ++++++++++ .../subsystems/ElevatorSubsystem.java | 2 +- .../subsystems/SwerveSubsystem.java | 5 ++++- tuning/backups/colorado_v1.json | 1 + .../backups/tuning_2025_03_17:20:25:13.json | 1 + .../backups/tuning_2025_03_21:10:45:15.json | 1 + tuning/fetch_tuning.sh | 2 +- 8 files changed, 29 insertions(+), 13 deletions(-) create mode 100755 tuning/backups/colorado_v1.json create mode 100755 tuning/backups/tuning_2025_03_17:20:25:13.json create mode 100755 tuning/backups/tuning_2025_03_21:10:45:15.json diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index 2ba4d84..e6a639d 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -76,7 +76,7 @@ public RobotContainer() { superSecretMissileTech.addOption( "LEFT_3", new SequentialCommandGroup( - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4")), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L2")), getAutoIntakeCommand(IntakeSides.LEFT), getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")), getAutoIntakeCommand(IntakeSides.LEFT), @@ -85,11 +85,11 @@ public RobotContainer() { superSecretMissileTech.addOption( "RIGHT_3", new SequentialCommandGroup( - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L4")), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L2")), getAutoIntakeCommand(IntakeSides.RIGHT), - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("6L4")), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("4L4")), getAutoIntakeCommand(IntakeSides.RIGHT), - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("7L4")))); + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("5L4")))); superSecretMissileTech.addOption( "CENTER_LEFT", @@ -355,13 +355,13 @@ private Command getAutoIntakeCommand(IntakeSides side) { intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI))); return new ParallelRaceGroup( + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE), new ParallelCommandGroup( new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "rough"), - new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)), - new ElevatorArmCommand( - elevatorSubsystem, - armSubsystem, - () -> ScoringConstants.ScoringHeights.INTAKE)); + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.INTAKE))); } private static Pose2d getPose2d(IntakeSides side) { @@ -388,7 +388,7 @@ private enum IntakeSides { RIGHT } - private class AutoEndCommand extends Command { + private static class AutoEndCommand extends Command { private double currTime = Timer.getFPGATimestamp() * 1000; @Override diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 10ae692..55061f2 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -12,6 +12,7 @@ import org.blackknights.constants.AlignConstants; import org.blackknights.framework.Odometry; import org.blackknights.subsystems.SwerveSubsystem; +import org.blackknights.utils.AlignUtils; import org.blackknights.utils.ConfigManager; import org.blackknights.utils.NetworkTablesUtils; @@ -251,6 +252,15 @@ public void initialize() { this.xAxisInfPid.setGoal(targetPos.getX()); this.yAxisInfPid.setGoal(targetPos.getY()); + + if (!stopWhenFinished) { + Pose2d fakePose = + AlignUtils.getXDistBack( + robotPose.toPose2d(), + ConfigManager.getInstance().get("fake_pose_dist_back", 0.5)); + this.xAxisPid.calculate(fakePose.getX()); + this.yAxisPid.calculate(fakePose.getY()); + } } @Override diff --git a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java index 553a444..2ea844c 100644 --- a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java @@ -146,7 +146,7 @@ public void setTargetPosition(double position) { elevatorPID.setGoal(position); double pidCalc = elevatorPID.calculate(getElevatorPosition()); - double ffCalc = elevatorFF.calculate(0.0); + double ffCalc = elevatorFF.calculate(elevatorPID.getSetpoint().velocity); // if (Math.abs(this.getLeftEncoderPosition() - this.getRightEncoderPosition()) // > ConfigManager.getInstance().get("max_roation_diff", 1)) { diff --git a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java index 9d7df57..2a2b7ae 100644 --- a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java @@ -255,7 +255,10 @@ public void drive( if (currentTranslationMagnitude != 0.0) { directionSlewRate = Math.abs( - DrivetrainConstants.DIRECTION_SLEW_RATE + ConfigManager.getInstance() + .get( + "drive_direction_slew_rate", + DrivetrainConstants.DIRECTION_SLEW_RATE) / currentTranslationMagnitude); } else { directionSlewRate = 500.0; // super high number means slew is instantaneous diff --git a/tuning/backups/colorado_v1.json b/tuning/backups/colorado_v1.json new file mode 100755 index 0000000..79a4ca6 --- /dev/null +++ b/tuning/backups/colorado_v1.json @@ -0,0 +1 @@ +{"align_enable_slow_mode":false,"back_time_sec":0.0,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":3.5,"align_rough_rotation_tolerance":40,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.5,"inf_switch_dist":2.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":7.5,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.0,"arm_intake":0.7,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.25,"align_dist_back":0.82,"swerve_drive_p":0.04,"outtake_wait_time_ms":280.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.48,"arm_max_vel_degs":90.0,"scoring_left_y":0.195,"align_fine_pos_tolerance":0.05,"l4_elevator_position":0.0,"align_x_d":0.25,"center_cam_pitch":-45.0,"controller_deadband":0.2,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.25,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":4.0,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"driver_max_speed_rot":360,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"back_mps":-3.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.95,"elevator_l4":1.7,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.5,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.5,"placeholder":0.0,"arm_kg":0.0,"scoring_right_x":0.48,"scoring_right_y":-0.14,"arm_encoder_offset":2.556,"elevator_intake":0.1,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"right_cam_angle":7.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":-0.22,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.5,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":0.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":520,"align_rough_x_max_vel_m":7.5} diff --git a/tuning/backups/tuning_2025_03_17:20:25:13.json b/tuning/backups/tuning_2025_03_17:20:25:13.json new file mode 100755 index 0000000..bdf3230 --- /dev/null +++ b/tuning/backups/tuning_2025_03_17:20:25:13.json @@ -0,0 +1 @@ +{"align_enable_slow_mode":false,"back_time_sec":1.0,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":3.5,"align_rough_rotation_tolerance":40,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.5,"inf_switch_dist":2.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":7.5,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.0,"arm_intake":0.7,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.5,"align_dist_back":0.82,"swerve_drive_p":0.04,"outtake_wait_time_ms":200.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.5,"arm_max_vel_degs":90.0,"scoring_left_y":0.18,"align_fine_pos_tolerance":0.05,"l4_elevator_position":0.0,"align_x_d":0.25,"center_cam_pitch":-45.0,"controller_deadband":0.2,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.25,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":4.0,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"driver_max_speed_rot":360,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"back_mps":-3.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.95,"elevator_l4":1.7,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.5,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.5,"placeholder":0.0,"arm_kg":0.11,"scoring_right_x":0.5,"scoring_right_y":-0.2,"arm_encoder_offset":2.556,"elevator_intake":0.1,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"right_cam_angle":7.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":-0.22,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.63,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":0.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":520,"align_rough_x_max_vel_m":7.5} diff --git a/tuning/backups/tuning_2025_03_21:10:45:15.json b/tuning/backups/tuning_2025_03_21:10:45:15.json new file mode 100755 index 0000000..92da67a --- /dev/null +++ b/tuning/backups/tuning_2025_03_21:10:45:15.json @@ -0,0 +1 @@ +{"align_enable_slow_mode":false,"back_time_sec":1.0,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":3.5,"align_rough_rotation_tolerance":40,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.5,"inf_switch_dist":2.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":7.5,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.0,"arm_intake":0.7,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.25,"align_dist_back":0.82,"swerve_drive_p":0.04,"outtake_wait_time_ms":280.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.48,"arm_max_vel_degs":90.0,"scoring_left_y":0.195,"align_fine_pos_tolerance":0.05,"l4_elevator_position":0.0,"align_x_d":0.25,"center_cam_pitch":-45.0,"controller_deadband":0.2,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.25,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":4.0,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"driver_max_speed_rot":360,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"back_mps":-3.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.95,"elevator_l4":1.7,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.5,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.5,"placeholder":0.0,"arm_kg":0.0,"scoring_right_x":0.48,"scoring_right_y":-0.14,"arm_encoder_offset":2.556,"elevator_intake":0.13,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"right_cam_angle":7.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":-0.22,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.55,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":0.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":520,"align_rough_x_max_vel_m":7.5} diff --git a/tuning/fetch_tuning.sh b/tuning/fetch_tuning.sh index 514820f..978928d 100755 --- a/tuning/fetch_tuning.sh +++ b/tuning/fetch_tuning.sh @@ -1,3 +1,3 @@ #!/usr/bin/env bash -scp admin@10.20.36.2:/home/lvuser/deploy/tuning.json backups/tuning_$(date +%Y_%m_%a:%T).json +scp admin@10.20.36.2:/home/lvuser/deploy/tuning.json backups/tuning_$(date +%Y_%m_%d:%T).json From f59f751fa7364395d7457de077cfcc5b732588c1 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sun, 23 Mar 2025 19:29:48 -0600 Subject: [PATCH 08/17] misc: All code after CO --- .../java/org/blackknights/RobotContainer.java | 8 +++++--- .../org/blackknights/commands/AlignCommand.java | 2 +- .../org/blackknights/commands/IntakeCommand.java | 15 +++++++++------ .../org/blackknights/subsystems/ArmSubsystem.java | 15 +++++++++++---- ...025_03_21:10:45:15.json => colorado_v1.1.json} | 0 5 files changed, 26 insertions(+), 14 deletions(-) rename tuning/backups/{tuning_2025_03_21:10:45:15.json => colorado_v1.1.json} (100%) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index e6a639d..85428d7 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -76,7 +76,7 @@ public RobotContainer() { superSecretMissileTech.addOption( "LEFT_3", new SequentialCommandGroup( - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L2")), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4")), getAutoIntakeCommand(IntakeSides.LEFT), getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")), getAutoIntakeCommand(IntakeSides.LEFT), @@ -85,7 +85,7 @@ public RobotContainer() { superSecretMissileTech.addOption( "RIGHT_3", new SequentialCommandGroup( - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L2")), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L4")), getAutoIntakeCommand(IntakeSides.RIGHT), getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("4L4")), getAutoIntakeCommand(IntakeSides.RIGHT), @@ -314,7 +314,9 @@ private Command getPlaceCommand( armSubsystem, () -> nextSupplier.get().getHeight()), new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE) - .withTimeout(1)), + .withTimeout( + ConfigManager.getInstance() + .get("outtake_max_time_sec", 5.0))), new ParallelRaceGroup( new AutoEndCommand(), new BaseCommand(elevatorSubsystem, armSubsystem), diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 55061f2..ade3afb 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -337,7 +337,7 @@ public void execute() { this.targetPos.getRotation().getRadians() }); - swerveSubsystem.drive(finalX, finalY, rotationPidCalc, true, true, true); + swerveSubsystem.drive(finalX, finalY, rotationPidCalc, true, !stopWhenFinished, true); if (checkAtGoal() && doUpdate) { LOGGER.info("Hit goal, waiting for time to expire"); diff --git a/src/main/java/org/blackknights/commands/IntakeCommand.java b/src/main/java/org/blackknights/commands/IntakeCommand.java index d509ad4..52811d0 100644 --- a/src/main/java/org/blackknights/commands/IntakeCommand.java +++ b/src/main/java/org/blackknights/commands/IntakeCommand.java @@ -11,7 +11,7 @@ public class IntakeCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final IntakeMode mode; - private double currentTime; + private double startTime; /** * Create a new intake command @@ -27,7 +27,7 @@ public IntakeCommand(IntakeSubsystem intakeSubsystem, IntakeMode mode) { @Override public void initialize() { - this.currentTime = Timer.getFPGATimestamp() * 1000; + this.startTime = Timer.getFPGATimestamp() * 1000; } @Override @@ -41,8 +41,11 @@ public void execute() { } case OUTTAKE: { - intakeSubsystem.setVoltage( - ConfigManager.getInstance().get("outtake_speed", -8.0)); + if (Timer.getFPGATimestamp() * 1000 - this.startTime + > ConfigManager.getInstance().get("outtake_wait_time_ms", 40)) { + intakeSubsystem.setVoltage( + ConfigManager.getInstance().get("outtake_speed", -8.0)); + } break; } } @@ -58,8 +61,8 @@ public boolean isFinished() { return (mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak()) || (mode.equals(IntakeMode.OUTTAKE) && !intakeSubsystem.getLinebreak() - && Timer.getFPGATimestamp() * 1000 - this.currentTime - > ConfigManager.getInstance().get("outtake_wait_time_ms", 40)); + && Timer.getFPGATimestamp() * 1000 - this.startTime + > ConfigManager.getInstance().get("outtaking_time_ms", 40)); } /** Enum of the different intake modes */ diff --git a/src/main/java/org/blackknights/subsystems/ArmSubsystem.java b/src/main/java/org/blackknights/subsystems/ArmSubsystem.java index 51db177..57029bd 100644 --- a/src/main/java/org/blackknights/subsystems/ArmSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ArmSubsystem.java @@ -69,6 +69,8 @@ public ArmSubsystem() { pivotConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); + + pivotPID.enableContinuousInput(-Math.PI, Math.PI); } /** @@ -107,10 +109,15 @@ public double getPivotAngle() { // ? pivotAbsEncoder.getPosition() - 2 * Math.PI - // ArmConstants.PIVOT_ENCODER_OFFSET // : pivotAbsEncoder.getPosition() - ArmConstants.PIVOT_ENCODER_OFFSET; - return Math.PI * 2 - - pivotAbsEncoder.getPosition() - - ConfigManager.getInstance() - .get("arm_encoder_offset", ArmConstants.PIVOT_ENCODER_OFFSET); + double x = + Math.PI * 2 + - pivotAbsEncoder.getPosition() + - ConfigManager.getInstance() + .get("arm_encoder_offset", ArmConstants.PIVOT_ENCODER_OFFSET); + + if (x >= Math.PI) x -= Math.PI * 2; + + return x; } public double getPivotSpeed() { diff --git a/tuning/backups/tuning_2025_03_21:10:45:15.json b/tuning/backups/colorado_v1.1.json similarity index 100% rename from tuning/backups/tuning_2025_03_21:10:45:15.json rename to tuning/backups/colorado_v1.1.json From 0c58b3f872b73e2818cb0526d67ccefe63576256 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Thu, 27 Mar 2025 20:48:44 -0600 Subject: [PATCH 09/17] align: refactor some stuff --- .../blackknights/commands/AlignCommand.java | 256 +++++------------- .../controllers/MAXSwerveModule.java | 7 +- .../subsystems/SwerveSubsystem.java | 1 + 3 files changed, 68 insertions(+), 196 deletions(-) diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index ade3afb..6d1a204 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -1,6 +1,7 @@ /* Black Knights Robotics (C) 2025 */ package org.blackknights.commands; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -12,7 +13,6 @@ import org.blackknights.constants.AlignConstants; import org.blackknights.framework.Odometry; import org.blackknights.subsystems.SwerveSubsystem; -import org.blackknights.utils.AlignUtils; import org.blackknights.utils.ConfigManager; import org.blackknights.utils.NetworkTablesUtils; @@ -21,20 +21,6 @@ public class AlignCommand extends Command { private static final Logger LOGGER = LogManager.getLogger(); private final SwerveSubsystem swerveSubsystem; - private final ProfiledPIDController xAxisPid = - new ProfiledPIDController( - AlignConstants.X_AXIS_P, - AlignConstants.X_AXIS_I, - AlignConstants.X_AXIS_D, - AlignConstants.X_AXIS_CONSTRAINTS); - - private final ProfiledPIDController yAxisPid = - new ProfiledPIDController( - AlignConstants.Y_AXIS_P, - AlignConstants.Y_AXIS_I, - AlignConstants.Y_AXIS_D, - AlignConstants.Y_AXIS_CONSTRAINTS); - private final ProfiledPIDController rotationPid = new ProfiledPIDController( AlignConstants.ROTATION_P, @@ -42,20 +28,8 @@ public class AlignCommand extends Command { AlignConstants.ROTATION_D, AlignConstants.ROTATION_CONSTRAINTS); - /// Infinite acceleration PIDs - private final ProfiledPIDController xAxisInfPid = - new ProfiledPIDController( - AlignConstants.X_AXIS_P, - AlignConstants.X_AXIS_I, - AlignConstants.X_AXIS_D, - AlignConstants.X_AXIS_CONSTRAINTS); - - private final ProfiledPIDController yAxisInfPid = - new ProfiledPIDController( - AlignConstants.Y_AXIS_P, - AlignConstants.Y_AXIS_I, - AlignConstants.Y_AXIS_D, - AlignConstants.Y_AXIS_CONSTRAINTS); + private TrapezoidProfile xProfile; + private TrapezoidProfile yProfile; private final Odometry odometry = Odometry.getInstance(); private final ConfigManager configManager = ConfigManager.getInstance(); @@ -74,10 +48,6 @@ public class AlignCommand extends Command { private double distToTarget = Double.MAX_VALUE; - private double finalX = 0.0; - private double finalY = 0.0; - private boolean log = true; - /** * Align to a fieldspace position with odometry * @@ -102,50 +72,16 @@ public AlignCommand( LOGGER.debug("Created new align command with '{}' profile", this.profile); - this.xAxisPid.setTolerance( - configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); - this.yAxisPid.setTolerance( - configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); - this.rotationPid.setTolerance( - Math.toRadians( - configManager.get( - String.format("align_%s_rotation_tolerance", this.profile), 1))); - Pose3d robotPose = Odometry.getInstance().getRobotPose(); - this.rotationPid.enableContinuousInput(-Math.PI, Math.PI); - this.xAxisPid.reset( - robotPose.getX(), - swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); - this.yAxisPid.reset( - robotPose.getY(), - swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond); this.rotationPid.reset( robotPose.getRotation().getZ(), swerveSubsystem.getFieldRelativeChassisSpeeds().omegaRadiansPerSecond); - this.xAxisInfPid.reset( - robotPose.getX(), - swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); - this.yAxisInfPid.reset( - robotPose.getY(), - swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond); - - this.xAxisPid.setGoal(robotPose.getX()); - this.yAxisPid.setGoal(robotPose.getY()); this.rotationPid.setGoal(robotPose.getRotation().getZ()); - - this.xAxisInfPid.setGoal(robotPose.getX()); - this.yAxisInfPid.setGoal(robotPose.getY()); - - this.xAxisPid.calculate(robotPose.getX()); - this.yAxisPid.calculate(robotPose.getY()); this.rotationPid.calculate(robotPose.getRotation().getZ()); - this.xAxisInfPid.calculate(robotPose.getX()); - this.yAxisInfPid.calculate(robotPose.getY()); - addRequirements(swerveSubsystem); } @@ -155,45 +91,33 @@ public void initialize() { this.timeSenseFinished = -1; this.doUpdate = true; this.distToTarget = Double.MAX_VALUE; - this.finalX = 0.0; - this.finalY = 0.0; - this.log = true; LOGGER.info("Initializing AlignCommand"); Pose3d robotPose = odometry.getRobotPose(); - // PID updates - this.xAxisPid.setP(configManager.get("align_x_axis_p", 3)); - this.yAxisPid.setP(configManager.get("align_y_axis_p", 3)); + // rot PID updates this.rotationPid.setP(configManager.get("align_rot_p", 7.3)); - - this.xAxisInfPid.setP(configManager.get("align_x_axis_p", 3)); - this.yAxisInfPid.setP(configManager.get("align_y_axis_p", 3)); - - this.xAxisPid.setD(configManager.get("align_x_axis_d", .25)); - this.yAxisPid.setD(configManager.get("align_y_axis_d", .25)); this.rotationPid.setD(configManager.get("align_rot_d", 0.5)); - - this.xAxisInfPid.setD(configManager.get("align_x_axis_d", .25)); - this.yAxisInfPid.setD(configManager.get("align_y_axis_d", .25)); - - this.xAxisPid.setI(configManager.get("align_x_axis_i", 0.0)); - this.yAxisPid.setI(configManager.get("align_y_axis_i", 0.0)); this.rotationPid.setI(configManager.get("align_rot_i", 0.0)); - this.xAxisInfPid.setI(configManager.get("align_x_axis_i", 0.0)); - this.yAxisInfPid.setI(configManager.get("align_y_axis_i", 0.0)); + this.xProfile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + configManager.get( + String.format("align_%s_x_max_vel_m", this.profile), 3.0), + configManager.get( + String.format("align_%s_x_max_accel_mps", this.profile), + 2.5))); + + this.yProfile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + configManager.get( + String.format("align_%s_y_max_vel_m", this.profile), 3.0), + configManager.get( + String.format("align_%s_y_max_accel_mps", this.profile), + 2.5))); - this.xAxisPid.setConstraints( - new TrapezoidProfile.Constraints( - configManager.get(String.format("align_%s_x_max_vel_m", this.profile), 3.0), - configManager.get( - String.format("align_%s_x_max_accel_mps", this.profile), 2.5))); - this.yAxisPid.setConstraints( - new TrapezoidProfile.Constraints( - configManager.get(String.format("align_%s_y_max_vel_m", this.profile), 3.0), - configManager.get( - String.format("align_%s_y_max_accel_mps", this.profile), 2.5))); this.rotationPid.setConstraints( new TrapezoidProfile.Constraints( Math.toRadians( @@ -205,62 +129,17 @@ public void initialize() { String.format("align_%s_rot_max_accel_degps", this.profile), 360)))); - this.xAxisInfPid.setConstraints( - new TrapezoidProfile.Constraints( - configManager.get(String.format("align_%s_x_max_vel_m", this.profile), 3.0), - 5000)); - this.yAxisInfPid.setConstraints( - new TrapezoidProfile.Constraints( - configManager.get(String.format("align_%s_y_max_vel_m", this.profile), 3.0), - 5000)); - - this.xAxisPid.setTolerance( - configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); - this.yAxisPid.setTolerance( - configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); this.rotationPid.setTolerance( Math.toRadians( configManager.get( String.format("align_%s_rotation_tolerance", this.profile), 1))); - this.xAxisPid.setTolerance( - configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); - this.yAxisPid.setTolerance( - configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); - // Reset All pids - this.xAxisPid.reset( - robotPose.getX(), - swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); - this.yAxisPid.reset( - robotPose.getY(), - swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond); this.rotationPid.reset( robotPose.getRotation().getZ(), swerveSubsystem.getFieldRelativeChassisSpeeds().omegaRadiansPerSecond); - this.xAxisInfPid.reset( - robotPose.getX(), - swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); - this.yAxisInfPid.reset( - robotPose.getY(), - swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond); - - this.xAxisPid.setGoal(targetPos.getX()); - this.yAxisPid.setGoal(targetPos.getY()); this.rotationPid.setGoal(targetPos.getRotation().getRadians()); - - this.xAxisInfPid.setGoal(targetPos.getX()); - this.yAxisInfPid.setGoal(targetPos.getY()); - - if (!stopWhenFinished) { - Pose2d fakePose = - AlignUtils.getXDistBack( - robotPose.toPose2d(), - ConfigManager.getInstance().get("fake_pose_dist_back", 0.5)); - this.xAxisPid.calculate(fakePose.getX()); - this.yAxisPid.calculate(fakePose.getY()); - } } @Override @@ -271,28 +150,32 @@ public void execute() { Math.pow(robotPose.getX() - targetPos.getX(), 2) + Math.pow(robotPose.getY() - targetPos.getY(), 2); - double xAxisCalc = this.xAxisPid.calculate(robotPose.getX()); - double yAxisCalc = this.yAxisPid.calculate(robotPose.getY()); + double xAxisCalc = + this.xProfile.calculate( + 0, + new TrapezoidProfile.State( + robotPose.getX(), + swerveSubsystem.getFieldRelativeChassisSpeeds() + .vxMetersPerSecond), + new TrapezoidProfile.State(targetPos.getX(), 0)) + .velocity; + double yAxisCalc = + this.yProfile.calculate( + 0, + new TrapezoidProfile.State( + robotPose.getY(), + swerveSubsystem.getFieldRelativeChassisSpeeds() + .vyMetersPerSecond), + new TrapezoidProfile.State(targetPos.getY(), 0)) + .velocity; double rotationPidCalc = this.rotationPid.calculate(robotPose.getRotation().getZ()); - double infX = xAxisInfPid.calculate(robotPose.getX()); - double infY = yAxisInfPid.calculate(robotPose.getY()); - debug.setEntry("Dist to target (Error)", distToTarget); - debug.setEntry("X Pid Error", this.xAxisPid.getPositionError()); - debug.setEntry("Y Pid Error", this.yAxisPid.getPositionError()); + debug.setEntry("X Error", Math.abs(robotPose.getX() - targetPos.getX())); + debug.setEntry("Y Error", Math.abs(robotPose.getY() - targetPos.getY())); debug.setEntry("Rot Pid Error", this.rotationPid.getPositionError()); - debug.setEntry("X Pid Error (inf)", this.xAxisInfPid.getPositionError()); - debug.setEntry("Y Pid Error (inf)", this.yAxisInfPid.getPositionError()); - - debug.setEntry("X Pid setpoint", this.xAxisPid.atSetpoint()); - debug.setEntry("X Pid goal", this.xAxisPid.atGoal()); - - debug.setEntry("y Pid setpoint", this.xAxisPid.atSetpoint()); - debug.setEntry("Y Pid goal", this.yAxisPid.atGoal()); - debug.setEntry("Rot Pid setpoint", this.rotationPid.atSetpoint()); debug.setEntry("Rot Pid goal", this.rotationPid.atGoal()); debug.setEntry("Robot rotation: ", robotPose.getRotation().getZ()); @@ -304,29 +187,8 @@ public void execute() { Math.abs(this.targetPos.getRotation().getRadians()) - Math.abs(odometry.getRobotPose().getRotation().getZ()))); - this.finalX = - xAxisCalc - + (xAxisPid.atGoal() || xAxisPid.atSetpoint() - ? 0 - : Math.signum(xAxisCalc) * configManager.get("align_ff", 0.1)); - this.finalY = - yAxisCalc - + (yAxisPid.atGoal() || yAxisPid.atSetpoint() - ? 0 - : Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1)); - - if (!stopWhenFinished) { - this.finalX = infX; - this.finalY = infY; - } - - if (log) { - log = false; - LOGGER.info("First commanded speeds: {} {}", xAxisCalc, xAxisCalc); - } - - debug.setEntry("Xms", finalX); - debug.setEntry("Yms", finalY); + debug.setEntry("Xms", xAxisCalc); + debug.setEntry("Yms", yAxisCalc); debug.setEntry("Rrads", rotationPidCalc); this.debug.setArrayEntry( @@ -337,7 +199,7 @@ public void execute() { this.targetPos.getRotation().getRadians() }); - swerveSubsystem.drive(finalX, finalY, rotationPidCalc, true, !stopWhenFinished, true); + swerveSubsystem.drive(xAxisCalc, yAxisCalc, 0.0, true, !stopWhenFinished, true); if (checkAtGoal() && doUpdate) { LOGGER.info("Hit goal, waiting for time to expire"); @@ -356,21 +218,25 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - if (stopWhenFinished) swerveSubsystem.zeroVoltage(); - else swerveSubsystem.drive(this.finalX, this.finalY, 0.0, true, true, true); - LOGGER.info("Final commanded speeds: {} {}", this.finalX, this.finalY); + // if (stopWhenFinished) swerveSubsystem.zeroVoltage(); + // else swerveSubsystem.drive(this.finalX, this.finalY, 0.0, true, true, true); + // LOGGER.info("Final commanded speeds: {} {}", this.finalX, this.finalY); } private boolean checkAtGoal() { - return (stopWhenFinished && xAxisPid.atGoal() && yAxisPid.atGoal() && rotationPid.atGoal()) - || (!stopWhenFinished - && this.distToTarget - <= Math.pow( - configManager.get( - String.format( - "align_%s_pos_dist_tol", this.profile), - 0.05), - 2) - && rotationPid.atGoal()); + return distToTarget + <= configManager.get( + String.format("align_%s_pos_dist_tol", this.profile), 0) + && rotationPid.atGoal() + && MathUtil.isNear( + configManager.get( + String.format("align_%s_x_target_end_vel", this.profile), 0.0), + swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond, + configManager.get("align_vel_tol", 0.0)) + && MathUtil.isNear( + configManager.get( + String.format("align_%s_y_target_end_vel", this.profile), 0.0), + swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond, + configManager.get("align_vel_tol", 0.0)); } } diff --git a/src/main/java/org/blackknights/controllers/MAXSwerveModule.java b/src/main/java/org/blackknights/controllers/MAXSwerveModule.java index 7ede542..459360d 100644 --- a/src/main/java/org/blackknights/controllers/MAXSwerveModule.java +++ b/src/main/java/org/blackknights/controllers/MAXSwerveModule.java @@ -3,6 +3,7 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -15,6 +16,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import org.blackknights.utils.ConfigManager; /** A wrapper class for swerve modules */ public class MAXSwerveModule { @@ -105,7 +107,10 @@ public void setDesiredState(SwerveModuleState desiredState) { // Command driving and turning SPARKS towards their respective setpoints. drivingClosedLoopController.setReference( - correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity); + correctedDesiredState.speedMetersPerSecond, + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + ConfigManager.getInstance().get("swerve_module_ff", 0.0)); turningClosedLoopController.setReference( correctedDesiredState.angle.getRadians(), ControlType.kPosition); diff --git a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java index 2a2b7ae..225e863 100644 --- a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java @@ -331,6 +331,7 @@ public void drive( xSpeedDelivered, ySpeedDelivered, rotationDelivered)); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DrivetrainConstants.MAX_SPEED_METERS_PER_SECOND); + frontLeft.setDesiredState(swerveModuleStates[0]); frontRight.setDesiredState(swerveModuleStates[1]); rearLeft.setDesiredState(swerveModuleStates[2]); From 87c2fef5ff5a3032968a95155652a11a01e18a33 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sat, 29 Mar 2025 11:34:32 -0600 Subject: [PATCH 10/17] align: All after amp --- .../blackknights/commands/AlignCommand.java | 19 ++++---- .../blackknights/commands/DriveCommands.java | 9 +++- .../constants/DrivetrainConstants.java | 2 +- .../controllers/MAXSwerveModule.java | 25 +++++++++-- .../controllers/MAXSwerveModuleConfig.java | 5 +-- .../subsystems/SwerveSubsystem.java | 43 +++++++++---------- .../backups/tuning_2025_03_28:22:20:37.json | 1 + 7 files changed, 64 insertions(+), 40 deletions(-) create mode 100755 tuning/backups/tuning_2025_03_28:22:20:37.json diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 6d1a204..1c00ac8 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -152,7 +152,7 @@ public void execute() { double xAxisCalc = this.xProfile.calculate( - 0, + configManager.get("align_trap_t_sec", 0.2), new TrapezoidProfile.State( robotPose.getX(), swerveSubsystem.getFieldRelativeChassisSpeeds() @@ -161,14 +161,15 @@ public void execute() { .velocity; double yAxisCalc = this.yProfile.calculate( - 0, + configManager.get("align_trap_t_sec", 0.2), new TrapezoidProfile.State( robotPose.getY(), swerveSubsystem.getFieldRelativeChassisSpeeds() .vyMetersPerSecond), new TrapezoidProfile.State(targetPos.getY(), 0)) .velocity; - double rotationPidCalc = this.rotationPid.calculate(robotPose.getRotation().getZ()); + + double rotCalc = this.rotationPid.calculate(robotPose.getRotation().getZ()); debug.setEntry("Dist to target (Error)", distToTarget); @@ -189,7 +190,7 @@ public void execute() { debug.setEntry("Xms", xAxisCalc); debug.setEntry("Yms", yAxisCalc); - debug.setEntry("Rrads", rotationPidCalc); + debug.setEntry("Rrads", rotationPid.getSetpoint().velocity); this.debug.setArrayEntry( "target_pose", @@ -199,7 +200,7 @@ public void execute() { this.targetPos.getRotation().getRadians() }); - swerveSubsystem.drive(xAxisCalc, yAxisCalc, 0.0, true, !stopWhenFinished, true); + swerveSubsystem.drive(xAxisCalc, yAxisCalc, rotCalc, true, !stopWhenFinished, true); if (checkAtGoal() && doUpdate) { LOGGER.info("Hit goal, waiting for time to expire"); @@ -218,7 +219,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - // if (stopWhenFinished) swerveSubsystem.zeroVoltage(); + if (stopWhenFinished) swerveSubsystem.zeroVoltage(); // else swerveSubsystem.drive(this.finalX, this.finalY, 0.0, true, true, true); // LOGGER.info("Final commanded speeds: {} {}", this.finalX, this.finalY); } @@ -226,17 +227,17 @@ public void end(boolean interrupted) { private boolean checkAtGoal() { return distToTarget <= configManager.get( - String.format("align_%s_pos_dist_tol", this.profile), 0) + String.format("align_%s_pos_dist_tol", this.profile), 0.0) && rotationPid.atGoal() && MathUtil.isNear( configManager.get( String.format("align_%s_x_target_end_vel", this.profile), 0.0), swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond, - configManager.get("align_vel_tol", 0.0)) + configManager.get(String.format("align_%s_vel_tol", this.profile), 0.0)) && MathUtil.isNear( configManager.get( String.format("align_%s_y_target_end_vel", this.profile), 0.0), swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond, - configManager.get("align_vel_tol", 0.0)); + configManager.get(String.format("align_%s_vel_tol", this.profile), 0.0)); } } diff --git a/src/main/java/org/blackknights/commands/DriveCommands.java b/src/main/java/org/blackknights/commands/DriveCommands.java index 4dbbf84..4b00e6d 100644 --- a/src/main/java/org/blackknights/commands/DriveCommands.java +++ b/src/main/java/org/blackknights/commands/DriveCommands.java @@ -6,6 +6,7 @@ import java.util.function.DoubleSupplier; import org.blackknights.subsystems.SwerveSubsystem; import org.blackknights.utils.ConfigManager; +import org.blackknights.utils.NetworkTablesUtils; /** Command to drive swerve */ public class DriveCommands extends Command { @@ -56,12 +57,18 @@ public void execute() { radians.getAsDouble(), ConfigManager.getInstance().get("controller_deadband", 0.06)); + NetworkTablesUtils debug = NetworkTablesUtils.getTable("debug"); + + debug.setEntry("Forward desired", forwardDesired); + debug.setEntry("Sideways desired", sidewaysDesired); + debug.setEntry("Rotation desired", radiansDesired); + swerveSubsystem.drive( forwardDesired, sidewaysDesired, radiansDesired, fieldRelativeFromButton, - true, + false, false); } diff --git a/src/main/java/org/blackknights/constants/DrivetrainConstants.java b/src/main/java/org/blackknights/constants/DrivetrainConstants.java index c5cfefe..40f6380 100644 --- a/src/main/java/org/blackknights/constants/DrivetrainConstants.java +++ b/src/main/java/org/blackknights/constants/DrivetrainConstants.java @@ -55,7 +55,7 @@ public class DrivetrainConstants { // The MAXSwerve module can be configured with one of three pinion gears: 12T, // 13T, or 14T. This changes the drive speed of the module (a pinion gear with // more teeth will result in a robot that drives faster). - public static final int DRIVING_MOTOR_PINION_TEETH = 14; + public static final int DRIVING_MOTOR_PINION_TEETH = 13; public static final int BEVEL_GEAR_TEETH = 45; public static final int FIRST_STAGE_SPUR_GEAR_TEETH = 22; public static final int BEVEL_PINION_TEETH = 15; diff --git a/src/main/java/org/blackknights/controllers/MAXSwerveModule.java b/src/main/java/org/blackknights/controllers/MAXSwerveModule.java index 459360d..70dcd11 100644 --- a/src/main/java/org/blackknights/controllers/MAXSwerveModule.java +++ b/src/main/java/org/blackknights/controllers/MAXSwerveModule.java @@ -13,16 +13,19 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import org.blackknights.utils.ConfigManager; +import org.blackknights.utils.NetworkTablesUtils; /** A wrapper class for swerve modules */ public class MAXSwerveModule { private final SparkFlex drivingSpark; private final SparkMax turningSpark; + private final int drivingCanId; + private final RelativeEncoder drivingEncoder; private final AbsoluteEncoder turningEncoder; @@ -32,6 +35,9 @@ public class MAXSwerveModule { private final double chassisAngularOffset; private SwerveModuleState desiredState = new SwerveModuleState(0.0, new Rotation2d()); + private SimpleMotorFeedforward feedforward = + new SimpleMotorFeedforward(0.096286, 2.3216, 0.41854, 1); + /** * Constructs a MAXSwerveModule and configures the driving and turning motor, encoder, and PID * controller. This configuration is specific to the REV MAXSwerve Module built with NEOs, @@ -40,6 +46,7 @@ public class MAXSwerveModule { public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { drivingSpark = new SparkFlex(drivingCANId, MotorType.kBrushless); turningSpark = new SparkMax(turningCANId, MotorType.kBrushless); + this.drivingCanId = drivingCANId; drivingEncoder = drivingSpark.getEncoder(); turningEncoder = turningSpark.getAbsoluteEncoder(); @@ -105,12 +112,24 @@ public void setDesiredState(SwerveModuleState desiredState) { // Optimize the reference state to avoid spinning further than 90 degrees. correctedDesiredState.optimize(new Rotation2d(turningEncoder.getPosition())); - // Command driving and turning SPARKS towards their respective setpoints. + double ffOutput = + feedforward.calculateWithVelocities( + drivingEncoder.getVelocity(), correctedDesiredState.speedMetersPerSecond); + + NetworkTablesUtils.getTable("debug") + .setEntry(String.format("ID(%s) - Swerve FF Output", drivingCanId), ffOutput); + NetworkTablesUtils.getTable("debug") + .setEntry( + String.format("ID(%s) - Swerve target mps", drivingCanId), + correctedDesiredState.speedMetersPerSecond); + + // Command driving and turning SPARKS towards their respective endpoints. drivingClosedLoopController.setReference( correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity, ClosedLoopSlot.kSlot0, - ConfigManager.getInstance().get("swerve_module_ff", 0.0)); + ffOutput); + turningClosedLoopController.setReference( correctedDesiredState.angle.getRadians(), ControlType.kPosition); diff --git a/src/main/java/org/blackknights/controllers/MAXSwerveModuleConfig.java b/src/main/java/org/blackknights/controllers/MAXSwerveModuleConfig.java index bda4e55..ba71863 100644 --- a/src/main/java/org/blackknights/controllers/MAXSwerveModuleConfig.java +++ b/src/main/java/org/blackknights/controllers/MAXSwerveModuleConfig.java @@ -19,7 +19,6 @@ public class MAXSwerveModuleConfig { * Math.PI / DrivetrainConstants.DRIVING_MOTOR_REDUCTION; double turningFactor = 2 * Math.PI; - double drivingVelocityFeedForward = 1 / DrivetrainConstants.DRIVE_WHEEL_FREE_SPEED_RPS; drivingConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(50); drivingConfig @@ -30,8 +29,8 @@ public class MAXSwerveModuleConfig { .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) // These are example gains you may need to them for your own robot! - .pid(0.04, 0, 0) - .velocityFF(drivingVelocityFeedForward) + .pid(0.11185, 0, 0) + .velocityFF(0.0) .outputRange(-1, 1); turningConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(20); diff --git a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java index 225e863..428c4fb 100644 --- a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java @@ -1,8 +1,6 @@ /* Black Knights Robotics (C) 2025 */ package org.blackknights.subsystems; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.SparkMaxConfig; import com.studica.frc.AHRS; import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; @@ -24,7 +22,6 @@ import org.apache.logging.log4j.Logger; import org.blackknights.constants.DrivetrainConstants; import org.blackknights.controllers.MAXSwerveModule; -import org.blackknights.controllers.MAXSwerveModuleConfig; import org.blackknights.framework.Odometry; import org.blackknights.utils.ConfigManager; import org.blackknights.utils.NetworkTablesUtils; @@ -133,26 +130,26 @@ public SwerveSubsystem() { .getDoubleTopic("rlpos") .getEntry(rearLeft.getPosition().angle.getRadians()); - public void reconfigure() { - ConfigManager cm = ConfigManager.getInstance(); - - SparkFlexConfig drivingConfig = MAXSwerveModuleConfig.drivingConfig; - SparkMaxConfig turningConfig = MAXSwerveModuleConfig.turningConfig; - - drivingConfig.closedLoop.pid( - cm.get("swerve_drive_p", 0.5), - cm.get("swerve_drive_i", 0.0), - cm.get("swerve_drive_d", 0.0)); - turningConfig.closedLoop.pid( - cm.get("swerve_turning_p", 1), - cm.get("swerve_turning_i", 0.0), - cm.get("swerve_turning_d", 0.0)); - - frontLeft.reconfigure(drivingConfig, turningConfig); - rearRight.reconfigure(drivingConfig, turningConfig); - rearRight.reconfigure(drivingConfig, turningConfig); - rearLeft.reconfigure(drivingConfig, turningConfig); - } + // public void reconfigure() { + // ConfigManager cm = ConfigManager.getInstance(); + // + // SparkFlexConfig drivingConfig = MAXSwerveModuleConfig.drivingConfig; + // SparkMaxConfig turningConfig = MAXSwerveModuleConfig.turningConfig; + // + // drivingConfig.closedLoop.pid( + // cm.get("swerve_drive_p", 0.5), + // cm.get("swerve_drive_i", 0.0), + // cm.get("swerve_drive_d", 0.0)); + // turningConfig.closedLoop.pid( + // cm.get("swerve_turning_p", 1), + // cm.get("swerve_turning_i", 0.0), + // cm.get("swerve_turning_d", 0.0)); + // + // frontLeft.reconfigure(drivingConfig, turningConfig); + // rearRight.reconfigure(drivingConfig, turningConfig); + // rearRight.reconfigure(drivingConfig, turningConfig); + // rearLeft.reconfigure(drivingConfig, turningConfig); + // } @Override public void periodic() { diff --git a/tuning/backups/tuning_2025_03_28:22:20:37.json b/tuning/backups/tuning_2025_03_28:22:20:37.json new file mode 100755 index 0000000..ffd88b9 --- /dev/null +++ b/tuning/backups/tuning_2025_03_28:22:20:37.json @@ -0,0 +1 @@ +{"align_enable_slow_mode":false,"back_time_sec":0.0,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"outtake_max_time_sec":5.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":2.0,"align_rough_rotation_tolerance":40,"align_fine_x_max_vel_m":1.0,"align_close_y_p":3.1,"align_rough_vel_tol":0.5,"arm_tol":1.5,"vision_min_distance":0.5,"inf_switch_dist":2.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"outtaking_time_ms":550.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":1.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":4.5,"align_x_max_accel_mps":3.5,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":3.5,"arm_intake":0.7,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_fine_vel_tol":0.05,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.25,"align_dist_back":0.82,"swerve_drive_p":0.04,"outtake_wait_time_ms":300.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_fine_y_target_end_vel":0.0,"align_rough_y_target_end_vel":0.0,"align_x_p":3.0,"scoring_left_x":0.565,"arm_max_vel_degs":90.0,"scoring_left_y":0.184,"align_fine_pos_tolerance":0.05,"l4_elevator_position":0.0,"align_x_d":0.25,"center_cam_pitch":-45.0,"controller_deadband":0.2,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.25,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":3.5,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":4.5,"align_rot_i":0.0,"align_x_axis_i":0.0,"driver_max_speed_rot":360,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_fine_pos_dist_tol":0.02,"align_ff_ks":1.0,"back_mps":-3.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"align_ff_kv":1.0,"elevator_l1":0.2,"elevator_l2":0.5,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.982,"elevator_l4":1.67,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":2.5,"align_y_max_accel_mps":3.5,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":2.5,"placeholder":0.0,"align_trap_t_sec":0.2,"arm_kg":0.0,"scoring_right_x":0.565,"align_rough_x_target_end_vel":0.0,"drive_direction_slew_rate":18.0,"scoring_right_y":-0.152,"arm_encoder_offset":-0.153,"elevator_intake":0.13,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_fine_x_target_end_vel":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"right_cam_angle":7.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":4.5,"autointake_dist_back":-0.22,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.5,"arm_l3":-0.6,"align_vel_tol":0.25,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":0.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":520,"fake_pose_dist_back":0.5,"align_rough_x_max_vel_m":4.5} From bb1538bfefd232161234a0105a2a40a886d28885 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sat, 29 Mar 2025 17:44:36 -0600 Subject: [PATCH 11/17] align: Non-zero ending velocity --- .../java/org/blackknights/RobotContainer.java | 11 +- .../blackknights/commands/AlignCommand.java | 107 ++++++++++++++++-- .../subsystems/SwerveSubsystem.java | 50 +++----- 3 files changed, 120 insertions(+), 48 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index 85428d7..ae867fc 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -110,10 +110,12 @@ private void configureBindings() { new DriveCommands( swerveSubsystem, () -> - primaryController.getLeftY() + Math.pow(primaryController.getLeftY(), 2) + * Math.signum(primaryController.getLeftY()) * ConfigManager.getInstance().get("driver_max_speed", 3.5), () -> - primaryController.getLeftX() + Math.pow(primaryController.getLeftX(), 2) + * Math.signum(primaryController.getLeftX()) * ConfigManager.getInstance().get("driver_max_speed", 3.5), () -> -primaryController.getRightX() @@ -287,6 +289,7 @@ private Command getPlaceCommand( ConfigManager.getInstance() .get("align_dist_back", 0.5)), false, + true, "rough"), new BaseCommand(elevatorSubsystem, armSubsystem)), new ParallelRaceGroup( @@ -294,6 +297,7 @@ private Command getPlaceCommand( swerveSubsystem, () -> currentSupplier.get().getPose(), true, + false, "fine") .withTimeout( ConfigManager.getInstance() @@ -359,7 +363,8 @@ private Command getAutoIntakeCommand(IntakeSides side) { return new ParallelRaceGroup( new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE), new ParallelCommandGroup( - new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "rough"), + new AlignCommand( + swerveSubsystem, () -> intakePoseFinal, true, false, "rough"), new ElevatorArmCommand( elevatorSubsystem, armSubsystem, diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 1c00ac8..d219079 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -13,10 +13,35 @@ import org.blackknights.constants.AlignConstants; import org.blackknights.framework.Odometry; import org.blackknights.subsystems.SwerveSubsystem; +import org.blackknights.utils.AlignUtils; import org.blackknights.utils.ConfigManager; import org.blackknights.utils.NetworkTablesUtils; -/** Align the robot in fieldspace */ +/** + * Align the robot in fieldspace Config Manager Keys:
+ * - align_rot_p - Proportional gain for the rotation PID controller.
+ * - align_rot_i - Integral gain for the rotation PID controller.
+ * - align_rot_d - Derivative gain for the rotation PID controller.
+ * - align_rot_max_vel_deg - Maximum rotational velocity (degrees per second).
+ * - align_rot_max_accel_degps - Maximum rotational acceleration (degrees per second squared).
+ * - align_rot_tolerance - Tolerance for considering the rotation PID at goal (degrees).
+ * - align_[profile]_x_max_vel_m - Maximum velocity for X-axis movement (meters per second).
+ * - align_[profile]_x_max_accel_mps - Maximum acceleration for X-axis movement (meters per second + * squared).
+ * - align_[profile]_y_max_vel_m - Maximum velocity for Y-axis movement (meters per second).
+ * - align_[profile]_y_max_accel_mps - Maximum acceleration for Y-axis movement (meters per second + * squared).
+ * - align_[profile]_rotation_tolerance - Tolerance for rotational alignment (degrees).
+ * - align_[profile]_pos_dist_tol - Position error tolerance before considering the robot aligned + * (meters).
+ * - align_[profile]_vel_tol - Velocity tolerance for stopping criteria (meters per second).
+ * - align_[profile]_halfmoon_dist - Distance from the target for the exclusion point(meters).
+ * - align_[profile]_halfmoon_tol - The tolerance for the half moon exclusion point (radius of + * circle) (meters).
+ * - align_[profile]_finish_time - Time in milliseconds the robot must stay within tolerances before + * stopping.
+ * - align_trap_t_sec - Time step for trapezoidal motion profile calculations (seconds).
+ */ public class AlignCommand extends Command { private static final Logger LOGGER = LogManager.getLogger(); private final SwerveSubsystem swerveSubsystem; @@ -36,6 +61,7 @@ public class AlignCommand extends Command { private final String profile; private final boolean stopWhenFinished; + private final boolean useHalfMoon; private final Supplier pose2dSupplier; @@ -47,6 +73,9 @@ public class AlignCommand extends Command { private boolean doUpdate = true; private double distToTarget = Double.MAX_VALUE; + private double halfMoonDist = Double.MAX_VALUE; + private double finalXVel = 0; + private double finalYVel = 0; /** * Align to a fieldspace position with odometry @@ -64,10 +93,12 @@ public AlignCommand( SwerveSubsystem swerveSubsystem, Supplier poseSupplier, boolean stopWhenFinished, + boolean useHalfMoon, String profile) { this.swerveSubsystem = swerveSubsystem; this.pose2dSupplier = poseSupplier; this.stopWhenFinished = stopWhenFinished; + this.useHalfMoon = useHalfMoon; this.profile = profile; LOGGER.debug("Created new align command with '{}' profile", this.profile); @@ -91,6 +122,18 @@ public void initialize() { this.timeSenseFinished = -1; this.doUpdate = true; this.distToTarget = Double.MAX_VALUE; + this.halfMoonDist = Double.MAX_VALUE; + this.finalXVel = + Math.cos(this.targetPos.getRotation().getRadians()) + * configManager.get( + String.format("align_%s_ending_vel_mag", this.profile), 1.0); + this.finalYVel = + Math.sin(this.targetPos.getRotation().getRadians()) + * configManager.get( + String.format("align_%s_ending_vel_mag", this.profile), 1.0); + + debug.setEntry("Align/Final X vel", finalXVel); + debug.setEntry("Align/Final Y vel", finalYVel); LOGGER.info("Initializing AlignCommand"); Pose3d robotPose = odometry.getRobotPose(); @@ -147,8 +190,19 @@ public void execute() { Pose3d robotPose = odometry.getRobotPose(); this.distToTarget = - Math.pow(robotPose.getX() - targetPos.getX(), 2) - + Math.pow(robotPose.getY() - targetPos.getY(), 2); + Math.sqrt( + Math.pow(robotPose.getX() - targetPos.getX(), 2) + + Math.pow(robotPose.getY() - targetPos.getY(), 2)); + + Pose2d halfMoonClosePose = + AlignUtils.getXDistBack( + this.targetPos, + -configManager.get( + String.format("align_%s_halfmoon_dist", this.profile), 0.5)); + this.halfMoonDist = + Math.sqrt( + Math.pow(robotPose.getX() - halfMoonClosePose.getX(), 2) + + Math.pow(robotPose.getY() - halfMoonClosePose.getY(), 2)); double xAxisCalc = this.xProfile.calculate( @@ -157,7 +211,7 @@ public void execute() { robotPose.getX(), swerveSubsystem.getFieldRelativeChassisSpeeds() .vxMetersPerSecond), - new TrapezoidProfile.State(targetPos.getX(), 0)) + new TrapezoidProfile.State(targetPos.getX(), this.finalXVel)) .velocity; double yAxisCalc = this.yProfile.calculate( @@ -166,7 +220,7 @@ public void execute() { robotPose.getY(), swerveSubsystem.getFieldRelativeChassisSpeeds() .vyMetersPerSecond), - new TrapezoidProfile.State(targetPos.getY(), 0)) + new TrapezoidProfile.State(targetPos.getY(), this.finalYVel)) .velocity; double rotCalc = this.rotationPid.calculate(robotPose.getRotation().getZ()); @@ -200,7 +254,7 @@ public void execute() { this.targetPos.getRotation().getRadians() }); - swerveSubsystem.drive(xAxisCalc, yAxisCalc, rotCalc, true, !stopWhenFinished, true); + swerveSubsystem.drive(xAxisCalc, yAxisCalc, rotCalc, true, false, true); if (checkAtGoal() && doUpdate) { LOGGER.info("Hit goal, waiting for time to expire"); @@ -225,18 +279,51 @@ public void end(boolean interrupted) { } private boolean checkAtGoal() { + debug.setEntry( + "Align/Dist Check", + distToTarget + <= configManager.get( + String.format("align_%s_pos_dist_tol", this.profile), 0.0)); + + debug.setEntry( + "Align/Half moon check", + (!useHalfMoon + || halfMoonDist + >= configManager.get( + String.format("align_%s_halfmoon_tol", this.profile), + 0.0))); + + debug.setEntry("Align/Rotation check", rotationPid.atGoal()); + debug.setEntry( + "Align/X Vel Check", + MathUtil.isNear( + configManager.get( + String.format("align_%s_x_target_end_vel", this.profile), 0.0), + swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond, + configManager.get(String.format("align_%s_vel_tol", this.profile), 0.0))); + + debug.setEntry( + "Align/Y Vel Check", + MathUtil.isNear( + configManager.get( + String.format("align_%s_y_target_end_vel", this.profile), 0.0), + swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond, + configManager.get(String.format("align_%s_vel_tol", this.profile), 0.0))); + return distToTarget <= configManager.get( String.format("align_%s_pos_dist_tol", this.profile), 0.0) + && (!useHalfMoon + || halfMoonDist + >= configManager.get( + String.format("align_%s_halfmoon_tol", this.profile), 0.0)) && rotationPid.atGoal() && MathUtil.isNear( - configManager.get( - String.format("align_%s_x_target_end_vel", this.profile), 0.0), + this.finalXVel, swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond, configManager.get(String.format("align_%s_vel_tol", this.profile), 0.0)) && MathUtil.isNear( - configManager.get( - String.format("align_%s_y_target_end_vel", this.profile), 0.0), + this.finalYVel, swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond, configManager.get(String.format("align_%s_vel_tol", this.profile), 0.0)); } diff --git a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java index 428c4fb..695829b 100644 --- a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java @@ -240,7 +240,6 @@ public void drive( if (rateLimit) { - // Math that calculates important stuff about where the robot is heading double inputTranslationDirection = Math.atan2(sidewaysMetersPerSecond, forwardMetersPerSecond); double inputTranslationMagnitude = @@ -248,6 +247,11 @@ public void drive( Math.pow(forwardMetersPerSecond, 2.0) + Math.pow(sidewaysMetersPerSecond, 2.0)); + double currentTranslationMagnitude = + Math.sqrt( + Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2.0) + + Math.pow(getRobotRelativeSpeeds().vyMetersPerSecond, 2.0)); + double directionSlewRate; if (currentTranslationMagnitude != 0.0) { directionSlewRate = @@ -258,57 +262,33 @@ public void drive( DrivetrainConstants.DIRECTION_SLEW_RATE) / currentTranslationMagnitude); } else { - directionSlewRate = 500.0; // super high number means slew is instantaneous + directionSlewRate = + 500.0; // super high number means change in direction is instantaneous } double currentTime = WPIUtilJNI.now() * 1e-6; double elapsedTime = currentTime - previousTime; - double angleDifference = - SwerveUtils.angleDifference( - inputTranslationDirection, currentTranslationDirection); - if (angleDifference < 0.45 * Math.PI) { - currentTranslationDirection = - SwerveUtils.stepTowardsCircular( - currentTranslationDirection, - inputTranslationDirection, - directionSlewRate * elapsedTime); - currentTranslationMagnitude = magnitudeLimiter.calculate(inputTranslationMagnitude); - } else if (angleDifference > 0.85 * Math.PI) { - if (currentTranslationMagnitude - > 1e-4) { // small number avoids floating-point errors - currentTranslationMagnitude = magnitudeLimiter.calculate(0.0); - } else { - currentTranslationDirection = - SwerveUtils.wrapAngle(currentTranslationDirection + Math.PI); - currentTranslationMagnitude = - magnitudeLimiter.calculate(inputTranslationMagnitude); - } - } else { - currentTranslationDirection = - SwerveUtils.stepTowardsCircular( - currentTranslationDirection, - inputTranslationDirection, - directionSlewRate * elapsedTime); - currentTranslationMagnitude = magnitudeLimiter.calculate(inputTranslationMagnitude); - } + currentTranslationDirection = + SwerveUtils.stepTowardsCircular( + currentTranslationDirection, + inputTranslationDirection, + directionSlewRate * elapsedTime); previousTime = currentTime; - xSpeedCommanded = currentTranslationMagnitude * Math.cos(currentTranslationDirection); - ySpeedCommanded = currentTranslationMagnitude * Math.sin(currentTranslationDirection); - currentRotation = rotationLimiter.calculate(radiansPerSecond); + xSpeedCommanded = inputTranslationMagnitude * Math.cos(currentTranslationDirection); + ySpeedCommanded = inputTranslationMagnitude * Math.sin(currentTranslationDirection); } else { // If there's no rate limit, robot does the exact inputs given. xSpeedCommanded = forwardMetersPerSecond; ySpeedCommanded = sidewaysMetersPerSecond; - currentRotation = radiansPerSecond; } double xSpeedDelivered = xSpeedCommanded; double ySpeedDelivered = ySpeedCommanded; - double rotationDelivered = currentRotation; + double rotationDelivered = radiansPerSecond; var swerveModuleStates = DrivetrainConstants.DRIVE_KINEMATICS.toSwerveModuleStates( From 4044de1502aec16f2dd5bf3c9d83384c31ce7501 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Tue, 1 Apr 2025 19:01:17 -0600 Subject: [PATCH 12/17] misc: All after 1619 --- .../java/org/blackknights/RobotContainer.java | 25 ++- .../blackknights/commands/AlignCommand.java | 163 +++++++++++------- .../constants/VisionConstants.java | 5 +- .../controllers/MAXSwerveModule.java | 9 +- 4 files changed, 126 insertions(+), 76 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index ae867fc..50db069 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -78,6 +78,8 @@ public RobotContainer() { new SequentialCommandGroup( getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4")), getAutoIntakeCommand(IntakeSides.LEFT), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("7L4")), + getAutoIntakeCommand(IntakeSides.LEFT), getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")), getAutoIntakeCommand(IntakeSides.LEFT), getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4")))); @@ -87,7 +89,7 @@ public RobotContainer() { new SequentialCommandGroup( getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L4")), getAutoIntakeCommand(IntakeSides.RIGHT), - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("4L4")), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("6L4")), getAutoIntakeCommand(IntakeSides.RIGHT), getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("5L4")))); @@ -361,14 +363,19 @@ private Command getAutoIntakeCommand(IntakeSides side) { intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI))); return new ParallelRaceGroup( - new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE), - new ParallelCommandGroup( - new AlignCommand( - swerveSubsystem, () -> intakePoseFinal, true, false, "rough"), - new ElevatorArmCommand( - elevatorSubsystem, - armSubsystem, - () -> ScoringConstants.ScoringHeights.INTAKE))); + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE), + new ParallelCommandGroup( + new AlignCommand( + swerveSubsystem, + () -> intakePoseFinal, + true, + false, + "rough"), + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.INTAKE))) + .withTimeout(5); } private static Pose2d getPose2d(IntakeSides side) { diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index d219079..d5d5e77 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -2,7 +2,7 @@ package org.blackknights.commands; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Timer; @@ -10,7 +10,6 @@ import java.util.function.Supplier; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; -import org.blackknights.constants.AlignConstants; import org.blackknights.framework.Odometry; import org.blackknights.subsystems.SwerveSubsystem; import org.blackknights.utils.AlignUtils; @@ -46,15 +45,11 @@ public class AlignCommand extends Command { private static final Logger LOGGER = LogManager.getLogger(); private final SwerveSubsystem swerveSubsystem; - private final ProfiledPIDController rotationPid = - new ProfiledPIDController( - AlignConstants.ROTATION_P, - AlignConstants.ROTATION_I, - AlignConstants.ROTATION_D, - AlignConstants.ROTATION_CONSTRAINTS); - private TrapezoidProfile xProfile; private TrapezoidProfile yProfile; + private TrapezoidProfile rotationProfile; + + private SimpleMotorFeedforward rotationFF; private final Odometry odometry = Odometry.getInstance(); private final ConfigManager configManager = ConfigManager.getInstance(); @@ -103,16 +98,6 @@ public AlignCommand( LOGGER.debug("Created new align command with '{}' profile", this.profile); - Pose3d robotPose = Odometry.getInstance().getRobotPose(); - this.rotationPid.enableContinuousInput(-Math.PI, Math.PI); - - this.rotationPid.reset( - robotPose.getRotation().getZ(), - swerveSubsystem.getFieldRelativeChassisSpeeds().omegaRadiansPerSecond); - - this.rotationPid.setGoal(robotPose.getRotation().getZ()); - this.rotationPid.calculate(robotPose.getRotation().getZ()); - addRequirements(swerveSubsystem); } @@ -138,11 +123,6 @@ public void initialize() { LOGGER.info("Initializing AlignCommand"); Pose3d robotPose = odometry.getRobotPose(); - // rot PID updates - this.rotationPid.setP(configManager.get("align_rot_p", 7.3)); - this.rotationPid.setD(configManager.get("align_rot_d", 0.5)); - this.rotationPid.setI(configManager.get("align_rot_i", 0.0)); - this.xProfile = new TrapezoidProfile( new TrapezoidProfile.Constraints( @@ -161,28 +141,27 @@ public void initialize() { String.format("align_%s_y_max_accel_mps", this.profile), 2.5))); - this.rotationPid.setConstraints( - new TrapezoidProfile.Constraints( - Math.toRadians( - configManager.get( - String.format("align_%s_rot_max_vel_deg", this.profile), - 360)), - Math.toRadians( - configManager.get( - String.format("align_%s_rot_max_accel_degps", this.profile), - 360)))); - - this.rotationPid.setTolerance( - Math.toRadians( - configManager.get( - String.format("align_%s_rotation_tolerance", this.profile), 1))); - - // Reset All pids - this.rotationPid.reset( - robotPose.getRotation().getZ(), - swerveSubsystem.getFieldRelativeChassisSpeeds().omegaRadiansPerSecond); - - this.rotationPid.setGoal(targetPos.getRotation().getRadians()); + this.rotationProfile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + Math.toRadians( + configManager.get( + String.format( + "align_%s_rot_max_vel_deg", this.profile), + 360)), + Math.toRadians( + configManager.get( + String.format( + "align_%s_rot_max_accel_degps", + this.profile), + 360)))); + + this.rotationFF = + new SimpleMotorFeedforward( + configManager.get("align_rotation_ff_ks", 0.01622), + configManager.get("align_rotation_ff_kv", 0.0), + 0.0, + 1); } @Override @@ -213,6 +192,7 @@ public void execute() { .vxMetersPerSecond), new TrapezoidProfile.State(targetPos.getX(), this.finalXVel)) .velocity; + double yAxisCalc = this.yProfile.calculate( configManager.get("align_trap_t_sec", 0.2), @@ -223,18 +203,39 @@ public void execute() { new TrapezoidProfile.State(targetPos.getY(), this.finalYVel)) .velocity; - double rotCalc = this.rotationPid.calculate(robotPose.getRotation().getZ()); + double errorBound = Math.PI; + double goal = + MathUtil.inputModulus( + targetPos.getRotation().getRadians() - robotPose.getRotation().getZ(), + -errorBound, + errorBound); + + double rotCalc = + this.rotationProfile.calculate( + 0.1, + new TrapezoidProfile.State( + robotPose.getRotation().getZ(), + swerveSubsystem.getFieldRelativeChassisSpeeds() + .omegaRadiansPerSecond), + new TrapezoidProfile.State( + goal + robotPose.getRotation().getZ(), 0.0)) + .velocity; + + rotCalc += this.rotationFF.calculate(rotCalc); debug.setEntry("Dist to target (Error)", distToTarget); debug.setEntry("X Error", Math.abs(robotPose.getX() - targetPos.getX())); debug.setEntry("Y Error", Math.abs(robotPose.getY() - targetPos.getY())); - debug.setEntry("Rot Pid Error", this.rotationPid.getPositionError()); - debug.setEntry("Rot Pid setpoint", this.rotationPid.atSetpoint()); - debug.setEntry("Rot Pid goal", this.rotationPid.atGoal()); - debug.setEntry("Robot rotation: ", robotPose.getRotation().getZ()); - debug.setEntry("Rot setpoint", this.rotationPid.getSetpoint().position); + debug.setEntry("Align/Total time x", this.xProfile.totalTime()); + debug.setEntry("Align/Total time y", this.yProfile.totalTime()); + // debug.setEntry("Rot Pid Error", this.rotationPid.getPositionError()); + // + // debug.setEntry("Rot Pid setpoint", this.rotationPid.atSetpoint()); + // debug.setEntry("Rot Pid goal", this.rotationPid.atGoal()); + // debug.setEntry("Robot rotation: ", robotPose.getRotation().getZ()); + // debug.setEntry("Rot setpoint", this.rotationPid.getSetpoint().position); debug.setEntry( "Rot diff", @@ -244,7 +245,7 @@ public void execute() { debug.setEntry("Xms", xAxisCalc); debug.setEntry("Yms", yAxisCalc); - debug.setEntry("Rrads", rotationPid.getSetpoint().velocity); + debug.setEntry("Rrads", rotCalc); this.debug.setArrayEntry( "target_pose", @@ -293,7 +294,28 @@ private boolean checkAtGoal() { String.format("align_%s_halfmoon_tol", this.profile), 0.0))); - debug.setEntry("Align/Rotation check", rotationPid.atGoal()); + debug.setEntry( + "Align/Rotation check", + Math.abs( + Math.abs(Odometry.getInstance().getRobotPose().getRotation().getZ()) + - Math.abs(targetPos.getRotation().getRadians())) + <= Math.toRadians( + ConfigManager.getInstance() + .get( + String.format("align_%s_rot_tol_deg", this.profile), + 1.0))); + debug.setEntry( + "The value", + Math.abs( + Math.abs(Odometry.getInstance().getRobotPose().getRotation().getZ()) + - Math.abs(targetPos.getRotation().getRadians()))); + + debug.setEntry( + "The value current", + Math.abs(Odometry.getInstance().getRobotPose().getRotation().getZ())); + + debug.setEntry("The value target", Math.abs(targetPos.getRotation().getRadians())); + debug.setEntry( "Align/X Vel Check", MathUtil.isNear( @@ -313,18 +335,33 @@ private boolean checkAtGoal() { return distToTarget <= configManager.get( String.format("align_%s_pos_dist_tol", this.profile), 0.0) + && Math.abs( + Math.abs(Odometry.getInstance().getRobotPose().getRotation().getZ()) + - Math.abs( + targetPos.getRotation().getRadians() > Math.PI + ? targetPos.getRotation().getRadians() + - Math.PI * 2 + : targetPos.getRotation().getRadians())) + <= Math.toRadians( + ConfigManager.getInstance() + .get( + String.format("align_%s_rot_tol_deg", this.profile), + 1.0)) && (!useHalfMoon || halfMoonDist >= configManager.get( String.format("align_%s_halfmoon_tol", this.profile), 0.0)) - && rotationPid.atGoal() - && MathUtil.isNear( - this.finalXVel, - swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond, - configManager.get(String.format("align_%s_vel_tol", this.profile), 0.0)) - && MathUtil.isNear( - this.finalYVel, - swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond, - configManager.get(String.format("align_%s_vel_tol", this.profile), 0.0)); + && (!stopWhenFinished + || MathUtil.isNear( + this.finalXVel, + swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond, + configManager.get( + String.format("align_%s_vel_tol", this.profile), 0.0))) + && (!stopWhenFinished + || MathUtil.isNear( + this.finalYVel, + swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond, + configManager.get( + String.format("align_%s_vel_tol", this.profile), 0.0))); } } diff --git a/src/main/java/org/blackknights/constants/VisionConstants.java b/src/main/java/org/blackknights/constants/VisionConstants.java index 828ca8e..a66cf18 100644 --- a/src/main/java/org/blackknights/constants/VisionConstants.java +++ b/src/main/java/org/blackknights/constants/VisionConstants.java @@ -12,7 +12,7 @@ public class VisionConstants { public static final Transform3d LEFT_CAM_TRANSFORM = new Transform3d( - 0.253, + ConfigManager.getInstance().get("left_cam_x", 0.253), 0.336, 0.229, new Rotation3d( @@ -20,10 +20,9 @@ public class VisionConstants { 0.0, Math.toRadians( ConfigManager.getInstance().get("left_cam_angle", -10.0)))); - public static final Transform3d RIGHT_CAM_TRANSFORM = new Transform3d( - 0.253, + ConfigManager.getInstance().get("right_cam_x", .253), -0.3995, 0.229, new Rotation3d( diff --git a/src/main/java/org/blackknights/controllers/MAXSwerveModule.java b/src/main/java/org/blackknights/controllers/MAXSwerveModule.java index 70dcd11..29f7aa0 100644 --- a/src/main/java/org/blackknights/controllers/MAXSwerveModule.java +++ b/src/main/java/org/blackknights/controllers/MAXSwerveModule.java @@ -13,10 +13,12 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import org.blackknights.utils.ConfigManager; import org.blackknights.utils.NetworkTablesUtils; /** A wrapper class for swerve modules */ @@ -128,7 +130,12 @@ public void setDesiredState(SwerveModuleState desiredState) { correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity, ClosedLoopSlot.kSlot0, - ffOutput); + MathUtil.isNear( + 0.0, + correctedDesiredState.speedMetersPerSecond, + ConfigManager.getInstance().get("swerve_min_velocity", 0.01)) + ? 0.0 + : ffOutput); turningClosedLoopController.setReference( correctedDesiredState.angle.getRadians(), ControlType.kPosition); From 95df1b25d83b222698a1737760a53d2fa4ec5754 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Thu, 3 Apr 2025 17:59:18 -0600 Subject: [PATCH 13/17] misc: prop working code? --- src/main/java/org/blackknights/Robot.java | 2 +- .../java/org/blackknights/RobotContainer.java | 107 +++++++++----- .../blackknights/commands/AlignCommand.java | 139 +++++++++--------- .../blackknights/commands/IntakeCommand.java | 32 +++- .../constants/ScoringConstants.java | 5 + .../blackknights/framework/CoralQueue.java | 31 +++- .../org/blackknights/framework/Odometry.java | 19 ++- .../subsystems/ButtonBoardSubsystem.java | 11 +- .../java/org/blackknights/utils/Camera.java | 69 ++++----- .../framework/CoralQueueTest.java | 92 ------------ .../framework/CoralQueueTest.java.old | 99 +++++++++++++ 11 files changed, 359 insertions(+), 247 deletions(-) delete mode 100644 src/test/java/org/blackknights/framework/CoralQueueTest.java create mode 100644 src/test/java/org/blackknights/framework/CoralQueueTest.java.old diff --git a/src/main/java/org/blackknights/Robot.java b/src/main/java/org/blackknights/Robot.java index 067e411..11826b9 100644 --- a/src/main/java/org/blackknights/Robot.java +++ b/src/main/java/org/blackknights/Robot.java @@ -50,7 +50,7 @@ public void disabledExit() {} @Override public void autonomousInit() { - autonomousCommand = robotContainer.getAutonomousCommand(); + autonomousCommand = robotContainer.getAutonomousCommand().get(); if (autonomousCommand != null) { autonomousCommand.schedule(); diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index 50db069..e379804 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -56,7 +56,7 @@ public class RobotContainer { private final Odometry odometry = Odometry.getInstance(); // Auto Chooser - SendableChooser superSecretMissileTech = new SendableChooser<>(); + SendableChooser> superSecretMissileTech = new SendableChooser<>(); // CQ profile selector private final SendableChooser cqProfiles = @@ -75,32 +75,35 @@ public RobotContainer() { // Autos superSecretMissileTech.addOption( "LEFT_3", - new SequentialCommandGroup( - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4")), - getAutoIntakeCommand(IntakeSides.LEFT), - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("7L4")), - getAutoIntakeCommand(IntakeSides.LEFT), - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")), - getAutoIntakeCommand(IntakeSides.LEFT), - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4")))); + () -> + new SequentialCommandGroup( + getLocationPlaceCommand( + CoralQueue.CoralPosition.fromString("10L4")), + getAutoIntakeCommand(IntakeSides.LEFT), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")), + getAutoIntakeCommand(IntakeSides.LEFT), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4")), + getAutoIntakeCommand(IntakeSides.LEFT), + getLocationPlaceCommand( + CoralQueue.CoralPosition.fromString("7L4")))); superSecretMissileTech.addOption( "RIGHT_3", - new SequentialCommandGroup( - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L4")), - getAutoIntakeCommand(IntakeSides.RIGHT), - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("6L4")), - getAutoIntakeCommand(IntakeSides.RIGHT), - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("5L4")))); + () -> + new SequentialCommandGroup( + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L4")), + getAutoIntakeCommand(IntakeSides.RIGHT), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("5L4")), + getAutoIntakeCommand(IntakeSides.RIGHT), + getLocationPlaceCommand( + CoralQueue.CoralPosition.fromString("4L4")))); superSecretMissileTech.addOption( "CENTER_LEFT", - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("12L4"))); + () -> getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("12L4"))); superSecretMissileTech.addOption( "CENTER_RIGHT", - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("1L4"))); - - superSecretMissileTech.addOption("INTAKE_TEST", getAutoIntakeCommand(IntakeSides.RIGHT)); + () -> getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("1L4"))); } /** Configure controller bindings */ @@ -112,12 +115,10 @@ private void configureBindings() { new DriveCommands( swerveSubsystem, () -> - Math.pow(primaryController.getLeftY(), 2) - * Math.signum(primaryController.getLeftY()) + primaryController.getLeftY() * ConfigManager.getInstance().get("driver_max_speed", 3.5), () -> - Math.pow(primaryController.getLeftX(), 2) - * Math.signum(primaryController.getLeftX()) + primaryController.getLeftX() * ConfigManager.getInstance().get("driver_max_speed", 3.5), () -> -primaryController.getRightX() @@ -150,7 +151,9 @@ private void configureBindings() { armSubsystem, () -> ScoringConstants.ScoringHeights.INTAKE), new IntakeCommand( - intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)), + intakeSubsystem, + IntakeCommand.IntakeMode.INTAKE, + ScoringConstants.ScoringHeights.L1)), new RunCommand( () -> swerveSubsystem.drive( @@ -221,11 +224,19 @@ private void configureBindings() { secondaryController .rightTrigger(0.2) - .whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)); + .whileTrue( + new IntakeCommand( + intakeSubsystem, + IntakeCommand.IntakeMode.OUTTAKE, + ScoringConstants.ScoringHeights.L1)); secondaryController .leftTrigger(0.2) - .whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)); + .whileTrue( + new IntakeCommand( + intakeSubsystem, + IntakeCommand.IntakeMode.INTAKE, + ScoringConstants.ScoringHeights.L1)); } /** Runs once when the code starts */ @@ -254,7 +265,7 @@ public void teleopInit() { * * @return The command to run */ - public Command getAutonomousCommand() { + public Supplier getAutonomousCommand() { return superSecretMissileTech.getSelected(); } @@ -295,15 +306,27 @@ private Command getPlaceCommand( "rough"), new BaseCommand(elevatorSubsystem, armSubsystem)), new ParallelRaceGroup( - new AlignCommand( - swerveSubsystem, - () -> currentSupplier.get().getPose(), - true, - false, - "fine") - .withTimeout( - ConfigManager.getInstance() - .get("align_fine_max_time", 3.0)), + new SequentialCommandGroup( + new RunCommand( + () -> { + if (currentSupplier.get().getSide() + == ScoringConstants.ScoringSides.LEFT) { + rightCam.setEnabled(true); + leftCam.setEnabled(false); + } else { + leftCam.setEnabled(true); + rightCam.setEnabled(false); + } + }), + new AlignCommand( + swerveSubsystem, + () -> currentSupplier.get().getPose(), + true, + false, + "fine") + .withTimeout( + ConfigManager.getInstance() + .get("align_fine_max_time", 3.0))), new RunCommand( () -> intakeSubsystem.setSpeed( @@ -318,8 +341,11 @@ private Command getPlaceCommand( new ElevatorArmCommand( elevatorSubsystem, armSubsystem, - () -> nextSupplier.get().getHeight()), - new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE) + () -> currentSupplier.get().getHeight()), + new IntakeCommand( + intakeSubsystem, + IntakeCommand.IntakeMode.OUTTAKE, + nextSupplier.get().getHeight()) .withTimeout( ConfigManager.getInstance() .get("outtake_max_time_sec", 5.0))), @@ -363,7 +389,10 @@ private Command getAutoIntakeCommand(IntakeSides side) { intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI))); return new ParallelRaceGroup( - new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE), + new IntakeCommand( + intakeSubsystem, + IntakeCommand.IntakeMode.INTAKE, + ScoringConstants.ScoringHeights.L1), new ParallelCommandGroup( new AlignCommand( swerveSubsystem, diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index d5d5e77..5f2fa56 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -45,8 +45,7 @@ public class AlignCommand extends Command { private static final Logger LOGGER = LogManager.getLogger(); private final SwerveSubsystem swerveSubsystem; - private TrapezoidProfile xProfile; - private TrapezoidProfile yProfile; + private TrapezoidProfile distProfile; private TrapezoidProfile rotationProfile; private SimpleMotorFeedforward rotationFF; @@ -69,8 +68,6 @@ public class AlignCommand extends Command { private double distToTarget = Double.MAX_VALUE; private double halfMoonDist = Double.MAX_VALUE; - private double finalXVel = 0; - private double finalYVel = 0; /** * Align to a fieldspace position with odometry @@ -108,37 +105,16 @@ public void initialize() { this.doUpdate = true; this.distToTarget = Double.MAX_VALUE; this.halfMoonDist = Double.MAX_VALUE; - this.finalXVel = - Math.cos(this.targetPos.getRotation().getRadians()) - * configManager.get( - String.format("align_%s_ending_vel_mag", this.profile), 1.0); - this.finalYVel = - Math.sin(this.targetPos.getRotation().getRadians()) - * configManager.get( - String.format("align_%s_ending_vel_mag", this.profile), 1.0); - - debug.setEntry("Align/Final X vel", finalXVel); - debug.setEntry("Align/Final Y vel", finalYVel); LOGGER.info("Initializing AlignCommand"); - Pose3d robotPose = odometry.getRobotPose(); - - this.xProfile = - new TrapezoidProfile( - new TrapezoidProfile.Constraints( - configManager.get( - String.format("align_%s_x_max_vel_m", this.profile), 3.0), - configManager.get( - String.format("align_%s_x_max_accel_mps", this.profile), - 2.5))); - this.yProfile = + this.distProfile = new TrapezoidProfile( new TrapezoidProfile.Constraints( configManager.get( - String.format("align_%s_y_max_vel_m", this.profile), 3.0), + String.format("align_%s_max_vel_m", this.profile), 3.0), configManager.get( - String.format("align_%s_y_max_accel_mps", this.profile), + String.format("align_%s_max_accel_mps", this.profile), 2.5))); this.rotationProfile = @@ -167,11 +143,10 @@ public void initialize() { @Override public void execute() { Pose3d robotPose = odometry.getRobotPose(); + double d_x = this.targetPos.getX() - robotPose.getX(); + double d_y = this.targetPos.getY() - robotPose.getY(); - this.distToTarget = - Math.sqrt( - Math.pow(robotPose.getX() - targetPos.getX(), 2) - + Math.pow(robotPose.getY() - targetPos.getY(), 2)); + this.distToTarget = Math.sqrt(Math.pow(d_x, 2) + Math.pow(d_y, 2)); Pose2d halfMoonClosePose = AlignUtils.getXDistBack( @@ -183,25 +158,52 @@ public void execute() { Math.pow(robotPose.getX() - halfMoonClosePose.getX(), 2) + Math.pow(robotPose.getY() - halfMoonClosePose.getY(), 2)); - double xAxisCalc = - this.xProfile.calculate( + double trapCalc = + -this.distProfile.calculate( configManager.get("align_trap_t_sec", 0.2), new TrapezoidProfile.State( - robotPose.getX(), - swerveSubsystem.getFieldRelativeChassisSpeeds() - .vxMetersPerSecond), - new TrapezoidProfile.State(targetPos.getX(), this.finalXVel)) + distToTarget, + -Math.sqrt( + Math.pow( + swerveSubsystem + .getFieldRelativeChassisSpeeds() + .vxMetersPerSecond, + 2.0) + + Math.pow( + swerveSubsystem + .getFieldRelativeChassisSpeeds() + .vyMetersPerSecond, + 2.0))), + new TrapezoidProfile.State( + 0.0, + configManager.get( + String.format( + "align_%s_ending_vel_mag", this.profile), + 1.0))) .velocity; - double yAxisCalc = - this.yProfile.calculate( - configManager.get("align_trap_t_sec", 0.2), - new TrapezoidProfile.State( - robotPose.getY(), + double a = Math.atan2(d_y, d_x); + + debug.setEntry("Align/Trap Calc", trapCalc); + debug.setEntry("Align/Angle", Math.toDegrees(a)); + debug.setEntry( + "Align/Robot Vel", + Math.sqrt( + Math.pow( swerveSubsystem.getFieldRelativeChassisSpeeds() - .vyMetersPerSecond), - new TrapezoidProfile.State(targetPos.getY(), this.finalYVel)) - .velocity; + .vxMetersPerSecond, + 2.0) + + Math.pow( + swerveSubsystem.getRobotRelativeSpeeds().vyMetersPerSecond, + 2.0))); + + double xAxisCalc = trapCalc * Math.cos(a); + double yAxisCalc = trapCalc * Math.sin(a); + + // if (d_x < 0) { + // xAxisCalc = -xAxisCalc; + // yAxisCalc = -yAxisCalc; + // } double errorBound = Math.PI; double goal = @@ -225,17 +227,10 @@ public void execute() { debug.setEntry("Dist to target (Error)", distToTarget); - debug.setEntry("X Error", Math.abs(robotPose.getX() - targetPos.getX())); - debug.setEntry("Y Error", Math.abs(robotPose.getY() - targetPos.getY())); + debug.setEntry("X Error", Math.abs(d_x)); + debug.setEntry("Y Error", Math.abs(d_y)); - debug.setEntry("Align/Total time x", this.xProfile.totalTime()); - debug.setEntry("Align/Total time y", this.yProfile.totalTime()); - // debug.setEntry("Rot Pid Error", this.rotationPid.getPositionError()); - // - // debug.setEntry("Rot Pid setpoint", this.rotationPid.atSetpoint()); - // debug.setEntry("Rot Pid goal", this.rotationPid.atGoal()); - // debug.setEntry("Robot rotation: ", robotPose.getRotation().getZ()); - // debug.setEntry("Rot setpoint", this.rotationPid.getSetpoint().position); + debug.setEntry("Align/Total time", this.distProfile.totalTime()); debug.setEntry( "Rot diff", @@ -255,7 +250,15 @@ public void execute() { this.targetPos.getRotation().getRadians() }); - swerveSubsystem.drive(xAxisCalc, yAxisCalc, rotCalc, true, false, true); + if (Math.abs(xAxisCalc) + < configManager.get(String.format("align_%s_min_vel", this.profile), 0.002) + && Math.abs(yAxisCalc) + < configManager.get( + String.format("align_%s_min_vel", this.profile), 0.002)) { + swerveSubsystem.zeroVoltage(); + } else { + swerveSubsystem.drive(xAxisCalc, yAxisCalc, rotCalc, true, false, true); + } if (checkAtGoal() && doUpdate) { LOGGER.info("Hit goal, waiting for time to expire"); @@ -275,8 +278,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { if (stopWhenFinished) swerveSubsystem.zeroVoltage(); - // else swerveSubsystem.drive(this.finalX, this.finalY, 0.0, true, true, true); - // LOGGER.info("Final commanded speeds: {} {}", this.finalX, this.finalY); } private boolean checkAtGoal() { @@ -353,14 +354,20 @@ private boolean checkAtGoal() { String.format("align_%s_halfmoon_tol", this.profile), 0.0)) && (!stopWhenFinished || MathUtil.isNear( - this.finalXVel, - swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond, configManager.get( - String.format("align_%s_vel_tol", this.profile), 0.0))) - && (!stopWhenFinished - || MathUtil.isNear( - this.finalYVel, - swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond, + String.format("align_%s_ending_vel_mag", this.profile), + 1.0), + Math.sqrt( + Math.pow( + swerveSubsystem + .getFieldRelativeChassisSpeeds() + .vxMetersPerSecond, + 2) + + Math.pow( + swerveSubsystem + .getFieldRelativeChassisSpeeds() + .vxMetersPerSecond, + 2)), configManager.get( String.format("align_%s_vel_tol", this.profile), 0.0))); } diff --git a/src/main/java/org/blackknights/commands/IntakeCommand.java b/src/main/java/org/blackknights/commands/IntakeCommand.java index 52811d0..4b34ed8 100644 --- a/src/main/java/org/blackknights/commands/IntakeCommand.java +++ b/src/main/java/org/blackknights/commands/IntakeCommand.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import org.blackknights.constants.ScoringConstants; import org.blackknights.subsystems.IntakeSubsystem; import org.blackknights.utils.ConfigManager; @@ -10,6 +11,7 @@ public class IntakeCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final IntakeMode mode; + private final ScoringConstants.ScoringHeights height; private double startTime; @@ -19,9 +21,13 @@ public class IntakeCommand extends Command { * @param intakeSubsystem The instance of {@link IntakeSubsystem} * @param mode The intake mode ({@link IntakeMode}) */ - public IntakeCommand(IntakeSubsystem intakeSubsystem, IntakeMode mode) { + public IntakeCommand( + IntakeSubsystem intakeSubsystem, + IntakeMode mode, + ScoringConstants.ScoringHeights height) { this.intakeSubsystem = intakeSubsystem; this.mode = mode; + this.height = height; addRequirements(intakeSubsystem); } @@ -42,7 +48,12 @@ public void execute() { case OUTTAKE: { if (Timer.getFPGATimestamp() * 1000 - this.startTime - > ConfigManager.getInstance().get("outtake_wait_time_ms", 40)) { + > ConfigManager.getInstance() + .get( + String.format( + "outtake_%s_wait_time_ms", + this.height.toString().toLowerCase()), + 250)) { intakeSubsystem.setVoltage( ConfigManager.getInstance().get("outtake_speed", -8.0)); } @@ -62,7 +73,22 @@ public boolean isFinished() { || (mode.equals(IntakeMode.OUTTAKE) && !intakeSubsystem.getLinebreak() && Timer.getFPGATimestamp() * 1000 - this.startTime - > ConfigManager.getInstance().get("outtaking_time_ms", 40)); + > (ConfigManager.getInstance() + .get( + String.format( + "outtaking_%s_time_ms", + this.height + .toString() + .toLowerCase()), + 200) + + ConfigManager.getInstance() + .get( + String.format( + "outtake_%s_wait_time_ms", + this.height + .toString() + .toLowerCase()), + 250))); } /** Enum of the different intake modes */ diff --git a/src/main/java/org/blackknights/constants/ScoringConstants.java b/src/main/java/org/blackknights/constants/ScoringConstants.java index c03bbbe..d526ce1 100644 --- a/src/main/java/org/blackknights/constants/ScoringConstants.java +++ b/src/main/java/org/blackknights/constants/ScoringConstants.java @@ -98,6 +98,11 @@ public enum ScoringHeights { INTAKE, } + public enum ScoringSides { + LEFT, + RIGHT + } + /** * Get a scoring position from an april tag id * diff --git a/src/main/java/org/blackknights/framework/CoralQueue.java b/src/main/java/org/blackknights/framework/CoralQueue.java index 4c34ea8..a7e8257 100644 --- a/src/main/java/org/blackknights/framework/CoralQueue.java +++ b/src/main/java/org/blackknights/framework/CoralQueue.java @@ -145,6 +145,7 @@ public static class CoralPosition { private final String stringId; private final Pose2d pose; private final ScoringConstants.ScoringHeights height; + private final ScoringConstants.ScoringSides side; /** * Create a new coral position @@ -153,10 +154,15 @@ public static class CoralPosition { * @param pose A {@link Pose2d} for the soring position * @param height The target height */ - public CoralPosition(String stringId, Pose2d pose, ScoringConstants.ScoringHeights height) { + public CoralPosition( + String stringId, + Pose2d pose, + ScoringConstants.ScoringHeights height, + ScoringConstants.ScoringSides side) { this.stringId = stringId; this.pose = pose; this.height = height; + this.side = side; } /** Create an empty coral position */ @@ -164,6 +170,7 @@ public CoralPosition() { this.stringId = ""; this.pose = new Pose2d(); this.height = ScoringConstants.ScoringHeights.L1; + this.side = ScoringConstants.ScoringSides.RIGHT; } public static CoralPosition fromString(String string) { @@ -184,7 +191,14 @@ public static CoralPosition fromString(String string) { posIdx += 12; } - LOGGER.debug("CoralQ: posIdx = {}, heightString = {}", posIdx, heightString); + LOGGER.info( + "CoralP: name = {}, posIdx = {}, heightString = {}, side = {}", + string, + posIdx, + heightString, + posIdx % 2 == 0 + ? ScoringConstants.ScoringSides.RIGHT + : ScoringConstants.ScoringSides.LEFT); return new CoralPosition( string, @@ -192,7 +206,10 @@ public static CoralPosition fromString(String string) { ScoringConstants.CORAL_POSITIONS[posIdx].getX(), ScoringConstants.CORAL_POSITIONS[posIdx].getY(), ScoringConstants.CORAL_POSITIONS[posIdx].getRotation()), - ScoringConstants.ScoringHeights.valueOf(heightString.toUpperCase())); + ScoringConstants.ScoringHeights.valueOf(heightString.toUpperCase()), + posIdx % 2 == 0 + ? ScoringConstants.ScoringSides.RIGHT + : ScoringConstants.ScoringSides.LEFT); } /** @@ -213,6 +230,10 @@ public ScoringConstants.ScoringHeights getHeight() { return this.height; } + public ScoringConstants.ScoringSides getSide() { + return this.side; + } + /** * Return the pose as a double array * @@ -234,8 +255,8 @@ public boolean[] getBooleanHeights() { @Override public String toString() { return String.format( - "CoralPosition(stringId: %s, pose: %s, height: %s)", - this.stringId, this.pose.toString(), this.height); + "CoralPosition(stringId: %s, pose: %s, height: %s, side: %s)", + this.stringId, this.pose.toString(), this.height, this.side); } @Override diff --git a/src/main/java/org/blackknights/framework/Odometry.java b/src/main/java/org/blackknights/framework/Odometry.java index 4a3c5d9..67aa675 100644 --- a/src/main/java/org/blackknights/framework/Odometry.java +++ b/src/main/java/org/blackknights/framework/Odometry.java @@ -7,7 +7,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.kinematics.SwerveModulePosition; -import java.util.ArrayList; +import java.util.HashMap; import java.util.Optional; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; @@ -20,7 +20,7 @@ /** System for all odometry related stuff */ public class Odometry { /** List of cameras used for vision-based measurements to refine odometry. */ - private ArrayList cameras = new ArrayList<>(); + private HashMap cameras = new HashMap<>(); /** Singleton instance of the OdometrySubsystem. */ private static Odometry INSTANCE = null; @@ -92,7 +92,7 @@ public static Odometry getInstance() { * @param camera The {@link Camera} to add to the system. */ public void addCamera(Camera camera) { - this.cameras.add(camera); + this.cameras.put(camera.getName(), camera); } public SwerveDrivePoseEstimator3d getPoseEstimator() { @@ -106,11 +106,11 @@ public SwerveDrivePoseEstimator3d getPoseEstimator() { * @return Either an empty optional if the camera does not exist, or the camera */ public Optional getCamera(String name) { - for (Camera c : this.cameras) { - if (c.getName().equalsIgnoreCase(name)) return Optional.of(c); + if (!this.cameras.containsKey(name)) { + return Optional.empty(); } - return Optional.empty(); + return Optional.of(this.cameras.get(name)); } /** @@ -172,7 +172,7 @@ public void periodic() { this.getRobotPose().getY(), this.getRobotPose().getRotation().getZ() }); - for (Camera c : this.cameras) { + for (Camera c : this.cameras.values()) { Optional pose = c.getPoseFieldSpace(this.getRobotPose()); if (pose.isPresent()) { double dist = @@ -181,6 +181,11 @@ public void periodic() { + Math.pow(c.getTargetPose().getY(), 2)); debug.setEntry(String.format("%s_dist_to_target", c.getName()), dist); + debug.setArrayEntry( + String.format("%s_pose", c.getName()), + new double[] { + pose.get().getX(), pose.get().getY(), pose.get().getRotation().getZ() + }); if (dist <= ConfigManager.getInstance().get("vision_cutoff_distance", 3) && dist > ConfigManager.getInstance().get("vision_min_distance", 0.5)) { diff --git a/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java b/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java index c36c84a..43a5be6 100644 --- a/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java @@ -20,6 +20,7 @@ public class ButtonBoardSubsystem extends SubsystemBase { private Pose2d currentPose = null; private ScoringConstants.ScoringHeights currentHeight = null; + private ScoringConstants.ScoringSides currentSide = null; /** * Create a new Instance of the subsystem for the button board @@ -40,9 +41,13 @@ public void periodic() { if (this.currentHeight != null && this.currentPose != null) { coralQueue.interruptQueue( new CoralQueue.CoralPosition( - "BBInterrupt", this.currentPose, this.currentHeight)); + "BBInterrupt", + this.currentPose, + this.currentHeight, + this.currentSide = null)); this.currentPose = null; this.currentHeight = null; + this.currentSide = null; } } @@ -75,6 +80,10 @@ public void bind() { String.format("L%d", finalB - 12)); } else { int arrId = finalB + (isBlue ? 12 : 0) - 1; + this.currentSide = + arrId % 2 == 0 + ? ScoringConstants.ScoringSides.RIGHT + : ScoringConstants.ScoringSides.LEFT; this.currentPose = ScoringConstants.CORAL_POSITIONS[arrId]; } } diff --git a/src/main/java/org/blackknights/utils/Camera.java b/src/main/java/org/blackknights/utils/Camera.java index 05212c4..b60153e 100644 --- a/src/main/java/org/blackknights/utils/Camera.java +++ b/src/main/java/org/blackknights/utils/Camera.java @@ -17,18 +17,19 @@ /** */ public class Camera { - private Optional limelightTable; + private NetworkTablesUtils limelightTable; - private Optional photonCamera; - private Optional photonPoseEstimator; + private PhotonCamera photonCamera; + private PhotonPoseEstimator photonPoseEstimator; private final Transform3d camOffset; - private double photonTimestamp; - private Pose3d targetPose; private final String name; - private final CameraType camType; + private double photonTimestamp; + private Pose3d targetPose; + private boolean enabled; + private static final Logger LOGGER = LogManager.getLogger(); /** @@ -41,24 +42,22 @@ public Camera(String name, CameraType camType, Transform3d camOffset) { this.camType = camType; this.camOffset = camOffset; this.name = name; + this.enabled = true; switch (this.camType) { case PHOTONVISION -> { - photonCamera = Optional.of(new PhotonCamera(name)); + photonCamera = new PhotonCamera(name); photonPoseEstimator = - Optional.of( - new PhotonPoseEstimator( - AprilTagFieldLayout.loadField( - AprilTagFields.k2025ReefscapeWelded), - PhotonPoseEstimator.PoseStrategy - .MULTI_TAG_PNP_ON_COPROCESSOR, - this.camOffset)); - limelightTable = Optional.empty(); + new PhotonPoseEstimator( + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded), + PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + this.camOffset); + limelightTable = null; } case LIMELIGHT -> { - photonCamera = Optional.empty(); - photonPoseEstimator = Optional.empty(); - limelightTable = Optional.of(NetworkTablesUtils.getTable(name)); + photonCamera = null; + photonPoseEstimator = null; + limelightTable = NetworkTablesUtils.getTable(name); } } } @@ -72,12 +71,12 @@ public Camera(String name, CameraType camType, Transform3d camOffset) { public Optional getPoseFieldSpace(Pose3d prevPosition) { switch (this.camType) { case PHOTONVISION -> { - assert this.photonPoseEstimator.isPresent(); - assert this.photonCamera.isPresent(); + assert this.photonPoseEstimator != null; + assert this.photonCamera != null; PhotonPipelineResult res; try { - res = this.photonCamera.get().getAllUnreadResults().get(0); + res = this.photonCamera.getAllUnreadResults().get(0); Transform3d worstTarget = res.getBestTarget().getBestCameraToTarget(); this.targetPose = new Pose3d(worstTarget.getTranslation(), worstTarget.getRotation()); @@ -87,9 +86,8 @@ public Optional getPoseFieldSpace(Pose3d prevPosition) { if (!res.hasTargets()) return Optional.empty(); - this.photonPoseEstimator.get().setReferencePose(prevPosition); + this.photonPoseEstimator.setReferencePose(prevPosition); return photonPoseEstimator - .get() .update(res) .map( (e) -> { @@ -105,14 +103,13 @@ public Optional getPoseFieldSpace(Pose3d prevPosition) { } private Optional getPose3dLimelight() { - assert this.limelightTable.isPresent(); + assert this.limelightTable != null; - double[] rawPose = - this.limelightTable.get().getArrayEntry("botpose_wpiblue", new double[0]); + double[] rawPose = this.limelightTable.getArrayEntry("botpose_wpiblue", new double[0]); if (rawPose.length != 6) return Optional.empty(); double[] targetPoseRaw = - this.limelightTable.get().getArrayEntry("targetpose_cameraspace", new double[0]); + this.limelightTable.getArrayEntry("targetpose_cameraspace", new double[0]); if (targetPoseRaw.length == 6) { Pose3d cameraPose = new Pose3d( @@ -138,15 +135,13 @@ private Optional getPose3dLimelight() { public double getTimestamp() { switch (this.camType) { case LIMELIGHT -> { - assert this.limelightTable.isPresent(); + assert this.limelightTable != null; return Timer.getFPGATimestamp() - - (this.limelightTable.get().getEntry("tl", Double.POSITIVE_INFINITY) - / 1000) - - (this.limelightTable.get().getEntry("cl", Double.POSITIVE_INFINITY) - / 1000); + - (this.limelightTable.getEntry("tl", Double.POSITIVE_INFINITY) / 1000) + - (this.limelightTable.getEntry("cl", Double.POSITIVE_INFINITY) / 1000); } case PHOTONVISION -> { - assert this.photonCamera.isPresent(); + assert this.photonCamera != null; return photonTimestamp; } @@ -154,6 +149,14 @@ public double getTimestamp() { return 0; } + public void setEnabled(boolean e) { + this.enabled = e; + } + + public boolean isEnabled() { + return this.enabled; + } + /** * Get the name of the camera * diff --git a/src/test/java/org/blackknights/framework/CoralQueueTest.java b/src/test/java/org/blackknights/framework/CoralQueueTest.java deleted file mode 100644 index 7d2cdd3..0000000 --- a/src/test/java/org/blackknights/framework/CoralQueueTest.java +++ /dev/null @@ -1,92 +0,0 @@ -/* Black Knights Robotics (C) 2025 */ -package org.blackknights.framework; - -import static org.junit.jupiter.api.Assertions.assertEquals; - -import edu.wpi.first.hal.AllianceStationID; -import edu.wpi.first.wpilibj.simulation.DriverStationSim; -import org.blackknights.constants.ScoringConstants; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; - -class CoralQueueTest { - private CoralQueue coralQueue; - - @BeforeEach - void setUp() { - coralQueue = new CoralQueue(); - } - - @Test - void testListToQueue() { - coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("11L4,1L1,3L3")); - CoralQueue.CoralPosition expected = - new CoralQueue.CoralPosition( - "11L4", - ScoringConstants.CORAL_POSITIONS[10], - ScoringConstants.ScoringHeights.L4); - - assertEquals(expected, coralQueue.getNext()); - - expected = - new CoralQueue.CoralPosition( - "1L1", - ScoringConstants.CORAL_POSITIONS[0], - ScoringConstants.ScoringHeights.L1); - - assertEquals(expected, coralQueue.getNext()); - - expected = - new CoralQueue.CoralPosition( - "3L3", - ScoringConstants.CORAL_POSITIONS[2], - ScoringConstants.ScoringHeights.L3); - - assertEquals(expected, coralQueue.getNext()); - } - - @Test - void testSkipNextValue() { - coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("10L4,1L1")); - coralQueue.stepForwards(); - CoralQueue.CoralPosition expected = - new CoralQueue.CoralPosition( - "1L1", - ScoringConstants.CORAL_POSITIONS[0], - ScoringConstants.ScoringHeights.L1); - assertEquals(expected, coralQueue.getNext()); - } - - @Test - void testBlueAlliancePositionMapping() { - DriverStationSim.setAllianceStationId(AllianceStationID.Blue3); - DriverStationSim.notifyNewData(); - - coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("6L1,12L4,9L4")); - - CoralQueue.CoralPosition expected = - new CoralQueue.CoralPosition( - "6L1", - ScoringConstants.CORAL_POSITIONS[5 + 12], - ScoringConstants.ScoringHeights.L1); - assertEquals(expected, coralQueue.getNext()); - - expected = - new CoralQueue.CoralPosition( - "12L4", - ScoringConstants.CORAL_POSITIONS[11 + 12], - ScoringConstants.ScoringHeights.L4); - assertEquals(expected, coralQueue.getNext()); - - expected = - new CoralQueue.CoralPosition( - "9L4", - ScoringConstants.CORAL_POSITIONS[8 + 12], - ScoringConstants.ScoringHeights.L4); - - assertEquals(expected, coralQueue.getNext()); - - DriverStationSim.setAllianceStationId(AllianceStationID.Red1); // Set back to red - DriverStationSim.notifyNewData(); - } -} diff --git a/src/test/java/org/blackknights/framework/CoralQueueTest.java.old b/src/test/java/org/blackknights/framework/CoralQueueTest.java.old new file mode 100644 index 0000000..788acdf --- /dev/null +++ b/src/test/java/org/blackknights/framework/CoralQueueTest.java.old @@ -0,0 +1,99 @@ +/// * Black Knights Robotics (C) 2025 */ +// package org.blackknights.framework; +// +// import static org.junit.jupiter.api.Assertions.assertEquals; +// +// import edu.wpi.first.hal.AllianceStationID; +// import edu.wpi.first.wpilibj.simulation.DriverStationSim; +// import org.blackknights.constants.ScoringConstants; +// import org.junit.jupiter.api.BeforeEach; +// import org.junit.jupiter.api.Test; +// +// class CoralQueueTest { +// private CoralQueue coralQueue; +// +// @BeforeEach +// void setUp() { +// coralQueue = new CoralQueue(); +// } +// +// @Test +// void testListToQueue() { +// coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("11L4,1L1,3L3")); +// CoralQueue.CoralPosition expected = +// new CoralQueue.CoralPosition( +// "11L4", +// ScoringConstants.CORAL_POSITIONS[10], +// ScoringConstants.ScoringHeights.L4, +// ScoringConstants.ScoringSides.RIGHT); +// +// assertEquals(expected, coralQueue.getNext()); +// +// expected = +// new CoralQueue.CoralPosition( +// "1L1", +// ScoringConstants.CORAL_POSITIONS[0], +// ScoringConstants.ScoringHeights.L1, +// ScoringConstants.ScoringSides.RIGHT); +// +// assertEquals(expected, coralQueue.getNext()); +// +// expected = +// new CoralQueue.CoralPosition( +// "3L3", +// ScoringConstants.CORAL_POSITIONS[2], +// ScoringConstants.ScoringHeights.L3, +// ScoringConstants.ScoringSides.RIGHT); +// +// assertEquals(expected, coralQueue.getNext()); +// } +// +// @Test +// void testSkipNextValue() { +// coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("10L4,1L1")); +// coralQueue.stepForwards(); +// CoralQueue.CoralPosition expected = +// new CoralQueue.CoralPosition( +// "1L1", +// ScoringConstants.CORAL_POSITIONS[0], +// ScoringConstants.ScoringHeights.L1, +// ScoringConstants.ScoringSides.RIGHT); +// assertEquals(expected, coralQueue.getNext()); +// } +// +// @Test +// void testBlueAlliancePositionMapping() { +// DriverStationSim.setAllianceStationId(AllianceStationID.Blue3); +// DriverStationSim.notifyNewData(); +// +// coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("6L1,12L4,9L4")); +// +// CoralQueue.CoralPosition expected = +// new CoralQueue.CoralPosition( +// "6L1", +// ScoringConstants.CORAL_POSITIONS[17], +// ScoringConstants.ScoringHeights.L1, +// ScoringConstants.ScoringSides.LEFT); +// assertEquals(expected, coralQueue.getNext()); +// +// expected = +// new CoralQueue.CoralPosition( +// "12L4", +// ScoringConstants.CORAL_POSITIONS[22], +// ScoringConstants.ScoringHeights.L4, +// ScoringConstants.ScoringSides.LEFT); +// assertEquals(expected, coralQueue.getNext()); +// +// expected = +// new CoralQueue.CoralPosition( +// "9L4", +// ScoringConstants.CORAL_POSITIONS[20], +// ScoringConstants.ScoringHeights.L4, +// ScoringConstants.ScoringSides.RIGHT); +// +// assertEquals(expected, coralQueue.getNext()); +// +// DriverStationSim.setAllianceStationId(AllianceStationID.Red1); // Set back to red +// DriverStationSim.notifyNewData(); +// } +// } From 59ac42167dcf8c4d3b94a349683c89757f5099ba Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Thu, 3 Apr 2025 18:03:36 -0600 Subject: [PATCH 14/17] index on (no branch): 95df1b2 misc: prop working code? From 19b4641a4358db7917c9483bc041db127c312975 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Fri, 4 Apr 2025 13:24:56 -0600 Subject: [PATCH 15/17] bayou: working code --- .../java/org/blackknights/RobotContainer.java | 13 ++- .../blackknights/commands/AlignCommand.java | 18 +++- .../blackknights/commands/IntakeCommand.java | 31 ++---- .../org/blackknights/framework/Odometry.java | 9 +- .../subsystems/IntakeSubsystem.java | 8 +- .../subsystems/SwerveSubsystem.java | 2 +- .../framework/CoralQueueTest.java | 99 +++++++++++++++++++ .../framework/CoralQueueTest.java.old | 99 ------------------- .../backups/tuning_2025_04_04:08:14:51.json | 1 + 9 files changed, 146 insertions(+), 134 deletions(-) create mode 100644 src/test/java/org/blackknights/framework/CoralQueueTest.java delete mode 100644 src/test/java/org/blackknights/framework/CoralQueueTest.java.old create mode 100755 tuning/backups/tuning_2025_04_04:08:14:51.json diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index e379804..4ee072d 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -78,9 +78,9 @@ public RobotContainer() { () -> new SequentialCommandGroup( getLocationPlaceCommand( - CoralQueue.CoralPosition.fromString("10L4")), + CoralQueue.CoralPosition.fromString("11L4")), getAutoIntakeCommand(IntakeSides.LEFT), - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4")), getAutoIntakeCommand(IntakeSides.LEFT), getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4")), getAutoIntakeCommand(IntakeSides.LEFT), @@ -307,7 +307,7 @@ private Command getPlaceCommand( new BaseCommand(elevatorSubsystem, armSubsystem)), new ParallelRaceGroup( new SequentialCommandGroup( - new RunCommand( + new InstantCommand( () -> { if (currentSupplier.get().getSide() == ScoringConstants.ScoringSides.LEFT) { @@ -326,7 +326,12 @@ private Command getPlaceCommand( "fine") .withTimeout( ConfigManager.getInstance() - .get("align_fine_max_time", 3.0))), + .get("align_fine_max_time", 3.0)), + new InstantCommand( + () -> { + rightCam.setEnabled(true); + leftCam.setEnabled(true); + })), new RunCommand( () -> intakeSubsystem.setSpeed( diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index f2cffb3..54ad445 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.Supplier; @@ -159,7 +160,7 @@ public void execute() { + Math.pow(robotPose.getY() - halfMoonClosePose.getY(), 2)); double trapCalc = - this.distProfile.calculate( + -this.distProfile.calculate( configManager.get("align_trap_t_sec", 0.2), new TrapezoidProfile.State( distToTarget, @@ -176,10 +177,17 @@ public void execute() { 2.0))), new TrapezoidProfile.State( 0.0, - configManager.get( - String.format( - "align_%s_ending_vel_mag", this.profile), - 1.0))) + DriverStation.isAutonomous() + ? configManager.get( + String.format( + "align_%s_auto_ending_vel_mag", + this.profile), + 0.0) + : configManager.get( + String.format( + "align_%s_ending_vel_mag", + this.profile), + 1.0))) .velocity; double a = Math.atan2(d_y, d_x); diff --git a/src/main/java/org/blackknights/commands/IntakeCommand.java b/src/main/java/org/blackknights/commands/IntakeCommand.java index 4b34ed8..c05b670 100644 --- a/src/main/java/org/blackknights/commands/IntakeCommand.java +++ b/src/main/java/org/blackknights/commands/IntakeCommand.java @@ -6,6 +6,7 @@ import org.blackknights.constants.ScoringConstants; import org.blackknights.subsystems.IntakeSubsystem; import org.blackknights.utils.ConfigManager; +import org.blackknights.utils.NetworkTablesUtils; /** Command to intake and outtake */ public class IntakeCommand extends Command { @@ -47,13 +48,14 @@ public void execute() { } case OUTTAKE: { + NetworkTablesUtils.getTable("debug/IntakeCmd") + .setEntry( + "Time running", + Timer.getFPGATimestamp() * 1000 - this.startTime); + if (Timer.getFPGATimestamp() * 1000 - this.startTime - > ConfigManager.getInstance() - .get( - String.format( - "outtake_%s_wait_time_ms", - this.height.toString().toLowerCase()), - 250)) { + > ConfigManager.getInstance().get("outtake_wait_time_ms", 250.0)) { + intakeSubsystem.setVoltage( ConfigManager.getInstance().get("outtake_speed", -8.0)); } @@ -73,22 +75,9 @@ public boolean isFinished() { || (mode.equals(IntakeMode.OUTTAKE) && !intakeSubsystem.getLinebreak() && Timer.getFPGATimestamp() * 1000 - this.startTime - > (ConfigManager.getInstance() - .get( - String.format( - "outtaking_%s_time_ms", - this.height - .toString() - .toLowerCase()), - 200) + > (ConfigManager.getInstance().get("outtaking_time_ms", 200) + ConfigManager.getInstance() - .get( - String.format( - "outtake_%s_wait_time_ms", - this.height - .toString() - .toLowerCase()), - 250))); + .get("outtake_wait_time_ms", 250))); } /** Enum of the different intake modes */ diff --git a/src/main/java/org/blackknights/framework/Odometry.java b/src/main/java/org/blackknights/framework/Odometry.java index 90920ba..79782b9 100644 --- a/src/main/java/org/blackknights/framework/Odometry.java +++ b/src/main/java/org/blackknights/framework/Odometry.java @@ -29,7 +29,7 @@ public class Odometry { private static final Logger LOGGER = LogManager.getLogger(); private final NetworkTablesUtils NTTelemetry = NetworkTablesUtils.getTable("Telemetry"); - private final NetworkTablesUtils debug = NetworkTablesUtils.getTable("debug"); + private final NetworkTablesUtils debug = NetworkTablesUtils.getTable("debug/Odometry"); private Optional targetPose = Optional.of(new Pose3d()); @@ -174,14 +174,14 @@ public void periodic() { }); for (Camera c : this.cameras.values()) { Optional pose = c.getPoseFieldSpace(this.getRobotPose()); - if (pose.isPresent()) { + debug.setEntry(String.format("%s/enabled", c.getName()), c.isEnabled()); + if (pose.isPresent() && c.isEnabled()) { double dist = Math.sqrt( Math.pow(c.getTargetPose().getX(), 2) + Math.pow(c.getTargetPose().getY(), 2)); debug.setEntry(String.format("%s/dist_to_target", c.getName()), dist); - debug.setEntry(String.format("%s/enabled", c.getName()), c.isEnabled()); debug.setArrayEntry( String.format("%s/pose", c.getName()), new double[] { @@ -190,6 +190,7 @@ public void periodic() { if (dist <= ConfigManager.getInstance().get("vision_cutoff_distance", 3) && dist > ConfigManager.getInstance().get("vision_min_distance", 0.5)) { + debug.setEntry(String.format("%s/Adding target", c.getName()), true); this.hasSeenTarget = true; LOGGER.debug("Added vision measurement from `{}`", c.getName()); @@ -201,6 +202,8 @@ public void periodic() { c.getTargetPose().getZ(), c.getTargetPose().getRotation())); this.poseEstimator.addVisionMeasurement(pose.get(), c.getTimestamp()); + } else { + debug.setEntry(String.format("%s/Adding target", c.getName()), false); } } else { this.targetPose = Optional.empty(); diff --git a/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java b/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java index 27a10de..2a9bec8 100644 --- a/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java @@ -5,12 +5,13 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.blackknights.constants.ArmConstants; +import org.blackknights.utils.NetworkTablesUtils; /** Subsystem to manage the intake (NOT HAND) */ public class IntakeSubsystem extends SubsystemBase { private final WPI_TalonSRX motor = new WPI_TalonSRX(ArmConstants.MOTOR_ID); - private final DigitalInput intakeLinebreak = new DigitalInput(0); + private final DigitalInput intakeLinebreak = new DigitalInput(1); /** Create a new intake subsystem */ public IntakeSubsystem() { @@ -41,4 +42,9 @@ public void setVoltage(double voltage) { public boolean getLinebreak() { return !intakeLinebreak.get(); } + + @Override + public void periodic() { + NetworkTablesUtils.getTable("debug").setEntry("Intake/linebreak", this.getLinebreak()); + } } diff --git a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java index 695829b..9c734fb 100644 --- a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java @@ -55,7 +55,7 @@ public class SwerveSubsystem extends SubsystemBase { DrivetrainConstants.REAR_RIGHT_CHASSIS_ANGULAR_OFFSET); // The gyro sensor - private final AHRS gyro = new AHRS(AHRS.NavXComType.kMXP_SPI); + private final AHRS gyro = new AHRS(AHRS.NavXComType.kUSB1); // Slew Rate Constants private double currentRotation = 0.0; diff --git a/src/test/java/org/blackknights/framework/CoralQueueTest.java b/src/test/java/org/blackknights/framework/CoralQueueTest.java new file mode 100644 index 0000000..0527395 --- /dev/null +++ b/src/test/java/org/blackknights/framework/CoralQueueTest.java @@ -0,0 +1,99 @@ +/* Black Knights Robotics (C) 2025 */ +package org.blackknights.framework; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.hal.AllianceStationID; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import org.blackknights.constants.ScoringConstants; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class CoralQueueTest { + private CoralQueue coralQueue; + + @BeforeEach + void setUp() { + coralQueue = new CoralQueue(); + } + + @Test + void testListToQueue() { + coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("11L4,1L1,3L3")); + CoralQueue.CoralPosition expected = + new CoralQueue.CoralPosition( + "11L4", + ScoringConstants.CORAL_POSITIONS[10], + ScoringConstants.ScoringHeights.L4, + ScoringConstants.ScoringSides.RIGHT); + + assertEquals(expected, coralQueue.getNext()); + + expected = + new CoralQueue.CoralPosition( + "1L1", + ScoringConstants.CORAL_POSITIONS[0], + ScoringConstants.ScoringHeights.L1, + ScoringConstants.ScoringSides.RIGHT); + + assertEquals(expected, coralQueue.getNext()); + + expected = + new CoralQueue.CoralPosition( + "3L3", + ScoringConstants.CORAL_POSITIONS[2], + ScoringConstants.ScoringHeights.L3, + ScoringConstants.ScoringSides.RIGHT); + + assertEquals(expected, coralQueue.getNext()); + } + + @Test + void testSkipNextValue() { + coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("10L4,1L1")); + coralQueue.stepForwards(); + CoralQueue.CoralPosition expected = + new CoralQueue.CoralPosition( + "1L1", + ScoringConstants.CORAL_POSITIONS[0], + ScoringConstants.ScoringHeights.L1, + ScoringConstants.ScoringSides.RIGHT); + assertEquals(expected, coralQueue.getNext()); + } + + @Test + void testBlueAlliancePositionMapping() { + DriverStationSim.setAllianceStationId(AllianceStationID.Blue3); + DriverStationSim.notifyNewData(); + + coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("6L1,12L4,9L4")); + + CoralQueue.CoralPosition expected = + new CoralQueue.CoralPosition( + "6L1", + ScoringConstants.CORAL_POSITIONS[17], + ScoringConstants.ScoringHeights.L1, + ScoringConstants.ScoringSides.LEFT); + assertEquals(expected, coralQueue.getNext()); + + expected = + new CoralQueue.CoralPosition( + "12L4", + ScoringConstants.CORAL_POSITIONS[23], + ScoringConstants.ScoringHeights.L4, + ScoringConstants.ScoringSides.LEFT); + assertEquals(expected, coralQueue.getNext()); + + expected = + new CoralQueue.CoralPosition( + "9L4", + ScoringConstants.CORAL_POSITIONS[20], + ScoringConstants.ScoringHeights.L4, + ScoringConstants.ScoringSides.RIGHT); + + assertEquals(expected, coralQueue.getNext()); + + DriverStationSim.setAllianceStationId(AllianceStationID.Red1); // Set back to red + DriverStationSim.notifyNewData(); + } +} diff --git a/src/test/java/org/blackknights/framework/CoralQueueTest.java.old b/src/test/java/org/blackknights/framework/CoralQueueTest.java.old deleted file mode 100644 index 788acdf..0000000 --- a/src/test/java/org/blackknights/framework/CoralQueueTest.java.old +++ /dev/null @@ -1,99 +0,0 @@ -/// * Black Knights Robotics (C) 2025 */ -// package org.blackknights.framework; -// -// import static org.junit.jupiter.api.Assertions.assertEquals; -// -// import edu.wpi.first.hal.AllianceStationID; -// import edu.wpi.first.wpilibj.simulation.DriverStationSim; -// import org.blackknights.constants.ScoringConstants; -// import org.junit.jupiter.api.BeforeEach; -// import org.junit.jupiter.api.Test; -// -// class CoralQueueTest { -// private CoralQueue coralQueue; -// -// @BeforeEach -// void setUp() { -// coralQueue = new CoralQueue(); -// } -// -// @Test -// void testListToQueue() { -// coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("11L4,1L1,3L3")); -// CoralQueue.CoralPosition expected = -// new CoralQueue.CoralPosition( -// "11L4", -// ScoringConstants.CORAL_POSITIONS[10], -// ScoringConstants.ScoringHeights.L4, -// ScoringConstants.ScoringSides.RIGHT); -// -// assertEquals(expected, coralQueue.getNext()); -// -// expected = -// new CoralQueue.CoralPosition( -// "1L1", -// ScoringConstants.CORAL_POSITIONS[0], -// ScoringConstants.ScoringHeights.L1, -// ScoringConstants.ScoringSides.RIGHT); -// -// assertEquals(expected, coralQueue.getNext()); -// -// expected = -// new CoralQueue.CoralPosition( -// "3L3", -// ScoringConstants.CORAL_POSITIONS[2], -// ScoringConstants.ScoringHeights.L3, -// ScoringConstants.ScoringSides.RIGHT); -// -// assertEquals(expected, coralQueue.getNext()); -// } -// -// @Test -// void testSkipNextValue() { -// coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("10L4,1L1")); -// coralQueue.stepForwards(); -// CoralQueue.CoralPosition expected = -// new CoralQueue.CoralPosition( -// "1L1", -// ScoringConstants.CORAL_POSITIONS[0], -// ScoringConstants.ScoringHeights.L1, -// ScoringConstants.ScoringSides.RIGHT); -// assertEquals(expected, coralQueue.getNext()); -// } -// -// @Test -// void testBlueAlliancePositionMapping() { -// DriverStationSim.setAllianceStationId(AllianceStationID.Blue3); -// DriverStationSim.notifyNewData(); -// -// coralQueue.loadProfile(CoralQueue.CoralQueueProfile.fromString("6L1,12L4,9L4")); -// -// CoralQueue.CoralPosition expected = -// new CoralQueue.CoralPosition( -// "6L1", -// ScoringConstants.CORAL_POSITIONS[17], -// ScoringConstants.ScoringHeights.L1, -// ScoringConstants.ScoringSides.LEFT); -// assertEquals(expected, coralQueue.getNext()); -// -// expected = -// new CoralQueue.CoralPosition( -// "12L4", -// ScoringConstants.CORAL_POSITIONS[22], -// ScoringConstants.ScoringHeights.L4, -// ScoringConstants.ScoringSides.LEFT); -// assertEquals(expected, coralQueue.getNext()); -// -// expected = -// new CoralQueue.CoralPosition( -// "9L4", -// ScoringConstants.CORAL_POSITIONS[20], -// ScoringConstants.ScoringHeights.L4, -// ScoringConstants.ScoringSides.RIGHT); -// -// assertEquals(expected, coralQueue.getNext()); -// -// DriverStationSim.setAllianceStationId(AllianceStationID.Red1); // Set back to red -// DriverStationSim.notifyNewData(); -// } -// } diff --git a/tuning/backups/tuning_2025_04_04:08:14:51.json b/tuning/backups/tuning_2025_04_04:08:14:51.json new file mode 100755 index 0000000..6a61b10 --- /dev/null +++ b/tuning/backups/tuning_2025_04_04:08:14:51.json @@ -0,0 +1 @@ +{"align_fine_halfmoon_dist":0.5,"shooter_high_pass":0.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"left_cam_x":0.253,"align_fine_x_max_vel_m":1.0,"align_close_y_p":3.1,"vision_min_distance":0.5,"inf_switch_dist":2.5,"shooter_low_pass":0.0,"align_rough_max_accel_mps":8.5,"align_close_y_d":0.1,"drive_magnitude_slew_rate":22.0,"outtaking_time_ms":450.0,"outtake_speed":10,"align_fine_y_max_vel_m":1.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":727.0,"align_ff":0.1,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_fine_ending_vel_mag":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":4.5,"odom_vision_trust":0.5,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.5,"arm_intake":0.75,"swerve_drive_i":0.0,"align_rough_rot_tol_deg":90.0,"swerve_drive_d":0.0,"align_fine_vel_tol":0.25,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.25,"swerve_drive_p":0.04,"outtake_wait_time_ms":250.0,"outtake_l4_wait_time_ms":250,"align_rough_y_target_end_vel":0.0,"scoring_left_x":0.6,"align_fine_max_vel_m":2.0,"scoring_left_y":0.16,"align_fine_pos_tolerance":0.05,"center_cam_pitch":-45.0,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_fine_rot_vel_tol":0.005,"align_x_axis_p":3,"swerve_max_translational_speed_mps":4.5,"align_rough_x_max_accel_mps":4.5,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_x_max_vel_m":4.5,"align_rot_i":0.0,"align_x_axis_i":0.0,"intake_speed":-8,"align_rot_p":7.3,"odom_vision_trust_theta":0.08726646259971647,"align_rough_back":0.5,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"arm_max_accel_degs":90.0,"elevator_d":0.0,"align_rough_min_vel":0.01,"l1_elevator_position":1.0,"outtaking_l4_time_ms":200,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_rotation_ff_kv":0.0,"align_rotation_ff_ks":0.04,"arm_i":0.0,"arm_d":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"align_rough_halfmoon_dist":0.5,"placeholder":0.0,"align_fine_ms_before_rotate":0.0,"scoring_right_x":0.6,"align_rough_x_target_end_vel":0.0,"drive_direction_slew_rate":0.1,"scoring_right_y":-0.14,"elevator_intake":0.09,"align_fine_max_accel_mps":2.0,"align_fine_max_time":3.0,"align_rough_halfmoon_tol":0.0,"outtake_l1_wait_time_ms":250,"align_fine_rotation_tolerance":1,"align_y_max_vel_m":4.5,"align_vel_tol":0.25,"align_rough_finish_time":0.0,"align_rough_rot_max_accel_degps":720,"align_rough_x_max_vel_m":4.5,"align_enable_slow_mode":false,"back_time_sec":1.0,"align_close_speed_scaler":0.5,"align_rough_ms_before_rotate":0.0,"align_close__y_i":0.0,"align_close_distance":1.0,"outtake_max_time_sec":5.0,"align_rough_max_vel_m":5.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":3.5,"align_rough_rotation_tolerance":40,"align_rough_vel_tol":0.5,"arm_tol":1.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","swerve_max_rotation_velocity_radsps":10.5,"swerve_turning_p":1.0,"right_cam_x":0.163,"swerve_turning_i":0.0,"swerve_turning_d":0.0,"max_roation_diff":3.0,"swerve_abs_max_module_speed_mps":4.5,"elevator_pid_tolerance":0.05,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"align_x_max_accel_mps":3.5,"vision_cutoff_distance":3.0,"align_fine_rot_tol_deg":3,"elevator_current_max":20.0,"outtaking_l1_time_ms":200,"align_dist_back":1.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_fine_y_target_end_vel":0.0,"align_x_p":3.0,"arm_max_vel_degs":90.0,"l4_elevator_position":0.0,"align_x_d":0.25,"controller_deadband":0.2,"align_pos_rough_tol":0.25,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_y_axis_d":0.25,"driver_max_speed_rot":325,"align_slow_more":false,"align_auto_finish_time":200.0,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"shooter_speaker":0.0,"align_fine_pos_dist_tol":0.05,"align_ff_ks":1.0,"back_mps":-3.0,"align_auto_rotation_tolerance":1,"align_rough_ending_vel_mag":0.5,"align_ff_kv":1.0,"elevator_l1":0.2,"elevator_l2":0.45,"elevator_l3":0.982,"swerve_min_velocity":0.01,"elevator_l4":1.67,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":4.0,"align_y_max_accel_mps":3.5,"align_rough_rot_max_vel_deg":720,"align_fine_min_vel":0.002,"align_close_rot_p":3.0,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":4.0,"align_trap_t_sec":0.1,"arm_kg":0.0,"arm_encoder_offset":-0.153,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_fine_x_target_end_vel":0.0,"align_slow_mode":false,"test_drive_y":0.0,"right_cam_angle":5.0,"test_drive_x":0.1,"elevator_zero_voltage":-3.0,"autointake_dist_back":-0.25,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.5,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"elevator_zeroing_voltage":0.0,"fake_pose_dist_back":0.5} From 314bedbcadd454ae94b7437bbca2fa70da559e76 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Fri, 4 Apr 2025 17:28:12 -0600 Subject: [PATCH 16/17] bayou: End of day --- .../java/org/blackknights/RobotContainer.java | 27 ++++---------- .../blackknights/commands/IntakeCommand.java | 37 ++++++++++++++----- .../subsystems/ElevatorSubsystem.java | 5 +++ .../backups/tuning_2025_04_04:14:26:12.json | 1 + 4 files changed, 40 insertions(+), 30 deletions(-) create mode 100755 tuning/backups/tuning_2025_04_04:14:26:12.json diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index 4ee072d..3d72fd9 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -82,7 +82,7 @@ public RobotContainer() { getAutoIntakeCommand(IntakeSides.LEFT), getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4")), getAutoIntakeCommand(IntakeSides.LEFT), - getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4")), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")), getAutoIntakeCommand(IntakeSides.LEFT), getLocationPlaceCommand( CoralQueue.CoralPosition.fromString("7L4")))); @@ -151,9 +151,7 @@ private void configureBindings() { armSubsystem, () -> ScoringConstants.ScoringHeights.INTAKE), new IntakeCommand( - intakeSubsystem, - IntakeCommand.IntakeMode.INTAKE, - ScoringConstants.ScoringHeights.L1)), + intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)), new RunCommand( () -> swerveSubsystem.drive( @@ -224,19 +222,11 @@ private void configureBindings() { secondaryController .rightTrigger(0.2) - .whileTrue( - new IntakeCommand( - intakeSubsystem, - IntakeCommand.IntakeMode.OUTTAKE, - ScoringConstants.ScoringHeights.L1)); + .whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)); secondaryController .leftTrigger(0.2) - .whileTrue( - new IntakeCommand( - intakeSubsystem, - IntakeCommand.IntakeMode.INTAKE, - ScoringConstants.ScoringHeights.L1)); + .whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)); } /** Runs once when the code starts */ @@ -346,11 +336,11 @@ private Command getPlaceCommand( new ElevatorArmCommand( elevatorSubsystem, armSubsystem, - () -> currentSupplier.get().getHeight()), + () -> nextSupplier.get().getHeight()), new IntakeCommand( intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE, - nextSupplier.get().getHeight()) + elevatorSubsystem.isAtTargetSupplier()) .withTimeout( ConfigManager.getInstance() .get("outtake_max_time_sec", 5.0))), @@ -394,10 +384,7 @@ private Command getAutoIntakeCommand(IntakeSides side) { intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI))); return new ParallelRaceGroup( - new IntakeCommand( - intakeSubsystem, - IntakeCommand.IntakeMode.INTAKE, - ScoringConstants.ScoringHeights.L1), + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE), new ParallelCommandGroup( new AlignCommand( swerveSubsystem, diff --git a/src/main/java/org/blackknights/commands/IntakeCommand.java b/src/main/java/org/blackknights/commands/IntakeCommand.java index c05b670..fd72bc0 100644 --- a/src/main/java/org/blackknights/commands/IntakeCommand.java +++ b/src/main/java/org/blackknights/commands/IntakeCommand.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import org.blackknights.constants.ScoringConstants; +import java.util.function.BooleanSupplier; import org.blackknights.subsystems.IntakeSubsystem; import org.blackknights.utils.ConfigManager; import org.blackknights.utils.NetworkTablesUtils; @@ -12,9 +12,22 @@ public class IntakeCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final IntakeMode mode; - private final ScoringConstants.ScoringHeights height; + private final BooleanSupplier elevatorAtTargetSupplier; - private double startTime; + private double elevatorAtTargetTime; + + /** + * Create a new intake command + * + * @param intakeSubsystem The instance of {@link IntakeSubsystem} + * @param mode The intake mode ({@link IntakeMode}) + */ + public IntakeCommand(IntakeSubsystem intakeSubsystem, IntakeMode mode) { + this.elevatorAtTargetSupplier = () -> true; + this.intakeSubsystem = intakeSubsystem; + this.mode = mode; + addRequirements(intakeSubsystem); + } /** * Create a new intake command @@ -25,20 +38,23 @@ public class IntakeCommand extends Command { public IntakeCommand( IntakeSubsystem intakeSubsystem, IntakeMode mode, - ScoringConstants.ScoringHeights height) { + BooleanSupplier elevatorAtTargetSupplier) { + this.elevatorAtTargetSupplier = elevatorAtTargetSupplier; this.intakeSubsystem = intakeSubsystem; this.mode = mode; - this.height = height; addRequirements(intakeSubsystem); } @Override public void initialize() { - this.startTime = Timer.getFPGATimestamp() * 1000; + this.elevatorAtTargetTime = 0; } @Override public void execute() { + if (this.elevatorAtTargetTime == 0 && elevatorAtTargetSupplier.getAsBoolean()) { + this.elevatorAtTargetTime = Timer.getFPGATimestamp() * 1000; + } switch (mode) { case INTAKE: { @@ -51,10 +67,11 @@ public void execute() { NetworkTablesUtils.getTable("debug/IntakeCmd") .setEntry( "Time running", - Timer.getFPGATimestamp() * 1000 - this.startTime); + Timer.getFPGATimestamp() * 1000 - this.elevatorAtTargetTime); - if (Timer.getFPGATimestamp() * 1000 - this.startTime - > ConfigManager.getInstance().get("outtake_wait_time_ms", 250.0)) { + if (Timer.getFPGATimestamp() * 1000 - this.elevatorAtTargetTime + > ConfigManager.getInstance().get("outtake_wait_time_ms", 250.0) + && elevatorAtTargetSupplier.getAsBoolean()) { intakeSubsystem.setVoltage( ConfigManager.getInstance().get("outtake_speed", -8.0)); @@ -74,7 +91,7 @@ public boolean isFinished() { return (mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak()) || (mode.equals(IntakeMode.OUTTAKE) && !intakeSubsystem.getLinebreak() - && Timer.getFPGATimestamp() * 1000 - this.startTime + && Timer.getFPGATimestamp() * 1000 - this.elevatorAtTargetTime > (ConfigManager.getInstance().get("outtaking_time_ms", 200) + ConfigManager.getInstance() .get("outtake_wait_time_ms", 250))); diff --git a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java index 2ea844c..47bc3c7 100644 --- a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java @@ -13,6 +13,7 @@ import edu.wpi.first.networktables.DoubleEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.BooleanSupplier; import org.blackknights.constants.ElevatorConstants; import org.blackknights.utils.ConfigManager; @@ -206,6 +207,10 @@ public double getElevatorVelocity() { return encoderAverageVel * ElevatorConstants.ROTATIONS_TO_METERS; } + public BooleanSupplier isAtTargetSupplier() { + return this.elevatorPID::atGoal; + } + /** * Check if elevator is at target position * diff --git a/tuning/backups/tuning_2025_04_04:14:26:12.json b/tuning/backups/tuning_2025_04_04:14:26:12.json new file mode 100755 index 0000000..899f2e0 --- /dev/null +++ b/tuning/backups/tuning_2025_04_04:14:26:12.json @@ -0,0 +1 @@ +{"align_fine_halfmoon_dist":0.5,"shooter_high_pass":0.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"left_cam_x":0.253,"align_fine_x_max_vel_m":1.0,"align_close_y_p":3.1,"vision_min_distance":0.5,"inf_switch_dist":2.5,"shooter_low_pass":0.0,"align_rough_max_accel_mps":8.5,"align_close_y_d":0.1,"drive_magnitude_slew_rate":22.0,"outtaking_time_ms":200.0,"outtake_speed":10,"align_fine_y_max_vel_m":1.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":727.0,"align_ff":0.1,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_fine_ending_vel_mag":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":4.5,"odom_vision_trust":0.5,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.5,"arm_intake":0.75,"swerve_drive_i":0.0,"align_rough_rot_tol_deg":90.0,"swerve_drive_d":0.0,"align_fine_vel_tol":0.25,"align_close_x_d":0.1,"align_rough_pos_dist_tol":0.1,"swerve_drive_p":0.04,"outtake_wait_time_ms":0.0,"outtake_l4_wait_time_ms":10000,"align_rough_y_target_end_vel":0.0,"scoring_left_x":0.6,"align_fine_max_vel_m":2.0,"scoring_left_y":0.18,"align_fine_pos_tolerance":0.05,"center_cam_pitch":-45.0,"arm_movement_max":0.67,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_fine_rot_vel_tol":0.005,"align_x_axis_p":3,"swerve_max_translational_speed_mps":4.5,"align_rough_x_max_accel_mps":4.5,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_x_max_vel_m":4.5,"align_rot_i":0.0,"align_x_axis_i":0.0,"intake_speed":-8,"align_rot_p":7.3,"odom_vision_trust_theta":0.08726646259971647,"align_rough_back":0.5,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":30.0,"arm_max_accel_degs":90.0,"elevator_d":0.0,"align_rough_min_vel":0.01,"l1_elevator_position":1.0,"outtaking_l4_time_ms":200,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"arm_p":0.7,"align_fine_rot_max_vel_deg":360,"align_rotation_ff_kv":0.0,"align_rotation_ff_ks":0.04,"arm_i":0.0,"arm_d":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":4.0,"align_rough_halfmoon_dist":0.5,"placeholder":0.0,"align_fine_ms_before_rotate":0.0,"scoring_right_x":0.6,"align_rough_x_target_end_vel":0.0,"drive_direction_slew_rate":0.1,"scoring_right_y":-0.17,"elevator_intake":0.09,"align_fine_max_accel_mps":2.0,"align_fine_max_time":8.0,"align_rough_halfmoon_tol":0.0,"outtake_l1_wait_time_ms":250,"align_fine_rotation_tolerance":1,"align_y_max_vel_m":4.5,"align_vel_tol":0.25,"align_rough_finish_time":0.0,"align_rough_rot_max_accel_degps":720,"align_rough_x_max_vel_m":4.5,"align_enable_slow_mode":false,"back_time_sec":1.0,"align_close_speed_scaler":0.5,"align_rough_ms_before_rotate":0.0,"align_close__y_i":0.0,"align_close_distance":1.0,"outtake_max_time_sec":5.0,"align_rough_max_vel_m":5.0,"align_rough_pos_tolerance":0.01,"driver_max_speed":3.5,"align_rough_rotation_tolerance":40,"align_rough_vel_tol":0.5,"arm_tol":1.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","swerve_max_rotation_velocity_radsps":10.5,"swerve_turning_p":1.0,"right_cam_x":0.163,"swerve_turning_i":0.0,"swerve_turning_d":0.0,"max_roation_diff":3.0,"swerve_abs_max_module_speed_mps":4.5,"elevator_pid_tolerance":0.05,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.0,"align_auto_rot_max_accel_degps":360,"align_x_max_accel_mps":3.5,"vision_cutoff_distance":3.0,"align_fine_rot_tol_deg":3,"elevator_current_max":20.0,"outtaking_l1_time_ms":200,"align_dist_back":1.0,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_fine_y_target_end_vel":0.0,"align_x_p":3.0,"arm_max_vel_degs":90.0,"l4_elevator_position":0.0,"align_x_d":0.25,"controller_deadband":0.2,"align_pos_rough_tol":0.25,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_y_axis_d":0.25,"driver_max_speed_rot":325,"align_slow_more":false,"align_auto_finish_time":200.0,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"shooter_speaker":0.0,"align_fine_pos_dist_tol":0.02,"align_ff_ks":1.0,"back_mps":-3.0,"align_auto_rotation_tolerance":1,"align_rough_ending_vel_mag":0.0,"align_ff_kv":1.0,"elevator_l1":0.2,"elevator_l2":0.45,"elevator_l3":0.982,"swerve_min_velocity":0.01,"elevator_l4":1.67,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":4.0,"align_y_max_accel_mps":3.5,"align_rough_rot_max_vel_deg":720,"align_fine_min_vel":0.002,"align_close_rot_p":3.0,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"auto_place_backup_time_ms":100.0,"arm_ks":0.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":4.0,"align_trap_t_sec":0.1,"arm_kg":0.0,"arm_encoder_offset":-0.153,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_fine_x_target_end_vel":0.0,"align_slow_mode":false,"test_drive_y":0.0,"right_cam_angle":5.0,"test_drive_x":0.1,"elevator_zero_voltage":-3.0,"autointake_dist_back":-0.25,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.5,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"elevator_zeroing_voltage":0.0,"fake_pose_dist_back":0.5} From ed20bed2d588b03d0bcf6f335c5b086d80466205 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sat, 5 Apr 2025 11:59:26 -0600 Subject: [PATCH 17/17] bayou: All code after quals --- .../java/org/blackknights/RobotContainer.java | 24 +++++++++++++++++++ .../subsystems/ElevatorSubsystem.java | 13 ++++++++++ 2 files changed, 37 insertions(+) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index 3d72fd9..3ede56a 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -171,6 +171,30 @@ private void configureBindings() { primaryController.povDown().whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro())); + primaryController + .a() + .whileTrue( + new RunCommand( + () -> + elevatorSubsystem.setVoltage( + ConfigManager.getInstance() + .get("elevator_manual_zero", -2.0)), + elevatorSubsystem)); + + primaryController + .x() + .whileTrue(new InstantCommand(() -> elevatorSubsystem.resetEncoders())); + + // primaryController + // .povUp() + // .whileTrue( + // new InstantCommand( + // () -> + // elevatorSubsystem.setRightEncoder( + // ConfigManager.getInstance() + // .get("break_right_encoder_pos", + // -10.0)))); + secondaryController .rightStick() .onTrue(new InstantCommand(ScoringConstants::recomputeCoralPositions)); diff --git a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java index 47bc3c7..06a30ed 100644 --- a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java @@ -179,6 +179,19 @@ public double getRightEncoderPosition() { return rightEncoder.getPosition(); } + public void resetEncoders() { + rightEncoder.setPosition(0.0); + leftEncoder.setPosition(0.0); + } + + public void setRightEncoder(double position) { + rightEncoder.setPosition(position); + } + + public void setLeftEncoder(double position) { + leftEncoder.setPosition(position); + } + /** Reset elevator PID */ public void resetPID() { elevatorPID.reset(getElevatorPosition());