Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 00:38:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

Shooter Tuning

Browse Source
This commit is contained in:
ryan123rudder
2020-03-07 17:38:14 -07:00
parent 9787998778
commit 7f9b8d9c9f
4 changed files with 42 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -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;
src/main/java/frc4388/robot/commands/shooter/TrackTarget.java
+7 -5
View File
@@ -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);
src/main/java/frc4388/utility/ShooterTables.java
+20 -1
View File
@@ -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)) {