From ba62e650b1e4c369e127bb1e76a0600193289006 Mon Sep 17 00:00:00 2001 From: Stuart McCrorie <42500981+smccrorie@users.noreply.github.com> Date: Sat, 20 Jan 2024 09:02:11 -0600 Subject: [PATCH] Reimplementing Limelight and auto methods --- .../sample/auto/Programs/TestProgram.java | 14 +++++++------- .../sample/auto/Steps/StartOdometryStep.java | 8 ++++---- .../org/wildstang/sample/auto/Steps/TagOnStep.java | 12 ++++++------ .../org/wildstang/sample/robot/WsSubsystems.java | 2 +- .../sample/subsystems/swerve/SwerveDrive.java | 9 ++++----- .../subsystems/targeting/LimelightHelpers.java | 2 +- 6 files changed, 23 insertions(+), 24 deletions(-) diff --git a/src/main/java/org/wildstang/sample/auto/Programs/TestProgram.java b/src/main/java/org/wildstang/sample/auto/Programs/TestProgram.java index bb97bf5f..fcc8859b 100644 --- a/src/main/java/org/wildstang/sample/auto/Programs/TestProgram.java +++ b/src/main/java/org/wildstang/sample/auto/Programs/TestProgram.java @@ -16,13 +16,13 @@ public class TestProgram extends AutoProgram{ protected void defineSteps(){ - // SwerveDrive swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); - // addStep(new SetGyroStep(180.0, swerve)); - // AutoParallelStepGroup group1 = new AutoParallelStepGroup(); - // group1.addStep(new PathHeadingStep(180.0, swerve)); - // addStep(group1); - // addStep(new StartOdometryStep(3.0, 5.0, 180.0, true)); - // addStep(new SwervePathFollowerStep(Choreo.getTrajectory("ChoreoTest"), swerve, true)); + SwerveDrive swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); + addStep(new SetGyroStep(180.0, swerve)); + AutoParallelStepGroup group1 = new AutoParallelStepGroup(); + group1.addStep(new PathHeadingStep(180.0, swerve)); + addStep(group1); + addStep(new StartOdometryStep(3.0, 5.0, 180.0, true)); + addStep(new SwervePathFollowerStep(Choreo.getTrajectory("ChoreoTest"), swerve, true)); } public String toString(){ diff --git a/src/main/java/org/wildstang/sample/auto/Steps/StartOdometryStep.java b/src/main/java/org/wildstang/sample/auto/Steps/StartOdometryStep.java index 9eabb386..acf55f22 100644 --- a/src/main/java/org/wildstang/sample/auto/Steps/StartOdometryStep.java +++ b/src/main/java/org/wildstang/sample/auto/Steps/StartOdometryStep.java @@ -12,7 +12,7 @@ public class StartOdometryStep extends AutoStep{ private double x, y, heading; - //private SwerveDrive swerve; + private SwerveDrive swerve; private boolean color;//true for blue, false for red public StartOdometryStep(double X, double Y, double pathHeading, boolean allianceColor){ @@ -23,14 +23,14 @@ public StartOdometryStep(double X, double Y, double pathHeading, boolean allianc } public void update(){ if (color){ - // swerve.setOdo(new Pose2d(new Translation2d(x, y), new Rotation2d(Math.toRadians(360.0-heading)))); + swerve.setOdo(new Pose2d(new Translation2d(x, y), new Rotation2d(Math.toRadians(360.0-heading)))); } else { - //swerve.setOdo(new Pose2d(new Translation2d(x, 8.016-y), new Rotation2d(Math.toRadians(360.0-heading)))); + swerve.setOdo(new Pose2d(new Translation2d(x, 8.016-y), new Rotation2d(Math.toRadians(360.0-heading)))); } this.setFinished(); } public void initialize(){ - //swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); + swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); } public String toString(){ return "Start Odometry"; diff --git a/src/main/java/org/wildstang/sample/auto/Steps/TagOnStep.java b/src/main/java/org/wildstang/sample/auto/Steps/TagOnStep.java index 6b19f8ad..aabaa269 100644 --- a/src/main/java/org/wildstang/sample/auto/Steps/TagOnStep.java +++ b/src/main/java/org/wildstang/sample/auto/Steps/TagOnStep.java @@ -12,8 +12,8 @@ public class TagOnStep extends AutoStep{ - // private SwerveDrive swerve; - //private WsVision limelight; + private SwerveDrive swerve; + private WsVision limelight; private boolean color, on;//true for blue, false for red public TagOnStep(boolean isOn, boolean isBlue){ @@ -21,13 +21,13 @@ public TagOnStep(boolean isOn, boolean isBlue){ on = isOn; } public void update(){ - // swerve.setAutoTag(on, color); - //limelight.setGamePiece(false); + swerve.setAutoTag(on, color); + limelight.setGamePiece(false); this.setFinished(); } public void initialize(){ - // swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); - //limelight = (WsVision) Core.getSubsystemManager().getSubsystem(WsSubsystems.WS_VISION); + swerve = (SwerveDrive) Core.getSubsystemManager().getSubsystem(WsSubsystems.SWERVE_DRIVE); + limelight = (WsVision) Core.getSubsystemManager().getSubsystem(WsSubsystems.WS_VISION); } public String toString(){ return "Tag Align On"; diff --git a/src/main/java/org/wildstang/sample/robot/WsSubsystems.java b/src/main/java/org/wildstang/sample/robot/WsSubsystems.java index 616db7b7..96120698 100644 --- a/src/main/java/org/wildstang/sample/robot/WsSubsystems.java +++ b/src/main/java/org/wildstang/sample/robot/WsSubsystems.java @@ -11,7 +11,7 @@ public enum WsSubsystems implements Subsystems { // enumerate subsystems - //WS_VISION("Ws Vision", WsVision.class), + WS_VISION("Ws Vision", WsVision.class), SWERVE_DRIVE("Swerve Drive", SwerveDrive.class) ; diff --git a/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveDrive.java b/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveDrive.java index 95f417b6..6f27c355 100644 --- a/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/org/wildstang/sample/subsystems/swerve/SwerveDrive.java @@ -77,7 +77,7 @@ public class SwerveDrive extends SwerveDriveTemplate { private SwerveDriveOdometry odometry; private Timer autoTimer = new Timer(); - //private WsVision limelight; + private WsVision limelight; private LimeConsts LC; public enum driveType {TELEOP, AUTO, CROSS}; @@ -213,7 +213,7 @@ public void initOutputs() { }; //create default swerveSignal swerveSignal = new SwerveSignal(new double[]{0.0, 0.0, 0.0, 0.0}, new double[]{0.0, 0.0, 0.0, 0.0}); - //limelight = (WsVision) Core.getSubsystemManager().getSubsystem(WsSubsystems.WS_VISION); + limelight = (WsVision) Core.getSubsystemManager().getSubsystem(WsSubsystems.WS_VISION); odometry = new SwerveDriveOdometry(new SwerveDriveKinematics(new Translation2d(0.2794, 0.2794), new Translation2d(0.2794, -0.2794), new Translation2d(-0.2794, 0.2794), new Translation2d(-0.2794, -0.2794)), odoAngle(), odoPosition(), new Pose2d()); } @@ -255,8 +255,8 @@ public void update() { rotSpeed = Math.max(-0.2, Math.min(0.2, swerveHelper.getRotControl(pathTarget, getGyroAngle()))); //ensure rotation is never more than 0.2 to prevent normalization of translation from occuring if (autoTag){ - //xSpeed = limelight.getScoreX(aimOffset); - //ySpeed = limelight.getScoreY(vertOffset); + xSpeed = limelight.getScoreX(aimOffset); + ySpeed = limelight.getScoreY(vertOffset); if (Math.abs(xSpeed) > 0.3) xSpeed = Math.signum(xSpeed) * 0.3; if (Math.abs(ySpeed) > 0.3) ySpeed = Math.signum(ySpeed) * 0.3; if (Math.abs(pathVel * DriveConstants.DRIVE_F_V) > Math.abs(ySpeed*0.5)){ @@ -385,7 +385,6 @@ public void setGyro(double degrees) { public double getGyroAngle() { if (!isFieldCentric) return 0; - //limelight.setGyroValue((gyro.getYaw() + 360)%360); return (359.99 - gyro.getYaw()+360)%360; } public Rotation2d odoAngle(){ diff --git a/src/main/java/org/wildstang/sample/subsystems/targeting/LimelightHelpers.java b/src/main/java/org/wildstang/sample/subsystems/targeting/LimelightHelpers.java index 89da32a7..ea90bfa1 100644 --- a/src/main/java/org/wildstang/sample/subsystems/targeting/LimelightHelpers.java +++ b/src/main/java/org/wildstang/sample/subsystems/targeting/LimelightHelpers.java @@ -765,7 +765,7 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + //System.err.println("lljson error: " + e.getMessage()); } long end = System.nanoTime();