edited code, updated stuff

CyberCoyotes · May 2, 2024 · 935387c · 935387c
1 parent dae864f
commit 935387c
Show file tree

Hide file tree

Showing 4 changed files with 73 additions and 25 deletions.
diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -13,9 +13,33 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
 
 public class ArmSubsystem extends SubsystemBase {
 
+   //nasty limelight slop
+    NetworkTable limelightBack; // table for the limelight
+    NetworkTableEntry tid; // Table for the ID of target
+    NetworkTableEntry ty; // Table for the skew of the robot on a vertical axis
+/* 
+
+
+        
+    
+double TAG_ID = getID();
+double DISTANCE = DISTANCE_CALCULATIONS();
+    @Override
+    public void periodic(){
+    //if tag ID = 6 or 5 and the tag is a meter or less away, set arm pose to high
+System.out.println(DISTANCE_CALCULATIONS());
+System.out.println(getID());   
+if(getID() == 1.0){
+        setArmPose(55);
+    }
+}
+*/
     // Declare a variable for the motor you want to control
     private final TalonFX m_arm;
 
@@ -24,6 +48,12 @@ public class ArmSubsystem extends SubsystemBase {
 
     public ArmSubsystem() {
 
+        //more yummy slop
+       limelightBack = NetworkTableInstance.getDefault().getTable("limelight");
+      tid = limelightBack.getEntry("tid"); 
+       ty = limelightBack.getEntry("ty");
+
+
         // Initialize the motor in the constructor with the motor ID and optional canbus
         // ID
         m_arm = new TalonFX(Constants.CANIDs.ARM_ID);
@@ -104,6 +134,8 @@ public ArmSubsystem() {
 
     } /* End of the class-method */
 
+
+
     public StatusSignal<Double> getArmPos() {
         /* Reusing from drivetrain subsystem */
         return m_arm.getPosition();
@@ -121,6 +153,36 @@ public void setArmPose(double armPose) {
 
     }
 
+    //MMMMMMMMMMMMMMMMMMMMMM YUMMY SLOP slorp slorp slorp slorp
+    //gets the ID of the selected apriltag 
+        public double getID(){
+            return tid.getDouble(-1.0);
+        } 
+
+        // distance from the bot to the april tag in centimeters using trig
+        //CHANGE 170 to 122 after TEST!!!! !! ! !!!! 
+    public double DISTANCE_CALCULATIONS(){
+        double angleToGoalRadians = (ty.getDouble(0.0)) * (3.14159/180.0);
+        return (170 - CAMERA_HEIGHT) / Math.tan(angleToGoalRadians);
+    }
+
+     double CAMERA_HEIGHT = 156; 
+    //crescendo bot height from floor to middle of LL camera is 56
+    //CHANGE POST TEST
+    double CAMERA_ANGLE = 28.61;
+
+    double TAG_ID = getID();
+double DISTANCE = DISTANCE_CALCULATIONS();
+    @Override
+    public void periodic(){
+    //if tag ID = 6 or 5 and the tag is a meter or less away, set arm pose to high
+System.out.println(DISTANCE_CALCULATIONS());
+System.out.println(getID());   
+// proof of concept, please work 
+if(getID() == 1.0){
+        setArmPose(55);
+    }
+}
 
     /*
      * Currently only being called in subsystem-command;
@@ -133,20 +195,8 @@ public void showArmTelemetry() {
         SmartDashboard.putNumber("Arm Voltage", m_arm.getMotorVoltage().getValue());
     }
 
-    VisionSubsystem vision = new VisionSubsystem();
 
-    @Override
-    public void periodic(){
-    double TAG_ID = vision.getID();
-    double DISTANCE = vision.DISTANCE_CALCULATIONS();
-    //if tag ID = 6 or 5 and the tag is a meter or less away, set arm pose to high
-    if((TAG_ID == 1.0)
-    //&&(DISTANCE <= 100)
-    ){
-        setArmPose(55);
-    }
 
-}
 } // end of ArmSubsystem method
 
 /***************************

diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -233,17 +233,15 @@ public Pose2d getTargetPose(){
         }
     }
 */
-/*ArmSubsystem arm = new ArmSubsystem();
+ArmSubsystem arm = new ArmSubsystem();
 
 @Override
 public void periodic(){
-    double TAG_ID = getID();
-    double DISTANCE = DISTANCE_CALCULATIONS();
     //if tag ID = 6 or 5 and the tag is a meter or less away, set arm pose to high
-    if((TAG_ID == 1.0)&&(DISTANCE <= 100)){
+    if((getID() == 1.0)&&(DISTANCE_CALCULATIONS() <= 100)){
         arm.setArmPose(55);
     }
-}*/
+}
 
 }
 // end of class
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.2.4",
+    "version": "2024.2.8",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,15 +12,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.2.4"
+            "version": "2024.2.8"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.2.4",
+            "version": "2024.2.8",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,

diff --git a/vendordeps/playingwithfusion2024.json b/vendordeps/playingwithfusion2024.json
@@ -1,7 +1,7 @@
 {
     "fileName": "playingwithfusion2024.json",
     "name": "PlayingWithFusion",
-    "version": "2024.01.28",
+    "version": "2024.03.09",
     "uuid": "14b8ad04-24df-11ea-978f-2e728ce88125",
     "frcYear": "2024",
     "jsonUrl": "https://www.playingwithfusion.com/frc/playingwithfusion2024.json",
@@ -12,14 +12,14 @@
         {
             "groupId": "com.playingwithfusion.frc",
             "artifactId": "PlayingWithFusion-java",
-            "version": "2024.01.28"
+            "version": "2024.03.09"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.playingwithfusion.frc",
             "artifactId": "PlayingWithFusion-driver",
-            "version": "2024.01.28",
+            "version": "2024.03.09",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -36,7 +36,7 @@
         {
             "groupId": "com.playingwithfusion.frc",
             "artifactId": "PlayingWithFusion-cpp",
-            "version": "2024.01.28",
+            "version": "2024.03.09",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "libName": "PlayingWithFusion",
@@ -53,7 +53,7 @@
         {
             "groupId": "com.playingwithfusion.frc",
             "artifactId": "PlayingWithFusion-driver",
-            "version": "2024.01.28",
+            "version": "2024.03.09",
             "headerClassifier": "headers",
             "sharedLibrary": true,
             "libName": "PlayingWithFusionDriver",