From 15d30ef64b2df2a872e97b4b3e1a58f2e6b07a5f Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 21 Aug 2024 22:00:01 -0400 Subject: [PATCH 1/6] save energy by not applying voltage when arm wants to be at min angle --- src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 2268399..88509ed 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -176,6 +176,10 @@ public void periodic() { setVoltage(-1.5); controller.update(Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MIN_ANGLE.get()); } + else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE.get() && bumpSwitchTriggered.get()) { + setVoltage(0); + controller.update(Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MIN_ANGLE.get()); + } else { controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees()); if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING) { From 381c9b318ca36c004a544fd5b279e31bffce40af Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 21 Aug 2024 22:17:16 -0400 Subject: [PATCH 2/6] update open loop ramp rates --- .../java/com/stuypulse/robot/constants/Motors.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 3f6b4ac..cfc003b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -39,8 +39,8 @@ public static void disableStatusFrames(CANSparkBase motor, StatusFrame... ids) { /** Classes to store all of the values a motor needs */ public interface Arm { - CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.0); - CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.0); + CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.25); + CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25); } public interface Intake { @@ -50,9 +50,9 @@ public interface Intake { } public interface Shooter { - CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 1.0); - CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 1.0); - CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 1.0); + CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 0.5); + CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 0.5); + CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25); } /* Configurations */ From d4a2e3f47dae7a176906611526eddc99b0a21608 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Fri, 23 Aug 2024 17:57:36 -0400 Subject: [PATCH 3/6] fix camera offsets --- src/main/java/com/stuypulse/robot/constants/Cameras.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 734f419..8f69290 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -27,7 +27,7 @@ public interface Limelight { new CameraConfig( "tower-cam", new Pose3d( - new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(-3.333797), Units.inchesToMeters(23.929362)), + new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(+3.333797), Units.inchesToMeters(23.929362)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15), Units.degreesToRadians(180)) ), "11", @@ -37,7 +37,7 @@ public interface Limelight { new CameraConfig( "plate-cam", new Pose3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(4.863591), Units.inchesToMeters(19.216471)), + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(-4.863591), Units.inchesToMeters(19.216471)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(80), Units.degreesToRadians(0)) ), "96", From f74f6283c09c77d795de1924e65f2419b420ff49 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Fri, 23 Aug 2024 17:58:14 -0400 Subject: [PATCH 4/6] no accel limit for alignment --- .../commands/swerve/driveAligned/SwerveDriveDriveAligned.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java index 8cb0609..aee4420 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java @@ -64,7 +64,7 @@ public SwerveDriveDriveAligned(Gamepad driver) { // make angleVelocity contribute less once distance is less than REDUCED_FF_DIST // so that angular velocity doesn't oscillate x -> x * Math.min(1, getDistanceToTarget() / Assist.REDUCED_FF_DIST), - new RateLimit(Settings.Swerve.MAX_ANGULAR_ACCEL), + // new RateLimit(Settings.Swerve.MAX_ANGULAR_ACCEL), x -> SLMath.clamp(x, -Settings.Swerve.MAX_ANGULAR_VELOCITY, Settings.Swerve.MAX_ANGULAR_VELOCITY), x -> -x ); From 5526db1cac426c6311d01e0c323f23a57283fc7a Mon Sep 17 00:00:00 2001 From: Kalimul Kaif Date: Fri, 23 Aug 2024 21:42:21 -0400 Subject: [PATCH 5/6] Added workaround for switching speaker and amp tag poses in Field.java depending on alliance (only works every time you deploy for some reason) --- .../com/stuypulse/robot/constants/Field.java | 17 ++++++++++++++--- .../com/stuypulse/robot/constants/Settings.java | 4 ++-- .../robot/subsystems/swerve/SwerveDrive.java | 13 +++++++++++++ 3 files changed, 29 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 8c0451b..df4a83c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -12,10 +12,13 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import java.util.ArrayList; import java.util.Arrays; +import java.util.Optional; /** This interface stores information about the field elements. */ public interface Field { @@ -30,6 +33,8 @@ public interface Field { double SPEAKER_MAX_HEIGHT = 2.11; // represents the top of the speaker opening double SPEAKER_MIN_HEIGHT = 1.98; // represents the bottom of the speaker opening + Optional alliance = DriverStation.getAlliance(); + public static Pose3d transformToOppositeAlliance(Pose3d pose) { Pose3d rotated = pose.rotateBy(new Rotation3d(0, 0, Math.PI)); @@ -152,7 +157,8 @@ public static AprilTag getTag(int id) { /*** SPEAKER ***/ public static Pose2d getAllianceSpeakerPose() { - return (Robot.isBlue() ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER) + // return (Robot.isBlue() ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER).getLocation().toPose2d(); + return (alliance.isPresent() ? (alliance.get() == Alliance.Blue ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER ) : (null)) .getLocation().toPose2d(); } @@ -166,13 +172,17 @@ public static Pose2d getSpeakerPathFindPose() { /*** AMP ***/ public static Pose2d getAllianceAmpPose() { - return (Robot.isBlue() ? NamedTags.BLUE_AMP : NamedTags.RED_AMP) + return (alliance.isPresent() ? (alliance.get() == Alliance.Blue ? NamedTags.BLUE_AMP : NamedTags.RED_AMP ) : (null)) .getLocation().toPose2d(); + // return (Robot.isBlue() ? NamedTags.BLUE_AMP : NamedTags.RED_AMP) + // .getLocation().toPose2d(); } public static Pose2d getOpposingAmpPose() { - return (Robot.isBlue() ? NamedTags.RED_AMP : NamedTags.BLUE_AMP) + return (alliance.isPresent() ? (alliance.get() == Alliance.Blue ? NamedTags.RED_AMP : NamedTags.BLUE_AMP ) : (null)) .getLocation().toPose2d(); + // return (Robot.isBlue() ? NamedTags.RED_AMP : NamedTags.BLUE_AMP) + // .getLocation().toPose2d(); } public static Pose2d getAmpPathFindPose() { @@ -199,6 +209,7 @@ public static Pose2d getOpposingSourcePose() { /*** TRAP ***/ public static Pose2d[] getAllianceTrapPoses() { + if (Robot.isBlue()) { return new Pose2d[] { NamedTags.BLUE_STAGE_FAR.getLocation().toPose2d(), diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b572d03..eba2cdd 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -21,7 +21,7 @@ public interface Settings { double DT = 1.0 / 50.0; - boolean SAFE_MODE_ENABLED = false; + boolean SAFE_MODE_ENABLED = true; double WIDTH = Units.inchesToMeters(36); // intake side double LENGTH = Units.inchesToMeters(32); @@ -309,7 +309,7 @@ public interface Alignment { SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); - SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); + SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 20); SmartNumber CLIMB_SETUP_DISTANCE = new SmartNumber("Alignment/Climb/Setup Distance", Units.inchesToMeters(21.0)); SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Climb/Into Chain Speed", 0.25); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 0411814..5d712ed 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -255,6 +255,19 @@ public void periodic() { SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Drive Voltage", Modules[i].getDriveMotor().getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Turn Current", Modules[i].getSteerMotor().getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Turn Voltage", Modules[i].getSteerMotor().getMotorVoltage().getValueAsDouble()); + + // logging for testing field poses on opposite alliances, remove when finished + SmartDashboard.putNumber("Swerve/Speaker Pose X", Field.getAllianceSpeakerPose().getTranslation().getX()); + SmartDashboard.putNumber("Swerve/Speaker Pose Y", Field.getAllianceSpeakerPose().getTranslation().getY()); + SmartDashboard.putNumber("Swerve/Amp Pose X", Field.getAllianceAmpPose().getTranslation().getX()); + SmartDashboard.putNumber("Swerve/Amp Pose Y", Field.getAllianceAmpPose().getTranslation().getY()); + SmartDashboard.putNumber("Swerve/Opposing Amp Pose X", Field.getOpposingAmpPose().getTranslation().getX()); + SmartDashboard.putNumber("Swerve/Opposing Amp Pose Y", Field.getOpposingAmpPose().getTranslation().getY()); + + SmartDashboard.putNumber("Swerve/Current Pose X", SwerveDrive.getInstance().getPose().getX()); + SmartDashboard.putNumber("Swerve/Current Pose Y", SwerveDrive.getInstance().getPose().getY()); + SmartDashboard.putNumber("Swerve/Current Rotation", SwerveDrive.getInstance().getGyroAngle().getDegrees()); + } field.setRobotPose(getPose()); From e850bcf8bab8fc986511f081b1b2061b727cddb9 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Tue, 27 Aug 2024 00:39:56 -0400 Subject: [PATCH 6/6] Revert "Added workaround for switching speaker and amp tag poses in Field.java depending on alliance (only works every time you deploy for some reason)" --- .../com/stuypulse/robot/constants/Field.java | 17 +++-------------- .../com/stuypulse/robot/constants/Settings.java | 4 ++-- .../robot/subsystems/swerve/SwerveDrive.java | 13 ------------- 3 files changed, 5 insertions(+), 29 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index df4a83c..8c0451b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -12,13 +12,10 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import java.util.ArrayList; import java.util.Arrays; -import java.util.Optional; /** This interface stores information about the field elements. */ public interface Field { @@ -33,8 +30,6 @@ public interface Field { double SPEAKER_MAX_HEIGHT = 2.11; // represents the top of the speaker opening double SPEAKER_MIN_HEIGHT = 1.98; // represents the bottom of the speaker opening - Optional alliance = DriverStation.getAlliance(); - public static Pose3d transformToOppositeAlliance(Pose3d pose) { Pose3d rotated = pose.rotateBy(new Rotation3d(0, 0, Math.PI)); @@ -157,8 +152,7 @@ public static AprilTag getTag(int id) { /*** SPEAKER ***/ public static Pose2d getAllianceSpeakerPose() { - // return (Robot.isBlue() ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER).getLocation().toPose2d(); - return (alliance.isPresent() ? (alliance.get() == Alliance.Blue ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER ) : (null)) + return (Robot.isBlue() ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER) .getLocation().toPose2d(); } @@ -172,17 +166,13 @@ public static Pose2d getSpeakerPathFindPose() { /*** AMP ***/ public static Pose2d getAllianceAmpPose() { - return (alliance.isPresent() ? (alliance.get() == Alliance.Blue ? NamedTags.BLUE_AMP : NamedTags.RED_AMP ) : (null)) + return (Robot.isBlue() ? NamedTags.BLUE_AMP : NamedTags.RED_AMP) .getLocation().toPose2d(); - // return (Robot.isBlue() ? NamedTags.BLUE_AMP : NamedTags.RED_AMP) - // .getLocation().toPose2d(); } public static Pose2d getOpposingAmpPose() { - return (alliance.isPresent() ? (alliance.get() == Alliance.Blue ? NamedTags.RED_AMP : NamedTags.BLUE_AMP ) : (null)) + return (Robot.isBlue() ? NamedTags.RED_AMP : NamedTags.BLUE_AMP) .getLocation().toPose2d(); - // return (Robot.isBlue() ? NamedTags.RED_AMP : NamedTags.BLUE_AMP) - // .getLocation().toPose2d(); } public static Pose2d getAmpPathFindPose() { @@ -209,7 +199,6 @@ public static Pose2d getOpposingSourcePose() { /*** TRAP ***/ public static Pose2d[] getAllianceTrapPoses() { - if (Robot.isBlue()) { return new Pose2d[] { NamedTags.BLUE_STAGE_FAR.getLocation().toPose2d(), diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index eba2cdd..b572d03 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -21,7 +21,7 @@ public interface Settings { double DT = 1.0 / 50.0; - boolean SAFE_MODE_ENABLED = true; + boolean SAFE_MODE_ENABLED = false; double WIDTH = Units.inchesToMeters(36); // intake side double LENGTH = Units.inchesToMeters(32); @@ -309,7 +309,7 @@ public interface Alignment { SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); - SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 20); + SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); SmartNumber CLIMB_SETUP_DISTANCE = new SmartNumber("Alignment/Climb/Setup Distance", Units.inchesToMeters(21.0)); SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Climb/Into Chain Speed", 0.25); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 5d712ed..0411814 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -255,19 +255,6 @@ public void periodic() { SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Drive Voltage", Modules[i].getDriveMotor().getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Turn Current", Modules[i].getSteerMotor().getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Turn Voltage", Modules[i].getSteerMotor().getMotorVoltage().getValueAsDouble()); - - // logging for testing field poses on opposite alliances, remove when finished - SmartDashboard.putNumber("Swerve/Speaker Pose X", Field.getAllianceSpeakerPose().getTranslation().getX()); - SmartDashboard.putNumber("Swerve/Speaker Pose Y", Field.getAllianceSpeakerPose().getTranslation().getY()); - SmartDashboard.putNumber("Swerve/Amp Pose X", Field.getAllianceAmpPose().getTranslation().getX()); - SmartDashboard.putNumber("Swerve/Amp Pose Y", Field.getAllianceAmpPose().getTranslation().getY()); - SmartDashboard.putNumber("Swerve/Opposing Amp Pose X", Field.getOpposingAmpPose().getTranslation().getX()); - SmartDashboard.putNumber("Swerve/Opposing Amp Pose Y", Field.getOpposingAmpPose().getTranslation().getY()); - - SmartDashboard.putNumber("Swerve/Current Pose X", SwerveDrive.getInstance().getPose().getX()); - SmartDashboard.putNumber("Swerve/Current Pose Y", SwerveDrive.getInstance().getPose().getY()); - SmartDashboard.putNumber("Swerve/Current Rotation", SwerveDrive.getInstance().getGyroAngle().getDegrees()); - } field.setRobotPose(getPose());