From c12e657aeee7d361fd4707a9662fcd9f2ee182e6 Mon Sep 17 00:00:00 2001
From: Astatin3 <mj@mikovsky.com>
Date: Thu, 16 Feb 2023 19:46:21 -0700
Subject: [PATCH] Update that does not currently work

Work on shuffleboard integration
---
 simgui.json                                       | 15 +++++++++++++++
 src/main/java/frc4388/robot/Constants.java        |  2 +-
 src/main/java/frc4388/robot/Robot.java            | 15 ++++++++++++---
 .../java/frc4388/robot/subsystems/Apriltags.java  | 15 +++++++++++----
 .../java/frc4388/robot/subsystems/Location.java   |  6 +-----
 .../frc4388/robot/subsystems/SwerveDrive.java     |  2 +-
 6 files changed, 41 insertions(+), 14 deletions(-)

diff --git a/simgui.json b/simgui.json
index 64da7ff..59ccda3 100644
--- a/simgui.json
+++ b/simgui.json
@@ -5,6 +5,15 @@
     }
   },
   "NetworkTables": {
+    "Persistent Values": {
+      "open": false
+    },
+    "Retained Values": {
+      "open": false
+    },
+    "Transitory Values": {
+      "open": false
+    },
     "retained": {
       "apriltag": {
         "open": true
@@ -55,6 +64,12 @@
       },
       "open": true
     },
+    "shuffleboard": {
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
     "visible": true
   }
 }
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index e0b2dee..57db05a 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -139,6 +139,6 @@ public final class Constants {
     public static final int XBOX_DRIVER_ID = 0;
     public static final int XBOX_OPERATOR_ID = 1;
     public static final double LEFT_AXIS_DEADBAND = 0.04;
-    public static final boolean SKEW_STICKS = false;
+    public static final boolean SKEW_STICKS = false; // TODO: find the actual value
   }
 }
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index ca33f4c..d57fa12 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -15,7 +15,10 @@ import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.utility.RobotTime;
-import frc4388.robot.subsystems.Vision;
+
+import frc4388.robot.subsystems.Location;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -30,7 +33,7 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
-  //private Vision Apriltag = new Vision();
+  private Location location = new Location();
 
 
   /**
@@ -61,7 +64,13 @@ public class Robot extends TimedRobot {
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
 
-    //final Object[] apriltagPos = Apriltag.getApriltagLocation();
+    // Call position and rotation reorder
+    location.reoderPrio();
+
+    final Object[] pos = location.getPosition();
+    if (pos != null) {
+      SmartDashboard.putNumber("name", (Double) pos[0]);
+    }
 
     //ystem.out.print(apriltagPos[0]);
   }
diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java
index ccf1617..48b097c 100644
--- a/src/main/java/frc4388/robot/subsystems/Apriltags.java
+++ b/src/main/java/frc4388/robot/subsystems/Apriltags.java
@@ -11,10 +11,16 @@ public class Apriltags {
     public Object[] getApriltagPosition() {
         final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
 
+        // return new Object[] {true, 
+        //     tagTable.getEntry("TagPosX"),
+        //     tagTable.getEntry("TagPosY"),
+        //     tagTable.getEntry("TagPosZ")
+        // };
+
         return new Object[] {true, 
-            tagTable.getEntry("TagPosX"),
-            tagTable.getEntry("TagPosY"),
-            tagTable.getEntry("TagPosZ")
+            1,
+            2,
+            3
         };
     }
 
@@ -30,6 +36,7 @@ public class Apriltags {
 
     public boolean isAprilTag() {
         final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
-        return tagTable.getEntry("IsTag").getBoolean(false);
+        // return tagTable.getEntry("IsTag").getBoolean(false);
+        return true;
     }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Location.java b/src/main/java/frc4388/robot/subsystems/Location.java
index b3e786a..3683629 100644
--- a/src/main/java/frc4388/robot/subsystems/Location.java
+++ b/src/main/java/frc4388/robot/subsystems/Location.java
@@ -15,17 +15,13 @@ public class Location {
     }
 
     public Object[] getPosition() {
-        Object[] Position = {};
-
         if(isLimelight){
             //Return Limelight Position
         }else if(isApriltag){
             return Apriltag.getApriltagPosition();
-        }else{
-            //Return odometry Position, last resort
         }
 
-        return Position;
+        return null;
     }
 
     public Object[] getRotation() {
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index d93f892..e9ef898 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -28,7 +28,7 @@ public class SwerveDrive extends SubsystemBase {
   
   private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); 
 
-  // private SwerveDriveOdometry odometry = new SwerveDriveOdometry(
+  // private SwerveDriveOdometry odometry = new SwerveDrive(
   //   kinematics, 
   //   gyro.getRotation2d(),
   //   new SwerveModulePosition[] {