From c3e35182abf9e2db71247bf57c7e1c2bdedc3a81 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Mon, 5 Feb 2024 21:57:23 -0800 Subject: [PATCH 01/20] added the drive to note position code back --- .../robot/subsystems/LimelightSubsystem.java | 202 ++++++++++++++++++ 1 file changed, 202 insertions(+) create mode 100644 src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java new file mode 100644 index 00000000..ae542189 --- /dev/null +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -0,0 +1,202 @@ +package frc.team2412.robot.subsystems; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import frc.team2412.robot.subsystems.DrivebaseSubsystem; + +public class LimelightSubsystem extends SubsystemBase { + + // CONSTANTS + + private static final PIDController TRANSLATION_PID = new PIDController(10.0, 0, 0); + private static final PIDController ROTATION_PID = new PIDController(8.0, 0, 0); + + // meters? + public static final double CAMERA_MOUNT_HEIGHT = 0.1143; + public static final double CAMERA_ANGLE_OFFSET = 0; + public static final double TARGET_HEIGHT = 0.33; + + public static final double GOAL_DISTANCE_FROM_TARGET = 0.7; + public static final double GOAL_DISTANCE_FROM_CONE = 0.3; + public static final double GOAL_DISTANCE_FROM_CUBE = 0.3; + + // MEMBERS + + NetworkTable networkTable; + + String currentPoseString; + String targetPoseString; + + // network tables + + // CONSTRUCTOR ! + public LimelightSubsystem() { + + // broadcast 10.24.12.227 + + // logging + currentPoseString = ""; + targetPoseString = ""; + + networkTable = NetworkTableInstance.getDefault().getTable("limelight"); + ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight"); + + limelightTab.addBoolean("hasTarget", this::hasTargets).withPosition(0, 0).withSize(1, 1); + limelightTab + .addDouble("Horizontal Offset", this::getHorizontalOffset) + .withPosition(1, 0) + .withSize(1, 1); + limelightTab + .addDouble("Vertical Offset", this::getVerticalOffset) + .withPosition(2, 0) + .withSize(1, 1); + + limelightTab + .addDouble("Target Distance ", this::getDistanceFromTarget) + .withPosition(3, 0) + .withSize(1, 1); + limelightTab + .addDouble("Target Distance 2 - TEST ", this::getDistanceFromTargetTheSecond) + .withPosition(4, 0) + .withSize(1, 1); + limelightTab + .addDouble("Limelight Based Turn Power - TEST ", this::turnPower) + .withPosition(5, 0) + .withSize(1, 1); + limelightTab + .addString("Current Pose ", this::getCurrentPoseString) + .withPosition(0, 1) + .withSize(4, 1); + + limelightTab + .addString("Target Pose ", this::getTargetPoseString) + .withPosition(0, 2) + .withSize(4, 1); + } + + // METHODS + + public boolean hasTargets() { + return (networkTable.getEntry("tv").getDouble(0) != 0); + } + + public double getHorizontalOffset() { + return networkTable.getEntry("tx").getDouble(0); + } + + public double getVerticalOffset() { + return networkTable.getEntry("ty").getDouble(0); + } + + public double getBoxWidth() { + return networkTable.getEntry("tshort").getDouble(0); + } + + public double getDistanceFromTarget() { + + // 1.3 at 1.9 + // 1.0 at 1.3 + + double angleToTarget = getVerticalOffset(); + // return (TARGET_HEIGHT - CAMERA_MOUNT_HEIGHT) / Math.tan(CAMERA_ANGLE_OFFSET + angleToTarget); + + return (TARGET_HEIGHT - CAMERA_MOUNT_HEIGHT) + / (CAMERA_ANGLE_OFFSET + Math.tan(Units.degreesToRadians(getVerticalOffset()))); + } + + public double getDistanceFromTargetTheSecond() { + + // focal length = (P x D) / W + double focal_length = 346.1818; + + // distance = (W x F) / P + // returns inches for testing purposes, will divide by 39.3700787 to return meters + return (8.25 * focal_length) / getBoxWidth(); + } + + public double turnPower() { + return MathUtil.clamp( + (Math.signum(getHorizontalOffset()) + * (Math.pow(Math.abs(getHorizontalOffset()), 1.8) / 100)), + -1.5, + 1.5); + } + + // tan(degree) * distance = sideways distance + + // target height / tan(vertical angle) + + // TODO fix this or something + public Pose2d getTargetPose(Pose2d currentPose) { + + // math thing to get target pose using current pose + + Rotation2d targetHeading = + new Rotation2d( + currentPose.getRotation().getRadians() + Units.degreesToRadians(getHorizontalOffset())); + double targetDistance = getDistanceFromTargetTheSecond() / 39.3700787; + + double targetX = Math.sin(targetHeading.getRadians()) * targetDistance; + double targetY = Math.cos(targetHeading.getRadians()) * targetDistance; + + Pose2d targetPose = + new Pose2d(currentPose.getX() + targetY, currentPose.getY() + targetX, targetHeading); + + currentPoseString = currentPose.toString(); + targetPoseString = targetPose.toString(); + + return targetPose; + } + + public String getCurrentPoseString() { + return currentPoseString; + } + + public String getTargetPoseString() { + return targetPoseString; + } + + public boolean isWithinDistance() { + return (getDistanceFromTarget() <= GOAL_DISTANCE_FROM_TARGET); + } + + public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebaseSubsystem) { + final DoubleSupplier returnZero = () -> 0.0; + final Supplier returnTurn = () -> Rotation2d.fromDegrees(getHorizontalOffset()); + final DoubleSupplier returnDrive = () -> Math.abs(turnPower()) / 2; + Command moveCommand; + Pose2d targetPose; + if (hasTargets()) { + targetPose = getTargetPose(currentPose); + moveCommand = new DrivebaseSubsystem().driveJoystick(() -> 0.0, () -> 0.0, returnTurn); + + } else { + targetPose = currentPose; + moveCommand = new DrivebaseSubsystem().driveJoystick(() -> 0.0, () -> 0.0, returnTurn); + } + + + // create path + + return moveCommand; + } + + @Override + public void periodic() {} +} From d38721f85aebda5280a798a96621c5e4e29ec064 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Tue, 6 Feb 2024 19:26:42 -0800 Subject: [PATCH 02/20] Implemented limelight get to distance command, should be able to run on crane on Thursday :) --- .../robot/subsystems/LimelightSubsystem.java | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index ae542189..80f96abb 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -9,16 +9,10 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import frc.team2412.robot.subsystems.DrivebaseSubsystem; public class LimelightSubsystem extends SubsystemBase { @@ -175,7 +169,7 @@ public String getTargetPoseString() { public boolean isWithinDistance() { return (getDistanceFromTarget() <= GOAL_DISTANCE_FROM_TARGET); } - + public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebaseSubsystem) { final DoubleSupplier returnZero = () -> 0.0; final Supplier returnTurn = () -> Rotation2d.fromDegrees(getHorizontalOffset()); @@ -191,12 +185,11 @@ public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebas moveCommand = new DrivebaseSubsystem().driveJoystick(() -> 0.0, () -> 0.0, returnTurn); } - // create path return moveCommand; } - + @Override public void periodic() {} } From 3112274d066793859239eb12a055b2f96a4a005a Mon Sep 17 00:00:00 2001 From: Jesse S Date: Tue, 6 Feb 2024 19:27:59 -0800 Subject: [PATCH 03/20] forgor to commit all the files in the last push --- .../java/frc/team2412/robot/Controls.java | 15 ++++++ .../java/frc/team2412/robot/Subsystems.java | 5 ++ .../limelight/GetWithinDistanceCommand.java | 48 +++++++++++++++++++ .../robot/subsystems/LimelightSubsystem.java | 13 +++-- 4 files changed, 78 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 37bb15e8..52d8e078 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -3,11 +3,15 @@ import static frc.team2412.robot.Controls.ControlConstants.CODRIVER_CONTROLLER_PORT; import static frc.team2412.robot.Controls.ControlConstants.CONTROLLER_PORT; import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; +import static frc.team2412.robot.Subsystems.SubsystemConstants.LIMELIGHT_ENABLED; import edu.wpi.first.math.geometry.Rotation2d; +import frc.team2412.robot.subsystems.*; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.team2412.robot.commands.limelight.GetWithinDistanceCommand; public class Controls { public static class ControlConstants { @@ -17,17 +21,23 @@ public static class ControlConstants { private final CommandXboxController driveController; private final CommandXboxController codriveController; + private final Trigger getWithinDistanceTrigger; private final Subsystems s; public Controls(Subsystems s) { driveController = new CommandXboxController(CONTROLLER_PORT); codriveController = new CommandXboxController(CODRIVER_CONTROLLER_PORT); + getWithinDistanceTrigger = driveController.start(); this.s = s; if (DRIVEBASE_ENABLED) { bindDrivebaseControls(); } + if (LIMELIGHT_ENABLED) { + bindLimelightControls(); + } + } private void bindDrivebaseControls() { @@ -41,4 +51,9 @@ private void bindDrivebaseControls() { driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro)); driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels)); } + + public void bindLimelightControls() { + getWithinDistanceTrigger.onTrue( + new GetWithinDistanceCommand(s.limelightSubsystem, s.drivebaseSubsystem)); + } } diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index 6cb1f0ef..f4b10750 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -4,6 +4,7 @@ import frc.team2412.robot.subsystems.DrivebaseSubsystem; import frc.team2412.robot.subsystems.LauncherSubsystem; +import frc.team2412.robot.subsystems.LimelightSubsystem; public class Subsystems { public static class SubsystemConstants { @@ -20,6 +21,7 @@ public static class SubsystemConstants { public final DrivebaseSubsystem drivebaseSubsystem; public final LauncherSubsystem launcherSubsystem; + public LimelightSubsystem limelightSubsystem; public Subsystems() { // initialize subsystems here (wow thats wild) @@ -31,5 +33,8 @@ public Subsystems() { } else { launcherSubsystem = null; } + if (LIMELIGHT_ENABLED) { + limelightSubsystem = new LimelightSubsystem(); + } } } diff --git a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java new file mode 100644 index 00000000..27eb8155 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java @@ -0,0 +1,48 @@ +package frc.team2412.robot.commands.limelight; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.subsystems.DrivebaseSubsystem; +import frc.team2412.robot.subsystems.LimelightSubsystem; + +public class GetWithinDistanceCommand extends Command { + + private LimelightSubsystem limelightSubsystem; + private DrivebaseSubsystem drivebaseSubsystem; + private Pose2d currentPose; + + private Command getToPositionCommand; + + public GetWithinDistanceCommand( + LimelightSubsystem limelightSubsystem, DrivebaseSubsystem drivebaseSubsystem) { + this.limelightSubsystem = limelightSubsystem; + this.drivebaseSubsystem = drivebaseSubsystem; + + // find target -> get current pose -> use current pose and target to calculate target pose -> + // move towards target pose + + } + + @Override + public void initialize() { + currentPose = drivebaseSubsystem.getPose(); + getToPositionCommand = limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); + limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); + getToPositionCommand.initialize(); + } + + @Override + public void execute() { + getToPositionCommand.execute(); + limelightSubsystem.getTargetPose(currentPose); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return limelightSubsystem.isWithinDistance(); + } +} diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 80f96abb..ae542189 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -9,10 +9,16 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj2.command.Command; import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import frc.team2412.robot.subsystems.DrivebaseSubsystem; public class LimelightSubsystem extends SubsystemBase { @@ -169,7 +175,7 @@ public String getTargetPoseString() { public boolean isWithinDistance() { return (getDistanceFromTarget() <= GOAL_DISTANCE_FROM_TARGET); } - + public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebaseSubsystem) { final DoubleSupplier returnZero = () -> 0.0; final Supplier returnTurn = () -> Rotation2d.fromDegrees(getHorizontalOffset()); @@ -185,11 +191,12 @@ public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebas moveCommand = new DrivebaseSubsystem().driveJoystick(() -> 0.0, () -> 0.0, returnTurn); } + // create path return moveCommand; } - + @Override public void periodic() {} } From 66ddf5057a090cecec222acfa54d54f8aca15e3b Mon Sep 17 00:00:00 2001 From: Jesse S Date: Sat, 10 Feb 2024 13:11:50 -0800 Subject: [PATCH 04/20] fixed limelight subsystem from gunga selling --- src/main/java/frc/team2412/robot/Controls.java | 3 +-- .../limelight/GetWithinDistanceCommand.java | 1 - .../robot/subsystems/LimelightSubsystem.java | 17 +++++------------ 3 files changed, 6 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 52d8e078..663c1077 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -6,12 +6,12 @@ import static frc.team2412.robot.Subsystems.SubsystemConstants.LIMELIGHT_ENABLED; import edu.wpi.first.math.geometry.Rotation2d; -import frc.team2412.robot.subsystems.*; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team2412.robot.commands.limelight.GetWithinDistanceCommand; +import frc.team2412.robot.subsystems.*; public class Controls { public static class ControlConstants { @@ -37,7 +37,6 @@ public Controls(Subsystems s) { if (LIMELIGHT_ENABLED) { bindLimelightControls(); } - } private void bindDrivebaseControls() { diff --git a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java index 27eb8155..b177f878 100644 --- a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java +++ b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Command; import frc.team2412.robot.subsystems.DrivebaseSubsystem; import frc.team2412.robot.subsystems.LimelightSubsystem; diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index ae542189..de1bddb6 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -9,16 +9,10 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import frc.team2412.robot.subsystems.DrivebaseSubsystem; public class LimelightSubsystem extends SubsystemBase { @@ -175,7 +169,7 @@ public String getTargetPoseString() { public boolean isWithinDistance() { return (getDistanceFromTarget() <= GOAL_DISTANCE_FROM_TARGET); } - + public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebaseSubsystem) { final DoubleSupplier returnZero = () -> 0.0; final Supplier returnTurn = () -> Rotation2d.fromDegrees(getHorizontalOffset()); @@ -184,19 +178,18 @@ public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebas Pose2d targetPose; if (hasTargets()) { targetPose = getTargetPose(currentPose); - moveCommand = new DrivebaseSubsystem().driveJoystick(() -> 0.0, () -> 0.0, returnTurn); + moveCommand = new DrivebaseSubsystem().driveJoystick(returnZero, returnZero, returnTurn); } else { targetPose = currentPose; - moveCommand = new DrivebaseSubsystem().driveJoystick(() -> 0.0, () -> 0.0, returnTurn); + moveCommand = new DrivebaseSubsystem().driveJoystick(returnZero, returnZero, returnTurn); } - // create path return moveCommand; } - + @Override public void periodic() {} } From f7ffce4fdc19690c31f2911209c34bb9950f982b Mon Sep 17 00:00:00 2001 From: Jesse S Date: Sat, 10 Feb 2024 13:30:38 -0800 Subject: [PATCH 05/20] implemented jonah rotation method --- .../java/frc/team2412/robot/Controls.java | 33 +++++++++---------- .../java/frc/team2412/robot/Subsystems.java | 2 +- .../robot/subsystems/LimelightSubsystem.java | 8 ++--- 3 files changed, 19 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 809de48e..175ffc9b 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -4,7 +4,6 @@ import static frc.team2412.robot.Controls.ControlConstants.CONTROLLER_PORT; import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; import static frc.team2412.robot.Subsystems.SubsystemConstants.LIMELIGHT_ENABLED; -import static frc.team2412.robot.Subsystems.SubsystemConstants.LAUNCHER_ENABLED; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -13,8 +12,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team2412.robot.commands.limelight.GetWithinDistanceCommand; import frc.team2412.robot.subsystems.*; -import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; -import frc.team2412.robot.subsystems.LauncherSubsystem; public class Controls { public static class ControlConstants { @@ -51,9 +48,9 @@ public Controls(Subsystems s) { if (LIMELIGHT_ENABLED) { bindLimelightControls(); } - if (LAUNCHER_ENABLED) { - bindLauncherControls(); - } + // if (LAUNCHER_ENABLED) { + // bindLauncherControls(); + // } } private void bindDrivebaseControls() { @@ -71,17 +68,17 @@ private void bindDrivebaseControls() { public void bindLimelightControls() { getWithinDistanceTrigger.onTrue( new GetWithinDistanceCommand(s.limelightSubsystem, s.drivebaseSubsystem)); - private void bindLauncherControls() { - launcherPodiumPresetButton.onTrue( - new SetAngleLaunchCommand( - s.launcherSubsystem, - LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, - LauncherSubsystem.PODIUM_AIM_ANGLE)); - launcherSubwooferPresetButton.onTrue( - new SetAngleLaunchCommand( - s.launcherSubsystem, - LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, - LauncherSubsystem.SUBWOOFER_AIM_ANGLE)); + /*private void bindLauncherControls() { + launcherPodiumPresetButton.onTrue( + new SetAngleLaunchCommand( + s.launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.PODIUM_AIM_ANGLE)); + launcherSubwooferPresetButton.onTrue( + new SetAngleLaunchCommand( + s.launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.SUBWOOFER_AIM_ANGLE)); + }*/ } } -} \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index eb60b7f5..18aa3e1d 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -44,4 +44,4 @@ public Subsystems() { intakeSubsystem = null; } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index de1bddb6..1a16a316 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; public class LimelightSubsystem extends SubsystemBase { @@ -171,18 +170,17 @@ public boolean isWithinDistance() { } public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebaseSubsystem) { - final DoubleSupplier returnZero = () -> 0.0; final Supplier returnTurn = () -> Rotation2d.fromDegrees(getHorizontalOffset()); - final DoubleSupplier returnDrive = () -> Math.abs(turnPower()) / 2; + final Supplier returnZeroRotation = () -> Rotation2d.fromDegrees(0); Command moveCommand; Pose2d targetPose; if (hasTargets()) { targetPose = getTargetPose(currentPose); - moveCommand = new DrivebaseSubsystem().driveJoystick(returnZero, returnZero, returnTurn); + moveCommand = drivebaseSubsystem.rotateToAngle(returnTurn, true); } else { targetPose = currentPose; - moveCommand = new DrivebaseSubsystem().driveJoystick(returnZero, returnZero, returnTurn); + moveCommand = drivebaseSubsystem.rotateToAngle(returnZeroRotation, true); } // create path From 686d8ddeea16c13ad3a7f74e14da95ced83d40e5 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Tue, 13 Feb 2024 17:11:59 -0800 Subject: [PATCH 06/20] fixed command stuff and hopefully better turning methods --- .../java/frc/team2412/robot/Controls.java | 7 +- .../java/frc/team2412/robot/Subsystems.java | 2 +- .../commands/drivebase/DriveCommand.java | 104 ++++++++++++++++++ .../limelight/GetWithinDistanceCommand.java | 12 +- .../robot/subsystems/DrivebaseSubsystem.java | 22 ++++ .../robot/subsystems/LimelightSubsystem.java | 49 ++++++--- 6 files changed, 168 insertions(+), 28 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 175ffc9b..5d7f62db 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -10,8 +10,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.team2412.robot.commands.limelight.GetWithinDistanceCommand; -import frc.team2412.robot.subsystems.*; public class Controls { public static class ControlConstants { @@ -61,13 +59,12 @@ private void bindDrivebaseControls() { driveController::getLeftY, driveController::getLeftX, () -> Rotation2d.fromRotations(driveController.getRightX()))); - driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro)); + // driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro)); driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels)); } public void bindLimelightControls() { - getWithinDistanceTrigger.onTrue( - new GetWithinDistanceCommand(s.limelightSubsystem, s.drivebaseSubsystem)); + getWithinDistanceTrigger.onTrue(s.limelightSubsystem.getWithinDistance(s.drivebaseSubsystem)); /*private void bindLauncherControls() { launcherPodiumPresetButton.onTrue( new SetAngleLaunchCommand( diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index 18aa3e1d..30c9107c 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -13,7 +13,7 @@ public static class SubsystemConstants { private static final boolean IS_COMP = Robot.getInstance().isCompetition(); public static final boolean APRILTAGS_ENABLED = false; - public static final boolean LIMELIGHT_ENABLED = false; + public static final boolean LIMELIGHT_ENABLED = true; public static final boolean CLIMB_ENABLED = false; public static final boolean LAUNCHER_ENABLED = false; public static final boolean INTAKE_ENABLED = false; diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java new file mode 100644 index 00000000..5af1a5a5 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java @@ -0,0 +1,104 @@ +package frc.team2412.robot.commands.drivebase; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.subsystems.DrivebaseSubsystem; +import java.util.Map; +import java.util.function.DoubleSupplier; + +public class DriveCommand extends Command { + private static final double TURBO_ROTATION_DEFAULT = 1.5; + + private final DrivebaseSubsystem drivebaseSubsystem; + private final DoubleSupplier forward; + private final DoubleSupplier strafe; + private final DoubleSupplier rotation; + private final DoubleSupplier turboRotation; + // temporary because code is kinda messy + Rotation2d MAX_ROTATIONS_PER_SEC = Rotation2d.fromRotations(0.8574); + + // shuffleboard + private static GenericEntry driveSpeedEntry = + Shuffleboard.getTab("Drivebase") + .addPersistent("Drive Speed 1", 1) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("Min", 0, "Max", 1)) + .getEntry(); + private static GenericEntry rotationSpeedEntry = + Shuffleboard.getTab("Drivebase") + .addPersistent("Rotation Speed 1", 1) + .withSize(2, 1) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("Min", 0, "Max", 1)) + .getEntry(); + private static GenericEntry fieldOrientedEntry = + Shuffleboard.getTab("Drivebase") + .addPersistent("Field Oriented 1", true) + .withWidget(BuiltInWidgets.kToggleSwitch) + .getEntry(); + private static GenericEntry cubeSpeedEntry = + Shuffleboard.getTab("Drivebase") + .addPersistent("Cube Speed 1", true) + .withWidget(BuiltInWidgets.kToggleSwitch) + .getEntry(); + private static GenericEntry turboRotationEntry = + Shuffleboard.getTab("Drivebase") + .addPersistent("Turbo Rotation Modifier 1", TURBO_ROTATION_DEFAULT) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("Min", 0.5, "Max", 2.5)) + .getEntry(); + + public DriveCommand( + DrivebaseSubsystem drivebaseSubsystem, + DoubleSupplier forward, + DoubleSupplier strafe, + DoubleSupplier rotation, + DoubleSupplier turboRotation) { + this.drivebaseSubsystem = drivebaseSubsystem; + this.forward = forward; + this.strafe = strafe; + this.rotation = rotation; + // this variable give the right trigger input + this.turboRotation = turboRotation; + + addRequirements(drivebaseSubsystem); + } + + @Override + public void execute() { + double rotationSpeedModifier = + rotationSpeedEntry.getDouble(1.0) + * (1 + - (turboRotation.getAsDouble() + * (1 - turboRotationEntry.getDouble(TURBO_ROTATION_DEFAULT)))); + + double x = deadbandCorrection(-forward.getAsDouble()); + double y = deadbandCorrection(strafe.getAsDouble()); + double rot = deadbandCorrection(-rotation.getAsDouble()); + + // math for normalizing and cubing inputs + double magnitude = Math.pow(Math.min(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)), 1), 3); + double angle = Math.atan2(y, x); + double cubed_x = magnitude * Math.cos(angle); + double cubed_y = magnitude * Math.sin(angle); + + drivebaseSubsystem.simpleDrive( + (cubeSpeedEntry.getBoolean(false) ? cubed_x : x) + * driveSpeedEntry.getDouble(1.0) + * 4.4196, // convert from percent to m/s + (cubeSpeedEntry.getBoolean(false) ? cubed_y : y) * driveSpeedEntry.getDouble(1.0) * 4.4196, + Rotation2d.fromRotations( + rot + * rotationSpeedModifier + * MAX_ROTATIONS_PER_SEC + .getRotations())); // convert from percent to rotations per second); + } + + public double deadbandCorrection(double input) { + return Math.abs(input) < 0.05 ? 0 : (input - Math.signum(input) * 0.05) / 0.95; + } +} diff --git a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java index b177f878..82f9e4ac 100644 --- a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java +++ b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java @@ -1,9 +1,5 @@ -package frc.team2412.robot.commands.limelight; +/*package frc.team2412.robot.commands.limelight; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.team2412.robot.subsystems.DrivebaseSubsystem; -import frc.team2412.robot.subsystems.LimelightSubsystem; public class GetWithinDistanceCommand extends Command { @@ -27,7 +23,7 @@ public GetWithinDistanceCommand( public void initialize() { currentPose = drivebaseSubsystem.getPose(); getToPositionCommand = limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); - limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); + // limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); getToPositionCommand.initialize(); } @@ -42,6 +38,6 @@ public void end(boolean interrupted) {} @Override public boolean isFinished() { + System.out.println("within distance"); return limelightSubsystem.isWithinDistance(); - } -} + }*/ diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 286d3042..ca9dd211 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -156,6 +156,20 @@ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldO } } + public void driveWithDoubleSupplierTurn( + Translation2d translation, Supplier rotation, boolean fieldOriented) { + // if we're requesting the robot to stay still, lock wheels in X formation + if (translation.getNorm() == 0 && rotation.get() == 0.0 && xWheelsEnabled) { + swerveDrive.lockPose(); + } + if (rotationSetpoint != null) { + swerveDrive.drive( + translation.unaryMinus(), rotationSetpoint.getRadians(), fieldOriented, false); + } else { + swerveDrive.drive(translation.unaryMinus(), rotation.get(), fieldOriented, false); + } + } + /** * Drives the robot using joystick inputs * @@ -185,6 +199,14 @@ public Command driveJoystick( }); } + public void simpleDrive(double forward, double strafe, Rotation2d rotation) { + + ChassisSpeeds chassisSpeeds = new ChassisSpeeds(0, 0, 0); + chassisSpeeds = new ChassisSpeeds(forward, -strafe, rotation.getRadians()); + + drive(chassisSpeeds); + } + // this might need to be put in its own file due to complexity public Command rotateToAngle(Supplier angle, boolean endWhenAligned) { Command alignCommand = diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 1a16a316..f6916834 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -11,7 +11,8 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.function.Supplier; +import frc.team2412.robot.commands.drivebase.DriveCommand; +import java.util.function.DoubleSupplier; public class LimelightSubsystem extends SubsystemBase { @@ -50,6 +51,8 @@ public LimelightSubsystem() { networkTable = NetworkTableInstance.getDefault().getTable("limelight"); ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight"); + setPipeline(); + limelightTab.addBoolean("hasTarget", this::hasTargets).withPosition(0, 0).withSize(1, 1); limelightTab .addDouble("Horizontal Offset", this::getHorizontalOffset) @@ -69,7 +72,7 @@ public LimelightSubsystem() { .withPosition(4, 0) .withSize(1, 1); limelightTab - .addDouble("Limelight Based Turn Power - TEST ", this::turnPower) + .addDouble("Limelight Based Turn Power - TEST ", this::turnPowerExp) .withPosition(5, 0) .withSize(1, 1); limelightTab @@ -83,6 +86,10 @@ public LimelightSubsystem() { .withSize(4, 1); } + public void setPipeline() { + networkTable.getEntry("pipeline").setNumber(2); + } + // METHODS public boolean hasTargets() { @@ -123,12 +130,24 @@ public double getDistanceFromTargetTheSecond() { return (8.25 * focal_length) / getBoxWidth(); } - public double turnPower() { + public double turnPowerExp() { + if(getHorizontalOffset() >= 10){ return MathUtil.clamp( (Math.signum(getHorizontalOffset()) - * (Math.pow(Math.abs(getHorizontalOffset()), 1.8) / 100)), - -1.5, - 1.5); + * (Math.pow(Math.abs(getHorizontalOffset()), 1.8) / 0.2)), + -0.25, + 0.25); + }else{ + return 0.0; + } + } + + public double turnPowerLin() { + if(getHorizontalOffset() >= 3.0){ + return MathUtil.clamp(0.04*getHorizontalOffset(), -0.25, 0.25); + }else{ + return 0.0; + } } // tan(degree) * distance = sideways distance @@ -169,22 +188,24 @@ public boolean isWithinDistance() { return (getDistanceFromTarget() <= GOAL_DISTANCE_FROM_TARGET); } - public Command getWithinDistance(Pose2d currentPose, DrivebaseSubsystem drivebaseSubsystem) { - final Supplier returnTurn = () -> Rotation2d.fromDegrees(getHorizontalOffset()); - final Supplier returnZeroRotation = () -> Rotation2d.fromDegrees(0); + public Command getWithinDistance(DrivebaseSubsystem drivebaseSubsystem) { + final DoubleSupplier returnTurn = () -> turnPowerExp(); + final DoubleSupplier returnZero = () -> 0.0; Command moveCommand; Pose2d targetPose; if (hasTargets()) { - targetPose = getTargetPose(currentPose); - moveCommand = drivebaseSubsystem.rotateToAngle(returnTurn, true); + System.out.println("has targets"); + moveCommand = + new DriveCommand(drivebaseSubsystem, returnZero, returnZero, returnTurn, returnZero); } else { - targetPose = currentPose; - moveCommand = drivebaseSubsystem.rotateToAngle(returnZeroRotation, true); + System.out.println("hasn't targets"); + moveCommand = + new DriveCommand(drivebaseSubsystem, returnZero, returnZero, returnZero, returnZero); } // create path - + System.out.println("return movecommand"); return moveCommand; } From 42f8b9f8b5dac853e5aa752bc07d0ddead630b8c Mon Sep 17 00:00:00 2001 From: Jesse S Date: Wed, 21 Feb 2024 16:49:57 -0800 Subject: [PATCH 07/20] Cleaned up simpleDrive --- .../java/frc/team2412/robot/Controls.java | 6 +---- .../commands/drivebase/DriveCommand.java | 11 ++------ .../robot/subsystems/LimelightSubsystem.java | 27 +++++++++---------- 3 files changed, 16 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index f42e0e84..a78837ac 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -3,10 +3,9 @@ import static frc.team2412.robot.Controls.ControlConstants.CODRIVER_CONTROLLER_PORT; import static frc.team2412.robot.Controls.ControlConstants.CONTROLLER_PORT; import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; -import static frc.team2412.robot.Subsystems.SubsystemConstants.LIMELIGHT_ENABLED; import static frc.team2412.robot.Subsystems.SubsystemConstants.INTAKE_ENABLED; import static frc.team2412.robot.Subsystems.SubsystemConstants.LAUNCHER_ENABLED; - +import static frc.team2412.robot.Subsystems.SubsystemConstants.LIMELIGHT_ENABLED; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; @@ -80,7 +79,6 @@ public Controls(Subsystems s) { if (INTAKE_ENABLED) { bindIntakeControls(); } - } // drivebase @@ -92,11 +90,9 @@ private void bindDrivebaseControls() { driveController::getLeftY, driveController::getLeftX, () -> Rotation2d.fromRotations(driveController.getRightX()))); - // driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro)); driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels)); } - public void bindLimelightControls() { getWithinDistanceTrigger.onTrue(s.limelightSubsystem.getWithinDistance(s.drivebaseSubsystem)); } diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java index 5af1a5a5..6b06fffc 100644 --- a/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java @@ -40,11 +40,6 @@ public class DriveCommand extends Command { .addPersistent("Field Oriented 1", true) .withWidget(BuiltInWidgets.kToggleSwitch) .getEntry(); - private static GenericEntry cubeSpeedEntry = - Shuffleboard.getTab("Drivebase") - .addPersistent("Cube Speed 1", true) - .withWidget(BuiltInWidgets.kToggleSwitch) - .getEntry(); private static GenericEntry turboRotationEntry = Shuffleboard.getTab("Drivebase") .addPersistent("Turbo Rotation Modifier 1", TURBO_ROTATION_DEFAULT) @@ -87,10 +82,8 @@ public void execute() { double cubed_y = magnitude * Math.sin(angle); drivebaseSubsystem.simpleDrive( - (cubeSpeedEntry.getBoolean(false) ? cubed_x : x) - * driveSpeedEntry.getDouble(1.0) - * 4.4196, // convert from percent to m/s - (cubeSpeedEntry.getBoolean(false) ? cubed_y : y) * driveSpeedEntry.getDouble(1.0) * 4.4196, + cubed_x * driveSpeedEntry.getDouble(1.0) * 4.4196, // convert from percent to m/s + cubed_y * driveSpeedEntry.getDouble(1.0) * 4.4196, Rotation2d.fromRotations( rot * rotationSpeedModifier diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index f6916834..c22d5edb 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -72,7 +72,7 @@ public LimelightSubsystem() { .withPosition(4, 0) .withSize(1, 1); limelightTab - .addDouble("Limelight Based Turn Power - TEST ", this::turnPowerExp) + .addDouble("Limelight Based Turn Power - TEST ", this::turnPowerLin) .withPosition(5, 0) .withSize(1, 1); limelightTab @@ -131,21 +131,21 @@ public double getDistanceFromTargetTheSecond() { } public double turnPowerExp() { - if(getHorizontalOffset() >= 10){ - return MathUtil.clamp( - (Math.signum(getHorizontalOffset()) - * (Math.pow(Math.abs(getHorizontalOffset()), 1.8) / 0.2)), - -0.25, - 0.25); - }else{ + if (Math.abs(getHorizontalOffset()) >= 1.0) { + return MathUtil.clamp( + (Math.signum(getHorizontalOffset()) + * (Math.pow(Math.abs(getHorizontalOffset()), 0.6) * 0.14)), + -0.25, + 0.25); + } else { return 0.0; } } - + public double turnPowerLin() { - if(getHorizontalOffset() >= 3.0){ - return MathUtil.clamp(0.04*getHorizontalOffset(), -0.25, 0.25); - }else{ + if (Math.abs(getHorizontalOffset()) >= 1.0) { + return MathUtil.clamp(0.04 * getHorizontalOffset(), -0.25, 0.25); + } else { return 0.0; } } @@ -189,7 +189,7 @@ public boolean isWithinDistance() { } public Command getWithinDistance(DrivebaseSubsystem drivebaseSubsystem) { - final DoubleSupplier returnTurn = () -> turnPowerExp(); + final DoubleSupplier returnTurn = () -> turnPowerLin(); final DoubleSupplier returnZero = () -> 0.0; Command moveCommand; Pose2d targetPose; @@ -197,7 +197,6 @@ public Command getWithinDistance(DrivebaseSubsystem drivebaseSubsystem) { System.out.println("has targets"); moveCommand = new DriveCommand(drivebaseSubsystem, returnZero, returnZero, returnTurn, returnZero); - } else { System.out.println("hasn't targets"); moveCommand = From f8ad4b8ae6ae909178a7812b1318da588598bc0b Mon Sep 17 00:00:00 2001 From: Jesse S Date: Thu, 22 Feb 2024 16:50:34 -0800 Subject: [PATCH 08/20] Fixed the majority of PR comments --- .../java/frc/team2412/robot/Controls.java | 4 -- .../limelight/GetWithinDistanceCommand.java | 43 ------------------- .../robot/subsystems/DrivebaseSubsystem.java | 5 +-- .../robot/subsystems/LimelightSubsystem.java | 33 ++++---------- 4 files changed, 10 insertions(+), 75 deletions(-) delete mode 100644 src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index a78837ac..337136c2 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -69,10 +69,6 @@ public Controls(Subsystems s) { if (LIMELIGHT_ENABLED) { bindLimelightControls(); } - // if (LAUNCHER_ENABLED) { - // bindLauncherControls(); - // } - if (LAUNCHER_ENABLED) { bindLauncherControls(); } diff --git a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java b/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java deleted file mode 100644 index 82f9e4ac..00000000 --- a/src/main/java/frc/team2412/robot/commands/limelight/GetWithinDistanceCommand.java +++ /dev/null @@ -1,43 +0,0 @@ -/*package frc.team2412.robot.commands.limelight; - - -public class GetWithinDistanceCommand extends Command { - - private LimelightSubsystem limelightSubsystem; - private DrivebaseSubsystem drivebaseSubsystem; - private Pose2d currentPose; - - private Command getToPositionCommand; - - public GetWithinDistanceCommand( - LimelightSubsystem limelightSubsystem, DrivebaseSubsystem drivebaseSubsystem) { - this.limelightSubsystem = limelightSubsystem; - this.drivebaseSubsystem = drivebaseSubsystem; - - // find target -> get current pose -> use current pose and target to calculate target pose -> - // move towards target pose - - } - - @Override - public void initialize() { - currentPose = drivebaseSubsystem.getPose(); - getToPositionCommand = limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); - // limelightSubsystem.getWithinDistance(currentPose, drivebaseSubsystem); - getToPositionCommand.initialize(); - } - - @Override - public void execute() { - getToPositionCommand.execute(); - limelightSubsystem.getTargetPose(currentPose); - } - - @Override - public void end(boolean interrupted) {} - - @Override - public boolean isFinished() { - System.out.println("within distance"); - return limelightSubsystem.isWithinDistance(); - }*/ diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index f9a7f24b..901d501f 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -170,7 +170,7 @@ public void driveWithDoubleSupplierTurn( if (translation.getNorm() == 0 && rotation.get() == 0.0 && xWheelsEnabled) { swerveDrive.lockPose(); } - if (rotationSetpoint != null) { + else if (rotationSetpoint != null) { swerveDrive.drive( translation.unaryMinus(), rotationSetpoint.getRadians(), fieldOriented, false); } else { @@ -209,8 +209,7 @@ public Command driveJoystick( public void simpleDrive(double forward, double strafe, Rotation2d rotation) { - ChassisSpeeds chassisSpeeds = new ChassisSpeeds(0, 0, 0); - chassisSpeeds = new ChassisSpeeds(forward, -strafe, rotation.getRadians()); + ChassisSpeeds chassisSpeeds = new ChassisSpeeds(forward, -strafe, rotation.getRadians()); drive(chassisSpeeds); } diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index c22d5edb..31b4d32d 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -32,7 +32,7 @@ public class LimelightSubsystem extends SubsystemBase { // MEMBERS - NetworkTable networkTable; + final NetworkTable networkTable; String currentPoseString; String targetPoseString; @@ -42,7 +42,7 @@ public class LimelightSubsystem extends SubsystemBase { // CONSTRUCTOR ! public LimelightSubsystem() { - // broadcast 10.24.12.227 + // camera stream 10.24.12.11 // logging currentPoseString = ""; @@ -62,13 +62,8 @@ public LimelightSubsystem() { .addDouble("Vertical Offset", this::getVerticalOffset) .withPosition(2, 0) .withSize(1, 1); - - limelightTab - .addDouble("Target Distance ", this::getDistanceFromTarget) - .withPosition(3, 0) - .withSize(1, 1); limelightTab - .addDouble("Target Distance 2 - TEST ", this::getDistanceFromTargetTheSecond) + .addDouble("Target Distance 2 - TEST ", this::getDistanceFromTarget) .withPosition(4, 0) .withSize(1, 1); limelightTab @@ -86,7 +81,7 @@ public LimelightSubsystem() { .withSize(4, 1); } - public void setPipeline() { + private void setPipeline() { networkTable.getEntry("pipeline").setNumber(2); } @@ -107,21 +102,9 @@ public double getVerticalOffset() { public double getBoxWidth() { return networkTable.getEntry("tshort").getDouble(0); } - + //TODO re-measure distances to make this right public double getDistanceFromTarget() { - // 1.3 at 1.9 - // 1.0 at 1.3 - - double angleToTarget = getVerticalOffset(); - // return (TARGET_HEIGHT - CAMERA_MOUNT_HEIGHT) / Math.tan(CAMERA_ANGLE_OFFSET + angleToTarget); - - return (TARGET_HEIGHT - CAMERA_MOUNT_HEIGHT) - / (CAMERA_ANGLE_OFFSET + Math.tan(Units.degreesToRadians(getVerticalOffset()))); - } - - public double getDistanceFromTargetTheSecond() { - // focal length = (P x D) / W double focal_length = 346.1818; @@ -130,6 +113,7 @@ public double getDistanceFromTargetTheSecond() { return (8.25 * focal_length) / getBoxWidth(); } + // returns turn power from exponential curve, input is from -27deg to +27deg public double turnPowerExp() { if (Math.abs(getHorizontalOffset()) >= 1.0) { return MathUtil.clamp( @@ -142,6 +126,7 @@ public double turnPowerExp() { } } + // returns turn power from linear curve, input is from -27deg to +27deg public double turnPowerLin() { if (Math.abs(getHorizontalOffset()) >= 1.0) { return MathUtil.clamp(0.04 * getHorizontalOffset(), -0.25, 0.25); @@ -154,7 +139,6 @@ public double turnPowerLin() { // target height / tan(vertical angle) - // TODO fix this or something public Pose2d getTargetPose(Pose2d currentPose) { // math thing to get target pose using current pose @@ -162,7 +146,7 @@ public Pose2d getTargetPose(Pose2d currentPose) { Rotation2d targetHeading = new Rotation2d( currentPose.getRotation().getRadians() + Units.degreesToRadians(getHorizontalOffset())); - double targetDistance = getDistanceFromTargetTheSecond() / 39.3700787; + double targetDistance = getDistanceFromTarget() / 39.3700787; double targetX = Math.sin(targetHeading.getRadians()) * targetDistance; double targetY = Math.cos(targetHeading.getRadians()) * targetDistance; @@ -192,7 +176,6 @@ public Command getWithinDistance(DrivebaseSubsystem drivebaseSubsystem) { final DoubleSupplier returnTurn = () -> turnPowerLin(); final DoubleSupplier returnZero = () -> 0.0; Command moveCommand; - Pose2d targetPose; if (hasTargets()) { System.out.println("has targets"); moveCommand = From 86738593542bdfbe316cec419e40ee0d6619cabd Mon Sep 17 00:00:00 2001 From: Jesse S Date: Thu, 22 Feb 2024 18:01:35 -0800 Subject: [PATCH 09/20] Update src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- .../frc/team2412/robot/subsystems/LimelightSubsystem.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 31b4d32d..d3fb41c8 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -148,11 +148,10 @@ public Pose2d getTargetPose(Pose2d currentPose) { currentPose.getRotation().getRadians() + Units.degreesToRadians(getHorizontalOffset())); double targetDistance = getDistanceFromTarget() / 39.3700787; - double targetX = Math.sin(targetHeading.getRadians()) * targetDistance; - double targetY = Math.cos(targetHeading.getRadians()) * targetDistance; + Translation2d translationOffset = new Translation2d(targetDistance, targetHeading); Pose2d targetPose = - new Pose2d(currentPose.getX() + targetY, currentPose.getY() + targetX, targetHeading); + new Pose2d(currentPose.getTranslation().plus(translationOffset), targetHeading); currentPoseString = currentPose.toString(); targetPoseString = targetPose.toString(); From ae0af8fe999c440792b4e7117366fed13152dcb5 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Thu, 22 Feb 2024 19:13:22 -0800 Subject: [PATCH 10/20] oscillation issue probably fixed --- .../java/frc/team2412/robot/Subsystems.java | 1 + .../commands/drivebase/DriveCommand.java | 1 + .../robot/subsystems/DrivebaseSubsystem.java | 17 +-------------- .../robot/subsystems/LimelightSubsystem.java | 21 ++++++++++--------- 4 files changed, 14 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index f100e2e6..6fdd352e 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -15,6 +15,7 @@ public static class SubsystemConstants { private static final boolean IS_COMP = Robot.getInstance().isCompetition(); public static final boolean APRILTAGS_ENABLED = false; + // should be true when testing lol public static final boolean LIMELIGHT_ENABLED = true; public static final boolean CLIMB_ENABLED = false; public static final boolean LAUNCHER_ENABLED = false; diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java index 6b06fffc..9286b115 100644 --- a/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java @@ -17,6 +17,7 @@ public class DriveCommand extends Command { private final DoubleSupplier strafe; private final DoubleSupplier rotation; private final DoubleSupplier turboRotation; + // temporary because code is kinda messy Rotation2d MAX_ROTATIONS_PER_SEC = Rotation2d.fromRotations(0.8574); diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 6f7ea282..137e3a06 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -155,8 +155,7 @@ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldO // if we're requesting the robot to stay still, lock wheels in X formation if (translation.getNorm() == 0 && rotation.getRotations() == 0 && xWheelsEnabled) { swerveDrive.lockPose(); - } - if (rotationSetpoint != null) { + } else if (rotationSetpoint != null) { swerveDrive.drive( translation.unaryMinus(), rotationSetpoint.getRadians(), fieldOriented, false); } else { @@ -164,20 +163,6 @@ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldO } } - public void driveWithDoubleSupplierTurn( - Translation2d translation, Supplier rotation, boolean fieldOriented) { - // if we're requesting the robot to stay still, lock wheels in X formation - if (translation.getNorm() == 0 && rotation.get() == 0.0 && xWheelsEnabled) { - swerveDrive.lockPose(); - } - else if (rotationSetpoint != null) { - swerveDrive.drive( - translation.unaryMinus(), rotationSetpoint.getRadians(), fieldOriented, false); - } else { - swerveDrive.drive(translation.unaryMinus(), rotation.get(), fieldOriented, false); - } - } - /** * Drives the robot using joystick inputs * diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index d3fb41c8..e839021f 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -42,7 +42,7 @@ public class LimelightSubsystem extends SubsystemBase { // CONSTRUCTOR ! public LimelightSubsystem() { - // camera stream 10.24.12.11 + // camera stream at http://10.24.12.21:5800 // logging currentPoseString = ""; @@ -64,17 +64,16 @@ public LimelightSubsystem() { .withSize(1, 1); limelightTab .addDouble("Target Distance 2 - TEST ", this::getDistanceFromTarget) - .withPosition(4, 0) + .withPosition(3, 0) .withSize(1, 1); limelightTab .addDouble("Limelight Based Turn Power - TEST ", this::turnPowerLin) - .withPosition(5, 0) + .withPosition(4, 0) .withSize(1, 1); limelightTab .addString("Current Pose ", this::getCurrentPoseString) .withPosition(0, 1) .withSize(4, 1); - limelightTab .addString("Target Pose ", this::getTargetPoseString) .withPosition(0, 2) @@ -82,7 +81,7 @@ public LimelightSubsystem() { } private void setPipeline() { - networkTable.getEntry("pipeline").setNumber(2); + networkTable.getEntry("pipeline").setNumber(0); } // METHODS @@ -102,7 +101,7 @@ public double getVerticalOffset() { public double getBoxWidth() { return networkTable.getEntry("tshort").getDouble(0); } - //TODO re-measure distances to make this right + // TODO re-measure distances to make this right public double getDistanceFromTarget() { // focal length = (P x D) / W @@ -129,7 +128,7 @@ public double turnPowerExp() { // returns turn power from linear curve, input is from -27deg to +27deg public double turnPowerLin() { if (Math.abs(getHorizontalOffset()) >= 1.0) { - return MathUtil.clamp(0.04 * getHorizontalOffset(), -0.25, 0.25); + return MathUtil.clamp(0.01 * getHorizontalOffset(), -0.25, 0.25); } else { return 0.0; } @@ -148,10 +147,11 @@ public Pose2d getTargetPose(Pose2d currentPose) { currentPose.getRotation().getRadians() + Units.degreesToRadians(getHorizontalOffset())); double targetDistance = getDistanceFromTarget() / 39.3700787; - Translation2d translationOffset = new Translation2d(targetDistance, targetHeading); + double targetX = Math.sin(targetHeading.getRadians()) * targetDistance; + double targetY = Math.cos(targetHeading.getRadians()) * targetDistance; Pose2d targetPose = - new Pose2d(currentPose.getTranslation().plus(translationOffset), targetHeading); + new Pose2d(currentPose.getX() + targetY, currentPose.getY() + targetX, targetHeading); currentPoseString = currentPose.toString(); targetPoseString = targetPose.toString(); @@ -179,10 +179,11 @@ public Command getWithinDistance(DrivebaseSubsystem drivebaseSubsystem) { System.out.println("has targets"); moveCommand = new DriveCommand(drivebaseSubsystem, returnZero, returnZero, returnTurn, returnZero); + } else { System.out.println("hasn't targets"); moveCommand = - new DriveCommand(drivebaseSubsystem, returnZero, returnZero, returnZero, returnZero); + new DriveCommand(drivebaseSubsystem, returnZero, returnZero, returnZero, returnTurn); } // create path From fa4def3e529db5d917808cc6dff7928df9afc696 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Sat, 24 Feb 2024 09:17:36 -0800 Subject: [PATCH 11/20] working turning method, fixed drivepower --- .../robot/subsystems/LimelightSubsystem.java | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index e839021f..8967f5f9 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -70,6 +70,10 @@ public LimelightSubsystem() { .addDouble("Limelight Based Turn Power - TEST ", this::turnPowerLin) .withPosition(4, 0) .withSize(1, 1); + limelightTab + .addDouble("Limelight Based Drive Power - TEST ", this::drivePowerLin) + .withPosition(5, 0) + .withSize(1, 1); limelightTab .addString("Current Pose ", this::getCurrentPoseString) .withPosition(0, 1) @@ -99,17 +103,17 @@ public double getVerticalOffset() { } public double getBoxWidth() { - return networkTable.getEntry("tshort").getDouble(0); + return networkTable.getEntry("tlong").getDouble(0); } // TODO re-measure distances to make this right public double getDistanceFromTarget() { // focal length = (P x D) / W - double focal_length = 346.1818; + double focal_length = 559.0394; // distance = (W x F) / P // returns inches for testing purposes, will divide by 39.3700787 to return meters - return (8.25 * focal_length) / getBoxWidth(); + return (14 * focal_length) / getBoxWidth(); } // returns turn power from exponential curve, input is from -27deg to +27deg @@ -134,6 +138,15 @@ public double turnPowerLin() { } } + // drive power returns linear + public double drivePowerLin() { + if (Math.abs(getDistanceFromTarget()) >= 20) { + return MathUtil.clamp(0.01 * getDistanceFromTarget() - 0.2, 0, 0.25); + } else { + return 0.0; + } + } + // tan(degree) * distance = sideways distance // target height / tan(vertical angle) @@ -173,12 +186,13 @@ public boolean isWithinDistance() { public Command getWithinDistance(DrivebaseSubsystem drivebaseSubsystem) { final DoubleSupplier returnTurn = () -> turnPowerLin(); + final DoubleSupplier returnDrive = () -> drivePowerLin(); final DoubleSupplier returnZero = () -> 0.0; Command moveCommand; if (hasTargets()) { System.out.println("has targets"); moveCommand = - new DriveCommand(drivebaseSubsystem, returnZero, returnZero, returnTurn, returnZero); + new DriveCommand(drivebaseSubsystem, returnDrive, returnZero, returnTurn, returnZero); } else { System.out.println("hasn't targets"); From 456417f01e5a6626c64e8e5cccc8388b38e52f2f Mon Sep 17 00:00:00 2001 From: Jesse S Date: Sat, 24 Feb 2024 16:18:55 -0800 Subject: [PATCH 12/20] Cleaned up a bunch of logic, removed the need for a DriveCommand or simpleDrive(), driving towards note is done in DriveToNoteCommand now --- .../java/frc/team2412/robot/Controls.java | 4 +- .../commands/drivebase/DriveCommand.java | 98 ------------------- .../drivebase/DriveToNoteCommand.java | 35 +++++++ .../robot/subsystems/DrivebaseSubsystem.java | 7 -- .../robot/subsystems/LimelightSubsystem.java | 29 ------ 5 files changed, 38 insertions(+), 135 deletions(-) delete mode 100644 src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java create mode 100644 src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index eccc63bf..06e6b064 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.team2412.robot.commands.drivebase.DriveToNoteCommand; import frc.team2412.robot.commands.intake.AllInCommand; import frc.team2412.robot.commands.intake.AllReverseCommand; import frc.team2412.robot.commands.intake.AllStopCommand; @@ -105,7 +106,8 @@ private void bindDrivebaseControls() { } public void bindLimelightControls() { - getWithinDistanceTrigger.onTrue(s.limelightSubsystem.getWithinDistance(s.drivebaseSubsystem)); + getWithinDistanceTrigger.onTrue( + new DriveToNoteCommand(s.drivebaseSubsystem, s.limelightSubsystem)); } // intake controls private void bindIntakeControls() { diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java deleted file mode 100644 index 9286b115..00000000 --- a/src/main/java/frc/team2412/robot/commands/drivebase/DriveCommand.java +++ /dev/null @@ -1,98 +0,0 @@ -package frc.team2412.robot.commands.drivebase; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.team2412.robot.subsystems.DrivebaseSubsystem; -import java.util.Map; -import java.util.function.DoubleSupplier; - -public class DriveCommand extends Command { - private static final double TURBO_ROTATION_DEFAULT = 1.5; - - private final DrivebaseSubsystem drivebaseSubsystem; - private final DoubleSupplier forward; - private final DoubleSupplier strafe; - private final DoubleSupplier rotation; - private final DoubleSupplier turboRotation; - - // temporary because code is kinda messy - Rotation2d MAX_ROTATIONS_PER_SEC = Rotation2d.fromRotations(0.8574); - - // shuffleboard - private static GenericEntry driveSpeedEntry = - Shuffleboard.getTab("Drivebase") - .addPersistent("Drive Speed 1", 1) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kNumberSlider) - .withProperties(Map.of("Min", 0, "Max", 1)) - .getEntry(); - private static GenericEntry rotationSpeedEntry = - Shuffleboard.getTab("Drivebase") - .addPersistent("Rotation Speed 1", 1) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kNumberSlider) - .withProperties(Map.of("Min", 0, "Max", 1)) - .getEntry(); - private static GenericEntry fieldOrientedEntry = - Shuffleboard.getTab("Drivebase") - .addPersistent("Field Oriented 1", true) - .withWidget(BuiltInWidgets.kToggleSwitch) - .getEntry(); - private static GenericEntry turboRotationEntry = - Shuffleboard.getTab("Drivebase") - .addPersistent("Turbo Rotation Modifier 1", TURBO_ROTATION_DEFAULT) - .withWidget(BuiltInWidgets.kNumberSlider) - .withProperties(Map.of("Min", 0.5, "Max", 2.5)) - .getEntry(); - - public DriveCommand( - DrivebaseSubsystem drivebaseSubsystem, - DoubleSupplier forward, - DoubleSupplier strafe, - DoubleSupplier rotation, - DoubleSupplier turboRotation) { - this.drivebaseSubsystem = drivebaseSubsystem; - this.forward = forward; - this.strafe = strafe; - this.rotation = rotation; - // this variable give the right trigger input - this.turboRotation = turboRotation; - - addRequirements(drivebaseSubsystem); - } - - @Override - public void execute() { - double rotationSpeedModifier = - rotationSpeedEntry.getDouble(1.0) - * (1 - - (turboRotation.getAsDouble() - * (1 - turboRotationEntry.getDouble(TURBO_ROTATION_DEFAULT)))); - - double x = deadbandCorrection(-forward.getAsDouble()); - double y = deadbandCorrection(strafe.getAsDouble()); - double rot = deadbandCorrection(-rotation.getAsDouble()); - - // math for normalizing and cubing inputs - double magnitude = Math.pow(Math.min(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)), 1), 3); - double angle = Math.atan2(y, x); - double cubed_x = magnitude * Math.cos(angle); - double cubed_y = magnitude * Math.sin(angle); - - drivebaseSubsystem.simpleDrive( - cubed_x * driveSpeedEntry.getDouble(1.0) * 4.4196, // convert from percent to m/s - cubed_y * driveSpeedEntry.getDouble(1.0) * 4.4196, - Rotation2d.fromRotations( - rot - * rotationSpeedModifier - * MAX_ROTATIONS_PER_SEC - .getRotations())); // convert from percent to rotations per second); - } - - public double deadbandCorrection(double input) { - return Math.abs(input) < 0.05 ? 0 : (input - Math.signum(input) * 0.05) / 0.95; - } -} diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java new file mode 100644 index 00000000..2d735611 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java @@ -0,0 +1,35 @@ +package frc.team2412.robot.commands.drivebase; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.subsystems.DrivebaseSubsystem; +import frc.team2412.robot.subsystems.LimelightSubsystem; + +public class DriveToNoteCommand extends Command { + + private final DrivebaseSubsystem drivebaseSubsystem; + private final LimelightSubsystem limelightSubsystem; + + private final double INCHES_TO_METERS = 39.3700787; + + public DriveToNoteCommand( + DrivebaseSubsystem drivebaseSubsystem, LimelightSubsystem limelightSubsystem) { + this.drivebaseSubsystem = drivebaseSubsystem; + this.limelightSubsystem = limelightSubsystem; + addRequirements(drivebaseSubsystem); + } + + @Override + public void execute() { + Translation2d move = + new Translation2d(-limelightSubsystem.getDistanceFromTarget() / INCHES_TO_METERS, 0.0); + Rotation2d turn = new Rotation2d().fromDegrees(-2 * limelightSubsystem.getHorizontalOffset()); + drivebaseSubsystem.drive(move, turn, false); + } + + @Override + public boolean isFinished() { + return (limelightSubsystem.getDistanceFromTarget() <= 20); + } +} diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 137e3a06..5fbb75ae 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -192,13 +192,6 @@ public Command driveJoystick( }); } - public void simpleDrive(double forward, double strafe, Rotation2d rotation) { - - ChassisSpeeds chassisSpeeds = new ChassisSpeeds(forward, -strafe, rotation.getRadians()); - - drive(chassisSpeeds); - } - // this might need to be put in its own file due to complexity public Command rotateToAngle(Supplier angle, boolean endWhenAligned) { Command alignCommand = diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 8967f5f9..f1f4c884 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -9,18 +9,10 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.team2412.robot.commands.drivebase.DriveCommand; -import java.util.function.DoubleSupplier; public class LimelightSubsystem extends SubsystemBase { - // CONSTANTS - - private static final PIDController TRANSLATION_PID = new PIDController(10.0, 0, 0); - private static final PIDController ROTATION_PID = new PIDController(8.0, 0, 0); - // meters? public static final double CAMERA_MOUNT_HEIGHT = 0.1143; public static final double CAMERA_ANGLE_OFFSET = 0; @@ -184,27 +176,6 @@ public boolean isWithinDistance() { return (getDistanceFromTarget() <= GOAL_DISTANCE_FROM_TARGET); } - public Command getWithinDistance(DrivebaseSubsystem drivebaseSubsystem) { - final DoubleSupplier returnTurn = () -> turnPowerLin(); - final DoubleSupplier returnDrive = () -> drivePowerLin(); - final DoubleSupplier returnZero = () -> 0.0; - Command moveCommand; - if (hasTargets()) { - System.out.println("has targets"); - moveCommand = - new DriveCommand(drivebaseSubsystem, returnDrive, returnZero, returnTurn, returnZero); - - } else { - System.out.println("hasn't targets"); - moveCommand = - new DriveCommand(drivebaseSubsystem, returnZero, returnZero, returnZero, returnTurn); - } - - // create path - System.out.println("return movecommand"); - return moveCommand; - } - @Override public void periodic() {} } From 5384c132092166a0a620356a764b988270929802 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Sat, 24 Feb 2024 16:29:05 -0800 Subject: [PATCH 13/20] Some more PR related changes --- src/main/java/frc/team2412/robot/Subsystems.java | 8 +++++--- .../robot/commands/drivebase/DriveToNoteCommand.java | 6 ++++-- .../robot/subsystems/LimelightSubsystem.java | 12 +++--------- 3 files changed, 12 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index 6fdd352e..035f2585 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -15,8 +15,8 @@ public static class SubsystemConstants { private static final boolean IS_COMP = Robot.getInstance().isCompetition(); public static final boolean APRILTAGS_ENABLED = false; - // should be true when testing lol - public static final boolean LIMELIGHT_ENABLED = true; + // should be true when testing limelight lol + public static final boolean LIMELIGHT_ENABLED = false; public static final boolean CLIMB_ENABLED = false; public static final boolean LAUNCHER_ENABLED = false; public static final boolean INTAKE_ENABLED = false; @@ -26,7 +26,7 @@ public static class SubsystemConstants { public final DrivebaseWrapper drivebaseWrapper; public final DrivebaseSubsystem drivebaseSubsystem; public final LauncherSubsystem launcherSubsystem; - public LimelightSubsystem limelightSubsystem; + public final LimelightSubsystem limelightSubsystem; public final IntakeSubsystem intakeSubsystem; public final AprilTagsProcessor apriltagsProcessor; @@ -51,6 +51,8 @@ public Subsystems() { } if (LIMELIGHT_ENABLED) { limelightSubsystem = new LimelightSubsystem(); + }else{ + limelightSubsystem = null; } if (INTAKE_ENABLED) { intakeSubsystem = new IntakeSubsystem(); diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java index 2d735611..431b00c8 100644 --- a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java @@ -12,6 +12,8 @@ public class DriveToNoteCommand extends Command { private final LimelightSubsystem limelightSubsystem; private final double INCHES_TO_METERS = 39.3700787; + // limelight placement might be different so this multiplier is convenient + private final double INVERT_DRIVE_DIRECTION = -1.0; public DriveToNoteCommand( DrivebaseSubsystem drivebaseSubsystem, LimelightSubsystem limelightSubsystem) { @@ -23,8 +25,8 @@ public DriveToNoteCommand( @Override public void execute() { Translation2d move = - new Translation2d(-limelightSubsystem.getDistanceFromTarget() / INCHES_TO_METERS, 0.0); - Rotation2d turn = new Rotation2d().fromDegrees(-2 * limelightSubsystem.getHorizontalOffset()); + new Translation2d(INVERT_DRIVE_DIRECTION * limelightSubsystem.getDistanceFromTarget() / INCHES_TO_METERS, 0.0); + Rotation2d turn = new Rotation2d().fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset()); drivebaseSubsystem.drive(move, turn, false); } diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index f1f4c884..e5f5ef16 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -13,14 +13,8 @@ public class LimelightSubsystem extends SubsystemBase { - // meters? - public static final double CAMERA_MOUNT_HEIGHT = 0.1143; - public static final double CAMERA_ANGLE_OFFSET = 0; - public static final double TARGET_HEIGHT = 0.33; - - public static final double GOAL_DISTANCE_FROM_TARGET = 0.7; - public static final double GOAL_DISTANCE_FROM_CONE = 0.3; - public static final double GOAL_DISTANCE_FROM_CUBE = 0.3; + // POTENTIAL CONSTANTS + public static final double GOAL_DISTANCE_FROM_NOTE = 20.0; // MEMBERS @@ -173,7 +167,7 @@ public String getTargetPoseString() { } public boolean isWithinDistance() { - return (getDistanceFromTarget() <= GOAL_DISTANCE_FROM_TARGET); + return (getDistanceFromTarget() <= GOAL_DISTANCE_FROM_NOTE); } @Override From 189f119984406a5ac52bfe17f964a136e6fc9097 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Sat, 24 Feb 2024 16:35:36 -0800 Subject: [PATCH 14/20] Removed the old drive and turn power curves due to redundancy and re added PR suggestion --- .../robot/subsystems/LimelightSubsystem.java | 50 ++----------------- 1 file changed, 4 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index e5f5ef16..0fb82a02 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -1,9 +1,8 @@ package frc.team2412.robot.subsystems; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -49,17 +48,9 @@ public LimelightSubsystem() { .withPosition(2, 0) .withSize(1, 1); limelightTab - .addDouble("Target Distance 2 - TEST ", this::getDistanceFromTarget) + .addDouble("Distance From Note", this::getDistanceFromTarget) .withPosition(3, 0) .withSize(1, 1); - limelightTab - .addDouble("Limelight Based Turn Power - TEST ", this::turnPowerLin) - .withPosition(4, 0) - .withSize(1, 1); - limelightTab - .addDouble("Limelight Based Drive Power - TEST ", this::drivePowerLin) - .withPosition(5, 0) - .withSize(1, 1); limelightTab .addString("Current Pose ", this::getCurrentPoseString) .withPosition(0, 1) @@ -102,37 +93,6 @@ public double getDistanceFromTarget() { return (14 * focal_length) / getBoxWidth(); } - // returns turn power from exponential curve, input is from -27deg to +27deg - public double turnPowerExp() { - if (Math.abs(getHorizontalOffset()) >= 1.0) { - return MathUtil.clamp( - (Math.signum(getHorizontalOffset()) - * (Math.pow(Math.abs(getHorizontalOffset()), 0.6) * 0.14)), - -0.25, - 0.25); - } else { - return 0.0; - } - } - - // returns turn power from linear curve, input is from -27deg to +27deg - public double turnPowerLin() { - if (Math.abs(getHorizontalOffset()) >= 1.0) { - return MathUtil.clamp(0.01 * getHorizontalOffset(), -0.25, 0.25); - } else { - return 0.0; - } - } - - // drive power returns linear - public double drivePowerLin() { - if (Math.abs(getDistanceFromTarget()) >= 20) { - return MathUtil.clamp(0.01 * getDistanceFromTarget() - 0.2, 0, 0.25); - } else { - return 0.0; - } - } - // tan(degree) * distance = sideways distance // target height / tan(vertical angle) @@ -146,11 +106,9 @@ public Pose2d getTargetPose(Pose2d currentPose) { currentPose.getRotation().getRadians() + Units.degreesToRadians(getHorizontalOffset())); double targetDistance = getDistanceFromTarget() / 39.3700787; - double targetX = Math.sin(targetHeading.getRadians()) * targetDistance; - double targetY = Math.cos(targetHeading.getRadians()) * targetDistance; - + Translation2d translationOffset = new Translation2d(targetDistance, targetHeading); Pose2d targetPose = - new Pose2d(currentPose.getX() + targetY, currentPose.getY() + targetX, targetHeading); + new Pose2d(currentPose.getTranslation().plus(translationOffset), targetHeading); currentPoseString = currentPose.toString(); targetPoseString = targetPose.toString(); From f781926a88cb6c1cf4dc24291e1db5ab07e435aa Mon Sep 17 00:00:00 2001 From: Jesse S Date: Thu, 29 Feb 2024 13:13:15 -0800 Subject: [PATCH 15/20] Majority of PR comments fixed --- .../java/frc/team2412/robot/Controls.java | 14 +++++++++-- .../drivebase/DriveToNoteCommand.java | 7 +++--- .../robot/subsystems/LimelightSubsystem.java | 24 +++++++++++++++---- 3 files changed, 35 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 06e6b064..13681384 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -31,7 +31,8 @@ public static class ControlConstants { private final CommandXboxController driveController; private final CommandXboxController codriveController; - private final Trigger getWithinDistanceTrigger; + // leaving this code in in case we need to test outside of auto + //private final Trigger getWithinDistanceTrigger; // Intake private final Trigger driveIntakeInButton; @@ -52,7 +53,8 @@ public static class ControlConstants { public Controls(Subsystems s) { driveController = new CommandXboxController(CONTROLLER_PORT); codriveController = new CommandXboxController(CODRIVER_CONTROLLER_PORT); - getWithinDistanceTrigger = driveController.start(); + // not sure what drive team wants (or if the trigger is even needed since we are only using the command in auto) + //getWithinDistanceTrigger = driveController.start(); this.s = s; // launcherAmpPresetButton = codriveController.povDown(); @@ -70,9 +72,12 @@ public Controls(Subsystems s) { if (DRIVEBASE_ENABLED) { bindDrivebaseControls(); } + // leaving this code in in case we need to test outside of auto + /* if (LIMELIGHT_ENABLED) { bindLimelightControls(); } + */ if (LAUNCHER_ENABLED) { bindLauncherControls(); } @@ -103,12 +108,17 @@ private void bindDrivebaseControls() { driveController::getLeftX, () -> Rotation2d.fromRotations(driveController.getRightX()))); driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels)); + driveController.start().onTrue(new InstantCommand(s.drivebaseSubsystem::resetGyro)); } + // leaving this code in in case we need to test outside of auto + /* public void bindLimelightControls() { getWithinDistanceTrigger.onTrue( new DriveToNoteCommand(s.drivebaseSubsystem, s.limelightSubsystem)); } + */ + // intake controls private void bindIntakeControls() { // CommandScheduler.getInstance() diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java index 431b00c8..abb417c3 100644 --- a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java @@ -2,6 +2,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Unit; import edu.wpi.first.wpilibj2.command.Command; import frc.team2412.robot.subsystems.DrivebaseSubsystem; import frc.team2412.robot.subsystems.LimelightSubsystem; @@ -11,7 +13,6 @@ public class DriveToNoteCommand extends Command { private final DrivebaseSubsystem drivebaseSubsystem; private final LimelightSubsystem limelightSubsystem; - private final double INCHES_TO_METERS = 39.3700787; // limelight placement might be different so this multiplier is convenient private final double INVERT_DRIVE_DIRECTION = -1.0; @@ -25,13 +26,13 @@ public DriveToNoteCommand( @Override public void execute() { Translation2d move = - new Translation2d(INVERT_DRIVE_DIRECTION * limelightSubsystem.getDistanceFromTarget() / INCHES_TO_METERS, 0.0); + new Translation2d(INVERT_DRIVE_DIRECTION * Units.inchesToMeters(limelightSubsystem.getDistanceFromTargetInches()), 0.0); Rotation2d turn = new Rotation2d().fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset()); drivebaseSubsystem.drive(move, turn, false); } @Override public boolean isFinished() { - return (limelightSubsystem.getDistanceFromTarget() <= 20); + return (limelightSubsystem.isWithinDistance()); } } diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 0fb82a02..f74fead8 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -1,19 +1,24 @@ package frc.team2412.robot.subsystems; +import java.util.Map; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.Unit; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.networktables.GenericEntry; public class LimelightSubsystem extends SubsystemBase { // POTENTIAL CONSTANTS - public static final double GOAL_DISTANCE_FROM_NOTE = 20.0; + private GenericEntry GOAL_DISTANCE_FROM_NOTE; // MEMBERS @@ -36,6 +41,7 @@ public LimelightSubsystem() { networkTable = NetworkTableInstance.getDefault().getTable("limelight"); ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight"); + // helpers setPipeline(); limelightTab.addBoolean("hasTarget", this::hasTargets).withPosition(0, 0).withSize(1, 1); @@ -48,7 +54,7 @@ public LimelightSubsystem() { .withPosition(2, 0) .withSize(1, 1); limelightTab - .addDouble("Distance From Note", this::getDistanceFromTarget) + .addDouble("Distance From Note", this::getDistanceFromTargetInches) .withPosition(3, 0) .withSize(1, 1); limelightTab @@ -59,6 +65,13 @@ public LimelightSubsystem() { .addString("Target Pose ", this::getTargetPoseString) .withPosition(0, 2) .withSize(4, 1); + GOAL_DISTANCE_FROM_NOTE = + limelightTab.addPersistent("Goal distance", 20.0) + .withWidget(BuiltInWidgets.kNumberSlider) + .withPosition(4, 0) + .withSize(2, 1) + .withProperties(Map.of("Min", 0.0, "Max", 30.0)) + .getEntry(); } private void setPipeline() { @@ -83,7 +96,7 @@ public double getBoxWidth() { return networkTable.getEntry("tlong").getDouble(0); } // TODO re-measure distances to make this right - public double getDistanceFromTarget() { + public double getDistanceFromTargetInches() { // focal length = (P x D) / W double focal_length = 559.0394; @@ -104,7 +117,8 @@ public Pose2d getTargetPose(Pose2d currentPose) { Rotation2d targetHeading = new Rotation2d( currentPose.getRotation().getRadians() + Units.degreesToRadians(getHorizontalOffset())); - double targetDistance = getDistanceFromTarget() / 39.3700787; + + double targetDistance = Units.inchesToMeters(getDistanceFromTargetInches()); Translation2d translationOffset = new Translation2d(targetDistance, targetHeading); Pose2d targetPose = @@ -125,7 +139,7 @@ public String getTargetPoseString() { } public boolean isWithinDistance() { - return (getDistanceFromTarget() <= GOAL_DISTANCE_FROM_NOTE); + return (getDistanceFromTargetInches() <= GOAL_DISTANCE_FROM_NOTE.getDouble(20.0)); } @Override From f57e583b81fbf6067ea178b98e8edee67932bd87 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Mon, 11 Mar 2024 18:38:27 -0700 Subject: [PATCH 16/20] spotlessapply --- src/main/java/frc/team2412/robot/Controls.java | 9 ++++----- src/main/java/frc/team2412/robot/Subsystems.java | 2 +- .../commands/drivebase/DriveToNoteCommand.java | 12 ++++++++---- .../robot/subsystems/LimelightSubsystem.java | 13 ++++++------- 4 files changed, 19 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 13681384..54b6b757 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -5,7 +5,6 @@ import static frc.team2412.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; import static frc.team2412.robot.Subsystems.SubsystemConstants.INTAKE_ENABLED; import static frc.team2412.robot.Subsystems.SubsystemConstants.LAUNCHER_ENABLED; -import static frc.team2412.robot.Subsystems.SubsystemConstants.LIMELIGHT_ENABLED; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; @@ -15,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.team2412.robot.commands.drivebase.DriveToNoteCommand; import frc.team2412.robot.commands.intake.AllInCommand; import frc.team2412.robot.commands.intake.AllReverseCommand; import frc.team2412.robot.commands.intake.AllStopCommand; @@ -32,7 +30,7 @@ public static class ControlConstants { private final CommandXboxController driveController; private final CommandXboxController codriveController; // leaving this code in in case we need to test outside of auto - //private final Trigger getWithinDistanceTrigger; + // private final Trigger getWithinDistanceTrigger; // Intake private final Trigger driveIntakeInButton; @@ -53,8 +51,9 @@ public static class ControlConstants { public Controls(Subsystems s) { driveController = new CommandXboxController(CONTROLLER_PORT); codriveController = new CommandXboxController(CODRIVER_CONTROLLER_PORT); - // not sure what drive team wants (or if the trigger is even needed since we are only using the command in auto) - //getWithinDistanceTrigger = driveController.start(); + // not sure what drive team wants (or if the trigger is even needed since we are only using the + // command in auto) + // getWithinDistanceTrigger = driveController.start(); this.s = s; // launcherAmpPresetButton = codriveController.povDown(); diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index 035f2585..72c5813b 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -51,7 +51,7 @@ public Subsystems() { } if (LIMELIGHT_ENABLED) { limelightSubsystem = new LimelightSubsystem(); - }else{ + } else { limelightSubsystem = null; } if (INTAKE_ENABLED) { diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java index abb417c3..1ca569fd 100644 --- a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java @@ -3,7 +3,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.Unit; import edu.wpi.first.wpilibj2.command.Command; import frc.team2412.robot.subsystems.DrivebaseSubsystem; import frc.team2412.robot.subsystems.LimelightSubsystem; @@ -17,7 +16,7 @@ public class DriveToNoteCommand extends Command { private final double INVERT_DRIVE_DIRECTION = -1.0; public DriveToNoteCommand( - DrivebaseSubsystem drivebaseSubsystem, LimelightSubsystem limelightSubsystem) { + DrivebaseSubsystem drivebaseSubsystem, LimelightSubsystem limelightSubsystem) { this.drivebaseSubsystem = drivebaseSubsystem; this.limelightSubsystem = limelightSubsystem; addRequirements(drivebaseSubsystem); @@ -26,8 +25,13 @@ public DriveToNoteCommand( @Override public void execute() { Translation2d move = - new Translation2d(INVERT_DRIVE_DIRECTION * Units.inchesToMeters(limelightSubsystem.getDistanceFromTargetInches()), 0.0); - Rotation2d turn = new Rotation2d().fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset()); + new Translation2d( + INVERT_DRIVE_DIRECTION + * Units.inchesToMeters(limelightSubsystem.getDistanceFromTargetInches()), + 0.0); + Rotation2d turn = + new Rotation2d() + .fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset()); drivebaseSubsystem.drive(move, turn, false); } diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index f74fead8..30034c4d 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -1,19 +1,17 @@ package frc.team2412.robot.subsystems; -import java.util.Map; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.units.Unit; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.networktables.GenericEntry; +import java.util.Map; public class LimelightSubsystem extends SubsystemBase { @@ -32,7 +30,7 @@ public class LimelightSubsystem extends SubsystemBase { // CONSTRUCTOR ! public LimelightSubsystem() { - // camera stream at http://10.24.12.21:5800 + // camera stream at http://10.24.12.11:5800 // logging currentPoseString = ""; @@ -66,7 +64,8 @@ public LimelightSubsystem() { .withPosition(0, 2) .withSize(4, 1); GOAL_DISTANCE_FROM_NOTE = - limelightTab.addPersistent("Goal distance", 20.0) + limelightTab + .addPersistent("Goal distance", 20.0) .withWidget(BuiltInWidgets.kNumberSlider) .withPosition(4, 0) .withSize(2, 1) @@ -117,7 +116,7 @@ public Pose2d getTargetPose(Pose2d currentPose) { Rotation2d targetHeading = new Rotation2d( currentPose.getRotation().getRadians() + Units.degreesToRadians(getHorizontalOffset())); - + double targetDistance = Units.inchesToMeters(getDistanceFromTargetInches()); Translation2d translationOffset = new Translation2d(targetDistance, targetHeading); From f34c9705aa9f97a0c32b0526e42b56123ed21867 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Tue, 19 Mar 2024 15:24:58 -0700 Subject: [PATCH 17/20] fixed pr comments (again) --- .../java/frc/team2412/robot/Controls.java | 2 +- .../drivebase/DriveToNoteCommand.java | 22 ++++++++++--------- .../robot/subsystems/LimelightSubsystem.java | 7 ++++-- 3 files changed, 18 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 24fbeacb..16aaaadc 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -132,7 +132,7 @@ private void bindDrivebaseControls() { // leaving this code in in case we need to test outside of auto /* public void bindLimelightControls() { - getWithinDistanceTrigger.onTrue( + getWithinDistanceTrigger.whileTrue( new DriveToNoteCommand(s.drivebaseSubsystem, s.limelightSubsystem)); } */ diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java index 1ca569fd..f75dbc3d 100644 --- a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java @@ -24,19 +24,21 @@ public DriveToNoteCommand( @Override public void execute() { - Translation2d move = - new Translation2d( - INVERT_DRIVE_DIRECTION - * Units.inchesToMeters(limelightSubsystem.getDistanceFromTargetInches()), - 0.0); - Rotation2d turn = - new Rotation2d() - .fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset()); - drivebaseSubsystem.drive(move, turn, false); + if(limelightSubsystem.hasTargets()){ + Translation2d move = + new Translation2d( + INVERT_DRIVE_DIRECTION + * Units.inchesToMeters(limelightSubsystem.getDistanceFromTargetInches()), + 0.0); + Rotation2d turn = + new Rotation2d() + .fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset()); + drivebaseSubsystem.drive(move, turn, false); + } } @Override public boolean isFinished() { - return (limelightSubsystem.isWithinDistance()); + return (limelightSubsystem.isWithinDistance() || !limelightSubsystem.hasTargets()); } } diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 30034c4d..98726cc2 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -102,9 +102,12 @@ public double getDistanceFromTargetInches() { // distance = (W x F) / P // returns inches for testing purposes, will divide by 39.3700787 to return meters - return (14 * focal_length) / getBoxWidth(); + if(hasTargets()){ + return (14 * focal_length) / getBoxWidth(); + }else{ + return 0.0; + } } - // tan(degree) * distance = sideways distance // target height / tan(vertical angle) From e30a7bfc6527d2002d650da7400e559357e40363 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Tue, 19 Mar 2024 17:47:40 -0700 Subject: [PATCH 18/20] spotlessapply maybe --- .../team2412/robot/commands/drivebase/DriveToNoteCommand.java | 2 +- .../frc/team2412/robot/subsystems/LimelightSubsystem.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java index f75dbc3d..933117c2 100644 --- a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java @@ -24,7 +24,7 @@ public DriveToNoteCommand( @Override public void execute() { - if(limelightSubsystem.hasTargets()){ + if (limelightSubsystem.hasTargets()) { Translation2d move = new Translation2d( INVERT_DRIVE_DIRECTION diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index 98726cc2..fbe4f460 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -102,9 +102,9 @@ public double getDistanceFromTargetInches() { // distance = (W x F) / P // returns inches for testing purposes, will divide by 39.3700787 to return meters - if(hasTargets()){ + if (hasTargets()) { return (14 * focal_length) / getBoxWidth(); - }else{ + } else { return 0.0; } } From 1e6874bfcc403a4c88bae4e320bb194f70cb0e23 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Tue, 19 Mar 2024 17:53:12 -0700 Subject: [PATCH 19/20] removed redundant methods from limelightsubsystem --- .../robot/subsystems/LimelightSubsystem.java | 35 +------------------ 1 file changed, 1 insertion(+), 34 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index fbe4f460..c70d02fe 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -23,7 +23,6 @@ public class LimelightSubsystem extends SubsystemBase { final NetworkTable networkTable; String currentPoseString; - String targetPoseString; // network tables @@ -34,7 +33,6 @@ public LimelightSubsystem() { // logging currentPoseString = ""; - targetPoseString = ""; networkTable = NetworkTableInstance.getDefault().getTable("limelight"); ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight"); @@ -59,10 +57,6 @@ public LimelightSubsystem() { .addString("Current Pose ", this::getCurrentPoseString) .withPosition(0, 1) .withSize(4, 1); - limelightTab - .addString("Target Pose ", this::getTargetPoseString) - .withPosition(0, 2) - .withSize(4, 1); GOAL_DISTANCE_FROM_NOTE = limelightTab .addPersistent("Goal distance", 20.0) @@ -108,38 +102,11 @@ public double getDistanceFromTargetInches() { return 0.0; } } - // tan(degree) * distance = sideways distance - - // target height / tan(vertical angle) - - public Pose2d getTargetPose(Pose2d currentPose) { - - // math thing to get target pose using current pose - - Rotation2d targetHeading = - new Rotation2d( - currentPose.getRotation().getRadians() + Units.degreesToRadians(getHorizontalOffset())); - - double targetDistance = Units.inchesToMeters(getDistanceFromTargetInches()); - - Translation2d translationOffset = new Translation2d(targetDistance, targetHeading); - Pose2d targetPose = - new Pose2d(currentPose.getTranslation().plus(translationOffset), targetHeading); - - currentPoseString = currentPose.toString(); - targetPoseString = targetPose.toString(); - - return targetPose; - } - + public String getCurrentPoseString() { return currentPoseString; } - public String getTargetPoseString() { - return targetPoseString; - } - public boolean isWithinDistance() { return (getDistanceFromTargetInches() <= GOAL_DISTANCE_FROM_NOTE.getDouble(20.0)); } From 73b94f7c31510f883db57030ae2cf97d0435ed86 Mon Sep 17 00:00:00 2001 From: Jesse S Date: Tue, 19 Mar 2024 17:55:16 -0700 Subject: [PATCH 20/20] more cleanup --- .../frc/team2412/robot/subsystems/LimelightSubsystem.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index c70d02fe..a3109e6a 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -1,9 +1,5 @@ package frc.team2412.robot.subsystems; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -102,7 +98,7 @@ public double getDistanceFromTargetInches() { return 0.0; } } - + public String getCurrentPoseString() { return currentPoseString; }