Reimplementing Limelight and auto methods

wildstang · Jan 20, 2024 · ba62e65 · ba62e65
1 parent e4cfc19
commit ba62e65
Show file tree

Hide file tree

Showing 6 changed files with 23 additions and 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
@@ -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
@@ -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
@@ -12,22 +12,22 @@
 
 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){
         color = 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
@@ -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
@@ -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
@@ -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();