diff --git a/build.gradle b/build.gradle index c1e3041..2a7ec8c 100644 --- a/build.gradle +++ b/build.gradle @@ -50,7 +50,6 @@ dependencies { nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) testCompile 'junit:junit:4.12' - compile 'com.2train395.limelight.api:limelight-api:1.0.0' } // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index d27dab8..0f5ee11 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -6,9 +6,8 @@ import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.buttons.JoystickButton; import edu.wpi.first.wpilibj.buttons.Trigger; -import frc.robot.commands.*; -import frc.robot.triggers.*; -import frc.robot.commands.ElevatorPreset.PresetHeight;; +import frc.robot.commands.ApproachTarget; +import frc.robot.enums.TargetType; public class OI { Joystick leftJoystick = new Joystick(0); @@ -27,13 +26,13 @@ public class OI { static final double xboxDeadzone = 0.25; public void setUpTriggers() { - elevatorTrigger = new ElevatorTrigger(); - elevatorTrigger.whenActive(new ElevatorJoystick()); - high.whenPressed(new ElevatorPreset(PresetHeight.kMaxHeight)); - medium.whenPressed(new ElevatorPreset(PresetHeight.kCargoMedium)); - low.whenPressed(new ElevatorPreset(PresetHeight.kCargoLow)); - stick.whenPressed(new ElevatorPreset(PresetHeight.kZero)); - climbMode.whileHeld(new LevelRobot()); + // elevatorTrigger = new ElevatorTrigger(); + // elevatorTrigger.whenActive(new ElevatorJoystick()); + // high.whenPressed(new ElevatorPreset(PresetHeight.kMaxHeight)); + // medium.whenPressed(new ElevatorPreset(PresetHeight.kCargoMedium)); + // low.whenPressed(new ElevatorPreset(PresetHeight.kCargoLow)); + // stick.whenPressed(new ElevatorPreset(PresetHeight.kZero)); + climbMode.whenPressed(new ApproachTarget(TargetType.kLowTarget)); } private double getJoyY(Joystick stick) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 23b6ab5..e2fae63 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,8 @@ import frc.robot.subsystems.DrivetrainGyro; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.RollerIntake; +import frc.robot.utils.limelight.Corners; +import frc.robot.utils.limelight.Limelight; /** * The VM is configured to automatically run this class, and to call the @@ -87,12 +89,15 @@ public void teleopInit() { @Override public void teleopPeriodic() { Scheduler.getInstance().run(); + Corners corners = Limelight.getContourCorners(); + if(corners.validCorners) + SmartDashboard.putBoolean("Left", corners.topRight.y > corners.topLeft.y); } - - /** - * This function is called periodically during test mode. - */ - @Override - public void testPeriodic() { - } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } } diff --git a/src/main/java/frc/robot/commands/AimAtOffset.java b/src/main/java/frc/robot/commands/AimAtOffset.java new file mode 100644 index 0000000..288c826 --- /dev/null +++ b/src/main/java/frc/robot/commands/AimAtOffset.java @@ -0,0 +1,81 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.utils.limelight.Contour; +import frc.robot.Robot; +import frc.robot.utils.limelight.*; +import frc.robot.enums.TargetType; + +public class AimAtOffset extends Command { + + TargetType targetType; + Contour contour; + + //Full power at 15 degrees + double p = 1.0/50; + //We want to overshoot/not settle this turn + double d = p * 10; + double maxOutput = 0.3; + //We only need this to roughly align, this gives time to either stop early or accelerate past + double acceptableErrorDegrees = 5; + + //Initalize a PIDController with a 10 ms period + PIDController pidController = new PIDController(p, 0, d, Robot.gyro.getYawSource(), Robot.drivetrain.getTurnOutput(), 0.01); + + public AimAtOffset(TargetType targetType) { + requires(Robot.drivetrain); + setInterruptible(false); + + this.targetType = targetType; + pidController.setOutputRange(-maxOutput, maxOutput); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + contour = Limelight.getBestContour(); + + if(contour != null) { + Corners corners = Limelight.getContourCorners(); + + double totalOffset = HeadingOffsetCalculator.calculateTotalOffset(contour, corners, targetType); + pidController.setSetpoint(Robot.gyro.getYaw() + totalOffset); + pidController.enable(); + } + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + //Not using onTarget as that may do stability checking, this should end as soon as we hit the range at all + return Math.abs(pidController.getError()) < acceptableErrorDegrees || contour == null; + } + + // Called once after isFinished returns true + @Override + protected void end() { + pidController.disable(); + Robot.drivetrain.tankDrive(0,0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/src/main/java/frc/robot/commands/AimAtOffset.java~df54caca7c2c7f198fa8a60ba76eb01d56961919 b/src/main/java/frc/robot/commands/AimAtOffset.java~df54caca7c2c7f198fa8a60ba76eb01d56961919 new file mode 100644 index 0000000..5946d64 --- /dev/null +++ b/src/main/java/frc/robot/commands/AimAtOffset.java~df54caca7c2c7f198fa8a60ba76eb01d56961919 @@ -0,0 +1,106 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.commands.DriveToTarget.Side; +import frc.robot.utils.limelight.Contour; +import frc.robot.Robot; +import frc.robot.utils.limelight.*; + +public class AimAtOffset extends Command { + static final double aggressiveSkewThreshold = 100000; //TODO: Tune + static final double maxOffsetAggressive = 20; //TODO: Tune + static final double maxOffsetNormal = 15; + static final double maxOffsetDistance = 6.0; + + Contour contour; + Side side; + final double targetHeight; + double initialSkewAbs; + double xOffset; + double distance; + + static final double cameraHeight = 8.375; + static final double cameraAngle = 30; + + static final double lowTargetHeight = 28.337; + static final double highTargetHeight = 35.962; + //Full power at 15 degrees + double p = 1.0/50; + //We want to overshoot/not settle this turn + double d = p * 10; + double maxOutput = 0.3; + //We only need this to roughly align, this gives time to either stop early or accelerate past + double acceptableErrorDegrees = 5; + + //Initalize a PIDController with a 10 ms period + PIDController pidController = new PIDController(p, 0, d, Robot.gyro.getYawSource(), Robot.drivetrain.getTurnOutput(), 0.01); + + public AimAtOffset() { + requires(Robot.drivetrain); + targetHeight = lowTargetHeight; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + contour = Limelight.getBestContour(); + if(contour != null) { + Corners corners = Limelight.getContourCorners(); + if(corners.validCorners) { + side = corners.topRight.y > corners.topLeft.y ? Side.kRight : Side.kLeft; + } + + double yOffset = contour.ty; + xOffset = contour.tx; + + distance = (targetHeight - cameraHeight) / + Math.tan(Math.toRadians(cameraAngle + yOffset)) / 12; + + double maxOffset = initialSkewAbs > aggressiveSkewThreshold ? maxOffsetAggressive : maxOffsetNormal; + double additionalOffsetSign = side == Side.kLeft ? -1 : 1; + double distanceScaling = Math.min(1, (distance * distance) / (maxOffsetDistance * maxOffsetDistance)); + + double additionalOffset = maxOffset * distanceScaling * additionalOffsetSign; + + pidController.setOutputRange(-maxOutput, maxOutput); + + pidController.setSetpoint(Robot.gyro.getYaw() - (xOffset + additionalOffset)); + pidController.enable(); + } + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + //Not using onTarget as that may do stability checking, this should end as soon as we hit the range at all + return Math.abs(pidController.getError()) < acceptableErrorDegrees || + contour == null; + } + + // Called once after isFinished returns true + @Override + protected void end() { + pidController.disable(); + Robot.drivetrain.tankDrive(0,0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/src/main/java/frc/robot/commands/ApproachTarget.java b/src/main/java/frc/robot/commands/ApproachTarget.java new file mode 100644 index 0000000..b3acdcc --- /dev/null +++ b/src/main/java/frc/robot/commands/ApproachTarget.java @@ -0,0 +1,13 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.enums.TargetType; + +public class ApproachTarget extends CommandGroup { + + public ApproachTarget(TargetType targetType) { + addSequential(new AimAtOffset(targetType)); + addSequential(new DriveToTarget(targetType)); + } + +} diff --git a/src/main/java/frc/robot/commands/DriveToTarget.java b/src/main/java/frc/robot/commands/DriveToTarget.java new file mode 100644 index 0000000..617943f --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToTarget.java @@ -0,0 +1,111 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.Robot; +import frc.robot.subsystems.Drivetrain.Gear; +import frc.robot.utils.LinearOutput; +import frc.robot.utils.limelight.Contour; +import frc.robot.utils.limelight.Corners; +import frc.robot.utils.limelight.HeadingOffsetCalculator; +import frc.robot.utils.limelight.Limelight; +import frc.robot.enums.TargetType; + +/** + * A command designed to approach a target perpendicularly which the robot is already pointed at. + * All units are in feet and degrees unless otherwise notated. + */ +public class DriveToTarget extends Command { + + //The amount of time the command will continue to run without seeing a contour in seconds. + static final double maxTimeWithoutContour = 0.30; + //A double variable holding the last time a valid contour was seen. + double contourLastSeenTime; + + //The PID coefficients of the linear drive. + static final double p = 0.075; + static final double i = 0; + static final double d = 0; + static final double maxOutput = 0.3; + static final double minOutput = 0.15; + + //The proportional constant for the angle adjustment + static final double rotationP = -0.005; + + //The PID controller and PIDOutput. We maintain a reference to linearOutput to set the heading correction. + LinearOutput linearOutput = Robot.drivetrain.getLinearOutput(); + PIDController pidController = new PIDController(p, i, d, Robot.encoders, linearOutput); + + TargetType targetType; + + Contour contour; + + public DriveToTarget(TargetType targetType) { + requires(Robot.drivetrain); + setInterruptible(false); + + this.targetType = targetType; + + pidController.setOutputRange(minOutput, maxOutput); + } + + + + // Called just before this Command runs the first time + @Override + protected void initialize() { + //Drive slow in + Robot.drivetrain.shift(Gear.kLow); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + contour = Limelight.getBestContour(); + + if(contour != null) { + contourLastSeenTime = Timer.getFPGATimestamp(); + + Corners corners = Limelight.getContourCorners(); + + //Set the setpoint for distance relative to current position + pidController.setSetpoint( + Robot.encoders.getAveragedEncoderFeet() + + HeadingOffsetCalculator.calculateDistance(contour, targetType) + ); + + linearOutput.setHeadingCorrection( + rotationP * HeadingOffsetCalculator.calculateTotalOffset(contour, corners, targetType) + ); + } else { + linearOutput.setHeadingCorrection(0); + } + + if(!pidController.isEnabled()){ + pidController.enable(); + } + + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return Timer.getFPGATimestamp() - contourLastSeenTime > maxTimeWithoutContour; + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.drivetrain.tankDrive(0, 0); + pidController.disable(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } + +} diff --git a/src/main/java/frc/robot/enums/TargetType.java b/src/main/java/frc/robot/enums/TargetType.java new file mode 100644 index 0000000..fe04628 --- /dev/null +++ b/src/main/java/frc/robot/enums/TargetType.java @@ -0,0 +1,16 @@ +package frc.robot.enums; + +public enum TargetType { + kHighTarget(35.962), + kLowTarget(28.337); + + double heightInches; + + public double getHeightInches() { + return heightInches; + } + + private TargetType(double heightInches) { + this.heightInches = heightInches; + } +} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index a83f740..faec2b5 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -28,10 +28,10 @@ public enum Gear { , RobotMap.driveShiftHighChannelID); public Drivetrain(){ - leftLeader.setInverted(true); - leftFollower.setInverted(true); - rightLeader.setInverted(false); - rightFollower.setInverted(false); + leftLeader.setInverted(false); + leftFollower.setInverted(false); + rightLeader.setInverted(true); + rightFollower.setInverted(true); leftFollower.follow(leftLeader); rightFollower.follow(rightLeader); diff --git a/src/main/java/frc/robot/subsystems/DrivetrainGyro.java b/src/main/java/frc/robot/subsystems/DrivetrainGyro.java index 197ebec..fa55cea 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainGyro.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainGyro.java @@ -11,6 +11,8 @@ import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.PIDSourceType; +import frc.robot.Robot; +import frc.robot.RobotMap; public class DrivetrainGyro { @@ -19,7 +21,7 @@ public class DrivetrainGyro { private static final int PITCH_INDEX = 1; private static final int ROLL_INDEX = 2; - private final PigeonIMU pigeon = new PigeonIMU(0); + private final PigeonIMU pigeon = new PigeonIMU(Robot.speedControllerMap.getTalonByID(RobotMap.driveLeftFollowerSparkID)); public double getYaw(){ double[] returnArray = new double[3]; diff --git a/src/main/java/frc/robot/utils/Targets.java b/src/main/java/frc/robot/utils/Targets.java deleted file mode 100644 index 5935eb0..0000000 --- a/src/main/java/frc/robot/utils/Targets.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.robot.utils; - -import com._2train395.limelight.api.Target; - -public class Targets { - // All constants are in either inches or radians. - private static final double TAPE_LENGTH = 5.5; - private static final double TAPE_WIDTH = 2.0; - private static final double TAPE_ANGLE = Math.toRadians(14.5); - private static final double TAPE_SEPARATION = 8.0; - private static final double TARGET_LENGTH = (2.0 * (TAPE_WIDTH * Math.cos(TAPE_ANGLE)) - + (TAPE_LENGTH * Math.sin(TAPE_ANGLE))) + TAPE_SEPARATION; - private static final double TARGET_WIDTH = (TAPE_LENGTH * Math.cos(TAPE_ANGLE)) - + (TAPE_WIDTH * Math.sin(TAPE_ANGLE)); - - /** - * @param target - * @param targetType - * @param cameraHeight the height in inches of the center of the camera from the - * ground - * @param cameraAngle the angle at which the camera is mounted, in degrees, - * from the horizontal - * @return the distance in inches to the target, parallel to the ground - */ - public static double getDistance(final Target target, final TargetType targetType, final double cameraHeight, - double cameraAngle) { - final double yOffset = Math.toRadians(target.getYOffset()); - final double targetHeight = targetType.getHeight(); - cameraAngle = Math.toRadians(cameraAngle); - return (targetHeight - cameraHeight) / Math.tan(cameraAngle + yOffset); - } - - public enum TargetType { - LOADING_STATION_HATCH(31.5 - (TARGET_WIDTH / 2)), - CARGO_SHIP_HATCH(LOADING_STATION_HATCH.getHeight()), - ROCKET_HATCH(LOADING_STATION_HATCH.getHeight()), - ROCKET_PORT(39.125 - (TARGET_WIDTH / 2)); - - private final double height; - - TargetType(final double height) { - this.height = height; - } - - /** - * @return the height of the center of this target from the ground, in inches - */ - public double getHeight() { - return height; - } - } -} diff --git a/src/main/java/frc/robot/utils/limelight/Contour.java b/src/main/java/frc/robot/utils/limelight/Contour.java index ba4845d..046c6ba 100644 --- a/src/main/java/frc/robot/utils/limelight/Contour.java +++ b/src/main/java/frc/robot/utils/limelight/Contour.java @@ -2,21 +2,21 @@ public class Contour { //Horizontal offset from -27 to 27 degrees - public final double tx; + public final double xOffset; //Vertical offset from -20.5 to 20.5 degrees - public final double ty; + public final double yOffset; //Area as a percent of total image - public final double ta; + public final double percentArea; //Skew of target from -90 to 0 degrees - public final double ts; + public final double skewAngle; //Latency of the pipeline in ms - public final double tl; + public final double pipelineLatency; - public Contour(double tx, double ty, double ta, double ts, double tl){ - this.tx = tx; - this.ty = ty; - this.ta = ta; - this.ts = ts; - this.tl = tl; + public Contour(double xOffset, double yOffset, double percentArea, double skewAngle, double pipelineLatency){ + this.xOffset = xOffset; + this.yOffset = yOffset; + this.percentArea = percentArea; + this.skewAngle = skewAngle; + this.pipelineLatency = pipelineLatency; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/limelight/Corners.java b/src/main/java/frc/robot/utils/limelight/Corners.java index 2d7a347..669edb9 100644 --- a/src/main/java/frc/robot/utils/limelight/Corners.java +++ b/src/main/java/frc/robot/utils/limelight/Corners.java @@ -3,7 +3,6 @@ import java.util.ArrayList; import java.util.Collections; -import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; /** @@ -11,31 +10,39 @@ */ public class Corners { - MatOfPoint2f corners; + public final boolean validCorners; + public final Point topLeft; + public final Point topRight; + public final Point bottomLeft; + public final Point bottomRight; public Corners(double[] xCorners, double[] yCorners) { - if(xCorners == null || yCorners == null) { - corners = null; + if((xCorners == null || xCorners.length != 8) || (yCorners == null || yCorners.length != 8)) { + validCorners = false; + topLeft = null; + topRight = null; + bottomLeft = null; + bottomRight = null; return; } + validCorners = true; ArrayList temp = new ArrayList<>(); - for(int i = 0; i < 4; i++) { + for(int i = 0; i < 8; i++) { temp.add(i, new Point(xCorners[i], yCorners[i])); } - corners = new MatOfPoint2f(Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x - pt1.y, -pt2.x - pt2.y)), - Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x + pt1.y, -pt2.x + pt2.y)), - Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x + pt1.y, pt2.x + pt2.y)), - Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x - pt1.y, pt2.x - pt2.y))); + /** + * This block maximizes certain functions (x+y, x-y, -x+y, -x-y) over the temporary list of points. + * Each function corresponds to the location in the image the point lies in, i.e. the point with the greatest + * x+y is going to be the bottom-rightest point in the list. Note, the coordinate system of the limelight is + * origined in the top-left, with an inverted y-axis. + */ + + topLeft = Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x - pt1.y, -pt2.x - pt2.y)); + bottomLeft = Collections.max(temp, (Point pt1, Point pt2)->Double.compare(-pt1.x + pt1.y, -pt2.x + pt2.y)); + bottomRight = Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x + pt1.y, pt2.x + pt2.y)); + topRight = Collections.max(temp, (Point pt1, Point pt2)->Double.compare( pt1.x - pt1.y, pt2.x - pt2.y)); } - - /** - * @return an array containing corners counterclockwise from the top left - */ - public MatOfPoint2f getCorners() { - return corners; - } - } diff --git a/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java b/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java new file mode 100644 index 0000000..33e86ac --- /dev/null +++ b/src/main/java/frc/robot/utils/limelight/HeadingOffsetCalculator.java @@ -0,0 +1,72 @@ +package frc.robot.utils.limelight; + +import frc.robot.enums.TargetType; +/** + * This class is responsible for calculating the heading offset used by + * AimAtOffset and DriveToTarget + */ +public class HeadingOffsetCalculator { + static final double cameraHeightInches = 8.375; + static final double cameraAngle = 30; + + + /** + * maxOffset: The maximum angle offset an approach will target. + * maxOffsetDistance: The distance at which we will use the maximum offset. Below this distance we scale offset down linearly + */ + static final double maxOffset = 12.5; + static final double maxOffsetDistance = 6.0; + + public static Side getSide(Corners corners) { + corners = Limelight.getContourCorners(); + + if(!corners.validCorners) { + return null; + } + + return corners.topRight.y > corners.topLeft.y ? Side.kRight : Side.kLeft; + } + + public static double calculateDistance(Contour contour, TargetType targetType) { + if(contour == null) { + throw new IllegalArgumentException("Contour passed in was null"); + } + + return (targetType.getHeightInches() - cameraHeightInches) / + Math.tan(Math.toRadians(cameraAngle + contour.yOffset)) / 12; + } + + /** + * Calculates the distance to turn past center to hit the offset point. + * A positive angle denotes turning in a positive direction. + */ + public static double calculateAdditionalOffset(Contour contour, Corners corners, TargetType targetType) { + if(contour == null) { + throw new IllegalArgumentException("Contour passed in was null"); + } + double distance = calculateDistance(contour, targetType); + Side side = getSide(corners); + + double additionalOffsetSign = side == Side.kLeft ? 1 : -1; + double distanceScaling = Math.min(1, (distance * distance) / (maxOffsetDistance * maxOffsetDistance)); + + return maxOffset * distanceScaling * additionalOffsetSign; + } + + /** + * Calculates the distance to turn to hit the offset point. + * A positive angle denotes turning in a positive direction. + */ + public static double calculateTotalOffset(Contour contour, Corners corners, TargetType targetType) { + return -contour.xOffset + calculateAdditionalOffset(contour, corners, targetType); + } + + private HeadingOffsetCalculator() { + + } + + private enum Side { + kLeft, + kRight + }; +} diff --git a/src/main/java/frc/robot/utils/limelight/Limelight.java b/src/main/java/frc/robot/utils/limelight/Limelight.java index 8220405..f97f116 100644 --- a/src/main/java/frc/robot/utils/limelight/Limelight.java +++ b/src/main/java/frc/robot/utils/limelight/Limelight.java @@ -47,10 +47,10 @@ public static Contour getBestContour() { //Check if a valid contour is found if(limelightTable.getEntry("tv").getNumber(0).equals(1.0)) { return new Contour(limelightTable.getEntry("tx").getDouble(0), - limelightTable.getEntry("ty").getDouble(0), - limelightTable.getEntry("ta").getDouble(0), - limelightTable.getEntry("ts").getDouble(0), - limelightTable.getEntry("tl").getDouble(0)); + limelightTable.getEntry("ty").getDouble(0), + limelightTable.getEntry("ta").getDouble(0), + limelightTable.getEntry("ts").getDouble(0), + limelightTable.getEntry("tl").getDouble(0)); } else { return null; }