From 7f9b8d9c9f49536128755d70d78b343bdc31ff6d Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 7 Mar 2020 17:38:14 -0700 Subject: [PATCH] Shooter Tuning --- src/main/deploy/Robot Data - Distances.csv | 22 +++++++++++-------- src/main/java/frc4388/robot/Constants.java | 4 ++-- .../robot/commands/shooter/TrackTarget.java | 12 +++++----- .../java/frc4388/utility/ShooterTables.java | 21 +++++++++++++++++- 4 files changed, 42 insertions(+), 17 deletions(-) diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv index 5a625da..b60700b 100644 --- a/src/main/deploy/Robot Data - Distances.csv +++ b/src/main/deploy/Robot Data - Distances.csv @@ -1,9 +1,13 @@ -Distance (in),Hood Ext. (u),Drum Velocity (u/ds) -74,20,8000 -144,23,10000 -162,28,10000 -208,29,10000 -296,32,12500 -418,33,16000 -430,31,16000 -450,31,16000 \ No newline at end of file +Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg) +1,10,7000,2 +70,20,8000,2, double check +100,24,9000,2 +145,28,10000,1,Add a 200 +230,31,12000,0,Add a 270 +320,32,17000,0,change 17000 and mark them lower +331,33,17000,0 +397,33,16000,0 +415,33,16250,0 +436,31,18000,0 +500,33,18000,0 +501,33,18000,0 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index de9ae73..c0e3dbf 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -197,8 +197,8 @@ public final class Constants { public static final class VisionConstants { public static final double FOV = 29.8; //Field of view of limelight - public static final double TARGET_HEIGHT = 66.75; - public static final double LIME_ANGLE = 28.387; + public static final double TARGET_HEIGHT = 71.5; + public static final double LIME_ANGLE = 24.7; public static final double TURN_P_VALUE = 0.6; public static final double X_ANGLE_ERROR = 1.3; public static final double MOTOR_DEAD_ZONE = 0.3; diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index d4bb084..a1e688e 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -68,8 +68,14 @@ public class TrackTarget extends CommandBase { xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); + // Finding Distance + distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); + realDistance = (1.09 * distance) - 12.8; + + if (target == 1.0) { // If target in view // Aiming Left/Right + xAngle = xAngle + m_shooter.m_shooterTable.getCenterDisplacement(realDistance); turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE; if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { turnAmount = 0; @@ -82,12 +88,8 @@ public class TrackTarget extends CommandBase { } m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim); - // Finding Distance - distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); - realDistance = (1.13 * distance) - 14.3; SmartDashboard.putNumber("Distance to Target", realDistance); - SmartDashboard.putNumber("ts Skew or Rotation", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ts").getDouble(0)); - SmartDashboard.putNumber("ta Area", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0)); + SmartDashboard.putNumber("Center Displacement", m_shooter.m_shooterTable.getCenterDisplacement(realDistance)); //START Equation Code double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT); double xVel = (distance * VisionConstants.GRAV) / (yVel); diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java index 85244ce..62af09e 100644 --- a/src/main/java/frc4388/utility/ShooterTables.java +++ b/src/main/java/frc4388/utility/ShooterTables.java @@ -20,12 +20,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; * Add your docs here. */ public class ShooterTables { - double[][] m_distance = new double[50][3]; + double[][] m_distance = new double[50][4]; double[][] m_angle = new double[50][2]; final int m_columnDistance = 0; final int m_columnHood = 1; final int m_columnVel = 2; + final int m_columnCenterDisplacement = 3; final int m_columnAngle = 0; final int m_columnDisplacement = 1; @@ -55,6 +56,7 @@ public class ShooterTables { m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]); m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]); m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]); + m_distance[lineNum - 1][m_columnCenterDisplacement] = Double.parseDouble(values[3]); lineNum ++; } @@ -123,6 +125,23 @@ public class ShooterTables { } } + public double getCenterDisplacement(double distance) { //Degrees of Lime FOV + int i = 0; + while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { + i ++; + } + if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { + return m_distance[i][m_columnCenterDisplacement]; + } else { + if (i >= m_distanceLength) { + i = m_distanceLength - 1; + } + return linearInterpolation(i, distance, m_columnCenterDisplacement, m_distance); + } + } + + + public double getAngleDisplacement(double angle) { int i = 0; while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) {