From 4190491459ed75d6bdec241bf9ee794fee1ff98e Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sun, 13 Mar 2022 19:59:55 -0600 Subject: [PATCH] Fixed limelight aiming --- src/main/java/frc4388/robot/Constants.java | 7 ++-- src/main/java/frc4388/robot/Robot.java | 2 + .../java/frc4388/robot/RobotContainer.java | 6 +-- .../frc4388/robot/commands/TrackTarget.java | 39 +++++++++++++------ 4 files changed, 37 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 277d3a2..57ff6a1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -221,16 +221,17 @@ public final class Constants { // public static final double TARGET_HEIGHT = 67.5; // public static final double FOV = 29.8; //Field of view limelight - public static final double LIME_ANGLE = 50; + public static final double LIME_ANGLE = 56.4; public static final String NAME = "photonCamera"; public static final double TARGET_HEIGHT = 8*12 + 8; //TODO: Convert to metric (does this still need to be converted?) + public static final double LIME_HEIGHT = 26; public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?) public static final double H_FOV = 59.6; public static final double V_FOV = 49.7; - public static final double LIME_VIXELS = 960; - public static final double LIME_HIXELS = 720; + public static final double LIME_HIXELS = 960; + public static final double LIME_VIXELS = 720; public static final double TURRET_kP = 0.1; public static final double RANGE = 10; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ddee6d5..13c5558 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -214,6 +214,8 @@ public class Robot extends TimedRobot { } else LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); } + + m_robotContainer.m_robotVisionOdometry.setLEDs(false); } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index de36846..44f3c02 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -107,7 +107,7 @@ public class RobotContainer { public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); - private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); + public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31); public final Climber m_robotClimber = new Climber(testElbowMotor); @@ -304,8 +304,8 @@ public class RobotContainer { .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ //B > Shoot with Lime - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); /* Button Box Buttons */ diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index ffe50c5..8cb25c2 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -16,7 +16,9 @@ import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.DesmosServer; public class TrackTarget extends CommandBase { /** Creates a new TrackTarget. */ @@ -37,6 +39,8 @@ public class TrackTarget extends CommandBase { Pose2d pos = new Pose2d(); ArrayList points = new ArrayList<>(); + boolean isExecuted = false; + // public static Gains m_aimGains; public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, SwerveDrive drive, VisionOdometry visionOdometry) { @@ -64,25 +68,37 @@ public class TrackTarget extends CommandBase { try { m_visionOdometry.setLEDs(true); points = m_visionOdometry.getTargetPoints(); - double pointTotal = 0; - for(Point point : points) { - pointTotal = pointTotal + point.x; + for(int i = 0; i < points.size(); i++) { + DesmosServer.putPoint("Point" + i, points.get(i)); } - average = pointTotal/points.size(); - output = average/VisionConstants.LIME_HIXELS * VisionConstants.TURRET_kP; - m_turret.runTurretWithInput(output); - pos = m_visionOdometry.getVisionOdometry(); - distance = Math.hypot(pos.getX(), pos.getY()); + + Point average = VisionOdometry.averagePoint(points); + DesmosServer.putPoint("average", average); + + output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; + output *= 2; + DesmosServer.putDouble("output", output); + // m_turret.runTurretWithInput(output); + + double y_rot = average.y / VisionConstants.LIME_VIXELS; + y_rot *= Math.toRadians(VisionConstants.V_FOV); + y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; + y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + + double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); + DesmosServer.putDouble("distance", distance); + + // isExecuted = true; } catch (Exception e){ System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); } - vel = m_boomBoom.getVelocity(distance); - hood = m_boomBoom.getHood(distance); + // vel = m_boomBoom.getVelocity(distance); + // hood = m_boomBoom.getHood(distance); // m_boomBoom.runDrumShooter(vel); // m_boomBoom.runDrumShooterVelocityPID(vel); // m_hood.runAngleAdjustPID(hood); - //m_turret.runshooterRotatePID(m_targetAngle); + // m_turret.runshooterRotatePID(m_targetAngle); } // Called once the command ends or is interrupted. @@ -94,5 +110,6 @@ public class TrackTarget extends CommandBase { @Override public boolean isFinished() { return false; + // return isExecuted && Math.abs(output) < .1; } }