From 632fc12ad0e5f9499d7b8678094f1cc8b014aed3 Mon Sep 17 00:00:00 2001
From: Pushkar9236 <104144@psdschools.org>
Date: Sat, 22 Jan 2022 11:00:53 -0700
Subject: [PATCH] finished vision code, still needs a few improvements

---
 src/main/java/frc4388/robot/Constants.java    |  19 ++-
 .../java/frc4388/robot/subsystems/Vision.java | 108 ++++++++++++++++--
 2 files changed, 117 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 95209ee..b085102 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -98,7 +98,24 @@ public final class Constants {
     public static final int SHOOTER_FALCON_BALLER_ID =; //unknown value, fix later//
     public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID =; //"//
   
-    public static final Gains DRUM_SHOOTER_GAINS = new Gains(x,y,z,a,b);//x,y,z,a,b are not actual values, fix laterS
+    public static final Gains DRUM_SHOOTER_GAINS = new Gains(x,y,z,a,b);//x,y,z,a,b are not actual values, fix later
+
+
+    public static final class VisionConstants {
+      public static final double TURN_P_VALUE = "unknown" // needs to be figured out after testing
+      public static final double X_ANGLE_ERROR = "alsoUnknown" //""//
+      public static final double TURN_P_VALUE = "alsoAlsoUnknown" //""//
+      public static final double GRAV = "alsoAlsoAlsoUnknown" //""//
+      public static final double TARGET_HEIGHT = "alsoAlsoAlsoAlsoUnknown" //""//
+      
+
+
+
+
+
+
+
+    }
 
   }
 }
diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java
index f6cc008..76ba03d 100644
--- a/src/main/java/frc4388/robot/subsystems/Vision.java
+++ b/src/main/java/frc4388/robot/subsystems/Vision.java
@@ -5,15 +5,20 @@
 package frc4388.robot.subsystems;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-
-
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj2.command.commandBase;
+import frc4388.robot.constants.VisionConstants;
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.Turret;
+import frc4388.robot.subsystems.Hood;
+import frc4388.utility.controller.IHandController;
 
 public class Vision extends SubsystemBase {
 //setup
   Turret m_turret;
-BoomBoom m_boomBoom;
-Hood m_hood;
+  BoomBoom m_boomBoom;
+  Hood m_hood;
 
 NetworkTableEntry xEntry;
 IHandController m_driverController;
@@ -38,10 +43,95 @@ public TrackTarget(ShooterAim aimSubsystem, LimeLight limeLight) {
   m_hood = m_boomBoom.m_hoodSubsystem;
   m_limeLight = limeLight;
   addRequirements(m_turret);
-
+  limeOff();
+  changePipeline(0);
+  NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+  NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(3);
 }
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
+
+public void track(){
+  target = getV()
+  xAngle = getX()
+  yAngle = getY()
+
+  //find distance
+  distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
+  realDistance = (1.09 * distance) - 12.8;
+
+  if (target == 1.0) { //checks if target is in view
+    //aims left and right
+    turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE);
+    if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
+      turnAmount = 0;
+    } 
+    else if (turnAmount > 0 && turnAmount < 0.1){
+      turnAmount = 0.1;
+    }
+    else if (turnAmount < 0 && turnAmount > -0.1){
+        turnAmount = -0.1
+    }
+  }
+  m_turret.runShooterWithInput(-turnAmount);
+
+  SmartDashboard.putNumber("Disance to Target", realDistance);
+
+
+  //start CSV
+
+  fireVel = m_boomBoom.m_shooterTable.getVelocity(realDistance);
+  fireAngle = m_boomBoom.m_shooterTable.getHood(realDistance);
+  //fire angle unknown so far
+  //end of CSV
+
+  m_boomBoom.m_fireVel = fireVel;
+  m_hood.m_fireAngle = fireAngle;
+  m_turret.m_targetDistance = distance;
+
+  checkFinished();
+}
+
+public void checkFinished(){
+  if (xAngle < 0.5 && xAngle > -0.5 && target == 1){
+    m_turret.m_isAimReady = true;
+  }
+  else{
+    m_turret.m_isAimReady = false;
   }
 }
+
+public void limeOff(){
+  NetworkTableInstance.getDefault.getTable("limelight").getEntry("camMode").setNumber(0);
+  NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
+}
+
+  public void limeOn(){
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
+}
+
+  public void changePipeline(int pipelineId)
+  {
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId);
+  }
+  
+  public double getV()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
+  }
+
+  public double getX()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
+  }
+
+  public double getY()
+  {
+    return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
+  }
+  @Override
+  public void periodic(){
+    //called once per scheduler run
+  }
+}
+
+