Push for functional drivebase, but limelight is out

wildstang · Jan 19, 2024 · 676d392 · 676d392
1 parent 52c139b
commit 676d392
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 22 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
@@ -12,7 +12,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),
     //SAMPLE("Sample", SampleSubsystem.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)){