add code from robot session

Team766 · Nov 7, 2024 · 2f36124 · 2f36124
1 parent e6c8497
commit 2f36124
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 11 deletions.
diff --git a/src/main/java/com/team766/robot/reva/VisionUtil/VisionPIDProcedure.java b/src/main/java/com/team766/robot/reva/VisionUtil/VisionPIDProcedure.java
@@ -12,8 +12,8 @@ public abstract class VisionPIDProcedure extends Procedure {
 
     protected PIDController yawPID = new PIDController(0.02, 0.001, 0, 0, -0.25, 0.25, 3);
 
-    protected PIDController anglePID = new PIDController(1.8, 0, 0, 0, -0.8, 0.8, 0.05);
-
+    //protected PIDController anglePID = new PIDController(1.8, 0, 0, 0, -0.8, 0.8, 0.05);
+    protected PIDController anglePID = new PIDController(0.9, 0, 0, 0, -0.8, 0.8, 0.05);
     /*
      * Scoringposition distances need to be in sequential order. Ie, the first one added needs to be the closest distance.
      */

diff --git a/src/main/java/com/team766/robot/reva/procedures/DriverShootNow.java b/src/main/java/com/team766/robot/reva/procedures/DriverShootNow.java
@@ -53,13 +53,14 @@ public void run(Context context) {
         }
 
         double x = toUse.getX();
-        double y = toUse.getY();
+        double z = toUse.getZ();
 
         anglePID.setSetpoint(0);
 
         double distanceOfRobotToTag =
-                Math.sqrt(Math.pow(toUse.getX(), 2) + Math.pow(toUse.getY(), 2));
-
+                Math.sqrt(Math.pow(toUse.getX(), 2) + Math.pow(toUse.getZ(), 2));
+
+        log("DIST: " + distanceOfRobotToTag);
         if (distanceOfRobotToTag
                 > VisionPIDProcedure.scoringPositions
                         .get(VisionPIDProcedure.scoringPositions.size() - 1)
@@ -79,30 +80,35 @@ public void run(Context context) {
         // Robot.shooter.shoot(power);
 
         Robot.shoulder.rotate(armAngle);
+        log("ArmAngle: " + armAngle);
+
+        angle = Math.atan2(x,z);
 
-        angle = Math.atan2(y, x);
+        log("ROBOT ANGLE: " + angle);
 
         anglePID.calculate(angle);
 
+        log("ANGLE PID: " + anglePID.getOutput());
+
         while (Math.abs(anglePID.getOutput()) > 0.075) {
             context.yield();
 
             // SmartDashboard.putNumber("[ANGLE PID OUTPUT]", anglePID.getOutput());
             // SmartDashboard.putNumber("[ANGLE PID ROTATION]", angle);
             try {
-                toUse = getTransform3dOfRobotToTag();
+                toUse = getTransform3dOfRobotToTagOrin();
 
-                y = toUse.getY();
+                z = toUse.getZ();
                 x = toUse.getX();
 
-                angle = Math.atan2(y, x);
+                angle = Math.atan2(x,z);
 
                 anglePID.calculate(angle);
-            } catch (AprilTagGeneralCheckedException e) {
+            } catch (NoTagFoundError e) {
                 continue;
             }
 
-            Robot.drive.controlRobotOriented(0, 0, -anglePID.getOutput());
+            Robot.drive.controlRobotOriented(0, 0, anglePID.getOutput());
         }
 
         Robot.drive.stopDrive();
@@ -126,6 +132,7 @@ private Transform3d getTransform3dOfRobotToTag() throws AprilTagGeneralCheckedEx
     private Transform3d getTransform3dOfRobotToTagOrin() throws NoTagFoundError {
         AprilTag tag = Robot.orin.getTagById(tagId);
 
+        log(tag.toString());
         Pose3d pose = tag.pose;
 
         Transform3d poseNew =