From a856a6633976af5c72f2c28eeca7357f5b4e01a6 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 7 Mar 2020 10:09:44 -0700 Subject: [PATCH 1/4] P val change Co-Authored-By: Keenan D. Buckley --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/commands/shooter/TrackTarget.java | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b951b15..221430e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -200,7 +200,7 @@ public final class Constants { public static final double FOV = 29.8; //Field of view of limelight public static final double TARGET_HEIGHT = 71; public static final double LIME_ANGLE = 25; - public static final double TURN_P_VALUE = 0.65; + 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; public static final double DISTANCE_ERROR_EQUATION_M = 1.1279; diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 6d35f49..16b770a 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -12,6 +12,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.VisionConstants; +import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.ShooterHood; @@ -48,6 +49,7 @@ public class TrackTarget extends CommandBase { m_shooterHood = m_shooter.m_shooterHoodSubsystem; addRequirements(m_shooterAim); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); } // Called when the command is initially scheduled. @@ -82,7 +84,8 @@ public class TrackTarget extends CommandBase { // Finding Distance distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); SmartDashboard.putNumber("Distance to Target", distance); - + 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)); //START Equation Code double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT); double xVel = (distance * VisionConstants.GRAV) / (yVel); @@ -115,7 +118,7 @@ public class TrackTarget extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (xAngle < 1 && xAngle > -1 && target == 1) + if (xAngle < 0.5 && xAngle > -0.5 && target == 1) { m_shooterAim.m_isAimReady = true; } else { From 9d4f9f7020a6b504337728cc0d3edbb74b55ab13 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 7 Mar 2020 11:22:32 -0700 Subject: [PATCH 2/4] Lime distance within 5 in Co-Authored-By: Keenan D. Buckley --- src/main/java/frc4388/robot/Constants.java | 4 ++-- .../java/frc4388/robot/commands/shooter/TrackTarget.java | 8 +++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 221430e..e9ac88d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -198,8 +198,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 = 71; - public static final double LIME_ANGLE = 25; + public static final double TARGET_HEIGHT = 66.75; + public static final double LIME_ANGLE = 28.387; 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 16b770a..d4bb084 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -32,6 +32,7 @@ public class TrackTarget extends CommandBase { double yAngle = 0; double target = 0; public double distance; + public double realDistance; public static double fireVel; public static double fireAngle; @@ -83,7 +84,8 @@ public class TrackTarget extends CommandBase { // Finding Distance distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); - SmartDashboard.putNumber("Distance to Target", distance); + 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)); //START Equation Code @@ -95,8 +97,8 @@ public class TrackTarget extends CommandBase { //END Equation Code //START CSV Code - fireVel = m_shooter.m_shooterTable.getVelocity(distance); - fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different + fireVel = m_shooter.m_shooterTable.getVelocity(realDistance); + fireAngle = m_shooter.m_shooterTable.getHood(realDistance); //Note: Ensure to follow because units are different //fireAngle = 33; //END CSV Code From 9787998778af1ce5c765b18aaba9fbb6a126bfd6 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 7 Mar 2020 13:22:19 -0700 Subject: [PATCH 3/4] PID fix and Idle --- src/main/java/frc4388/robot/Constants.java | 3 +-- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e9ac88d..de9ae73 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -122,8 +122,7 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); - //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0); + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0); public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c44884a..cc622d4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -117,7 +117,7 @@ public class RobotContainer { // runs the turret with joystick m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); // moves the drum not - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(1500), m_robotShooter)); // drives climber with input from triggers on the opperator controller m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // drives the leveler with an axis input from the driver controller 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 4/4] 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)) {