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 4782ff4..c20a589 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -116,8 +116,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; @@ -203,9 +202,9 @@ 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 TURN_P_VALUE = 0.65; + 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; public static final double DISTANCE_ERROR_EQUATION_M = 1.1279; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fd9a56c..122edeb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -119,7 +119,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 diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 6d35f49..a1e688e 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; @@ -31,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; @@ -48,6 +50,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. @@ -65,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; @@ -79,10 +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)); - SmartDashboard.putNumber("Distance to Target", distance); - + SmartDashboard.putNumber("Distance to Target", realDistance); + 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); @@ -92,8 +99,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 @@ -115,7 +122,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 { diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java index d8a4ac4..3e5dae3 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 ++; } @@ -126,6 +128,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)) {