diff --git a/ThePinkAlliance.json b/ThePinkAlliance.json index bb958c3..8d4a7ce 100644 --- a/ThePinkAlliance.json +++ b/ThePinkAlliance.json @@ -1,7 +1,7 @@ { "fileName": "ThePinkAlliance.json", "name": "ThePinkAlliance", - "version": "2.0.14", + "version": "2.0.15", "uuid": "9619F7EA-7F96-4236-9D94-02338DFED572", "mavenUrls": [ "https://jitpack.io", @@ -13,7 +13,7 @@ { "groupId": "com.github.ThePinkAlliance", "artifactId": "core", - "version": "2.0.14" + "version": "2.0.15" } ], "jniDependencies": [], diff --git a/build.gradle b/build.gradle index c5f2e47..ae3fa59 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ apply plugin: 'java-library' apply plugin: 'maven-publish' group = 'com.ThePinkAlliance.core' -version = '2.0.14' +version = '2.0.15' sourceCompatibility = JavaVersion.VERSION_11 // java 11 targetCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/com/ThePinkAlliance/core/joystick/Joystick.java b/src/main/java/com/ThePinkAlliance/core/joystick/Joystick.java index 29f5710..28f088b 100644 --- a/src/main/java/com/ThePinkAlliance/core/joystick/Joystick.java +++ b/src/main/java/com/ThePinkAlliance/core/joystick/Joystick.java @@ -27,7 +27,7 @@ public enum Buttons { } } - public enum POV_TYPE { + public enum PovType { NORTH(0), EAST(90), WEST(270), @@ -35,7 +35,7 @@ public enum POV_TYPE { int id = 0; - POV_TYPE(int id) { + PovType(int id) { this.id = id; } } @@ -69,7 +69,7 @@ public JoystickAxis getAxis(Axis axis) { return new JoystickAxis(this, axis); } - public boolean povActivated(POV_TYPE type) { + public boolean povActivated(PovType type) { int pov = this.joy.getPOV(); if (pov == type.id) { diff --git a/src/main/java/com/ThePinkAlliance/core/limelight/Limelight.java b/src/main/java/com/ThePinkAlliance/core/limelight/Limelight.java index 72b19b2..972488d 100644 --- a/src/main/java/com/ThePinkAlliance/core/limelight/Limelight.java +++ b/src/main/java/com/ThePinkAlliance/core/limelight/Limelight.java @@ -52,7 +52,8 @@ public double get() { /** * @param height_from_floor The limelight lens height from the floor in inches. - * @param mounted_angle The current pitch angle of the limelight on its mount. + * @param mounted_angle The current pitch angle of the limelight on its + * mount. */ public Limelight(double height_from_floor, double mounted_angle) { this.HEIGHT_FROM_FLOOR = height_from_floor; @@ -65,14 +66,15 @@ public Limelight(double height_from_floor, double mounted_angle) { /** * @param height_from_floor The limelight lens height from the floor in inches. - * @param mounted_angle The current pitch angle of the limelight on its mount. - * @param horizontal_offset The angluar offset of the limelight from the center of the robot. + * @param mounted_angle The current pitch angle of the limelight on its + * mount. + * @param horizontal_offset The angluar offset of the limelight from the center + * of the robot. */ public Limelight( - double height_from_floor, - double mounted_angle, - double horizontal_offset - ) { + double height_from_floor, + double mounted_angle, + double horizontal_offset) { this.HEIGHT_FROM_FLOOR = height_from_floor; this.MOUNTED_ANGLE = mounted_angle; this.REFLECTED_TAPE_HEIGHT = GAME_TARGET_HEIGHTS.RAPID_REACT_TOP_HUB.get(); @@ -81,6 +83,18 @@ public Limelight( configureLimelight(); } + /** + * @param constants Constants for the limelight. + */ + public Limelight(LimelightConstants constants) { + this.HEIGHT_FROM_FLOOR = constants.getHeightFromFloor(); + this.MOUNTED_ANGLE = constants.getMountedAngle(); + this.REFLECTED_TAPE_HEIGHT = constants.getTargetHeight(); + this.HORIZONTAL_OFFSET = constants.getHorizontalOffset(); + + configureLimelight(); + } + private void configureLimelight() { this.CURRENT_LED_MODE = LED_MODE.OFF; @@ -105,6 +119,20 @@ public void configureTargetHeight(double targetHeight) { this.REFLECTED_TAPE_HEIGHT = targetHeight; } + /** + * This will reconfigure the limelight with the passed parameters. + * + * @param constants LimelightConstants + */ + public void reconfigureLimelight(LimelightConstants constants) { + this.HEIGHT_FROM_FLOOR = constants.getHeightFromFloor(); + this.MOUNTED_ANGLE = constants.getMountedAngle(); + this.REFLECTED_TAPE_HEIGHT = constants.getTargetHeight(); + this.HORIZONTAL_OFFSET = constants.getHorizontalOffset(); + + configureLimelight(); + } + /** * * @param targetHeight The height of the reflective tape in inches. @@ -135,18 +163,24 @@ public double foundTargets() { /** * Returns the horizontal difference from the center of the detected target. + * + * @return The offset in degress, range of (27 <-> -27) */ public double getHorizontalDiff() { return this.tx.getDouble(0); } + /** + * In order to get accurate target info you need to run the limelight's onboard + * cross-hair calibration. + * + * @return The estimated distance in inches. + */ public double calculateDistance() { double verticalOffset = ty.getDouble(0); double targetAngleDeg = MOUNTED_ANGLE + verticalOffset; - return ( - (REFLECTED_TAPE_HEIGHT - HEIGHT_FROM_FLOOR) / - Math.tan(Math.toRadians(targetAngleDeg)) - ); + return ((REFLECTED_TAPE_HEIGHT - HEIGHT_FROM_FLOOR) / + Math.tan(Math.toRadians(targetAngleDeg))); } } diff --git a/src/main/java/com/ThePinkAlliance/core/limelight/LimelightConstants.java b/src/main/java/com/ThePinkAlliance/core/limelight/LimelightConstants.java index 8d92f71..d62fd3f 100644 --- a/src/main/java/com/ThePinkAlliance/core/limelight/LimelightConstants.java +++ b/src/main/java/com/ThePinkAlliance/core/limelight/LimelightConstants.java @@ -1,3 +1,98 @@ package com.ThePinkAlliance.core.limelight; -public class LimelightConstants {} +import com.ThePinkAlliance.core.limelight.Limelight.GAME_TARGET_HEIGHTS; + +/** + * This store's information about the limelight and it's target. + */ +public class LimelightConstants { + private double HEIGHT_FROM_FLOOR; + private double MOUNTED_ANGLE; + private double REFLECTED_TAPE_HEIGHT; + private double HORIZONTAL_OFFSET; + + /** + * @param HEIGHT_FROM_FLOOR The limelight lens height from the floor in + * inches. + * @param MOUNTED_ANGLE The current pitch angle of the limelight on its + * mount. + * @param REFLECTED_TAPE_HEIGHT The height of the reflective tape we are + * tracking. + */ + public LimelightConstants(double HEIGHT_FROM_FLOOR, double MOUNTED_ANGLE, double REFLECTED_TAPE_HEIGHT) { + this.HEIGHT_FROM_FLOOR = HEIGHT_FROM_FLOOR; + this.MOUNTED_ANGLE = MOUNTED_ANGLE; + this.REFLECTED_TAPE_HEIGHT = REFLECTED_TAPE_HEIGHT; + this.HORIZONTAL_OFFSET = 0; + } + + /** + * @param HEIGHT_FROM_FLOOR The limelight lens height from the floor in + * inches. + * @param MOUNTED_ANGLE The current pitch angle of the limelight on its + * mount. + * @param REFLECTED_TAPE_HEIGHT The height of the reflective tape we are + * tracking. + * @param HORIZONTAL_OFFSET The angluar offset of the limelight from the + * center + * of the robot. + */ + public LimelightConstants(double HEIGHT_FROM_FLOOR, double MOUNTED_ANGLE, double REFLECTED_TAPE_HEIGHT, + double HORIZONTAL_OFFSET) { + this.HEIGHT_FROM_FLOOR = HEIGHT_FROM_FLOOR; + this.MOUNTED_ANGLE = MOUNTED_ANGLE; + this.HORIZONTAL_OFFSET = HORIZONTAL_OFFSET; + this.REFLECTED_TAPE_HEIGHT = REFLECTED_TAPE_HEIGHT; + } + + /** + * @param HEIGHT_FROM_FLOOR The limelight lens height from the floor in + * inches. + * @param MOUNTED_ANGLE The current pitch angle of the limelight on its + * mount. + * @param TARGET The reflective tape target we are + * tracking. + */ + public LimelightConstants(double HEIGHT_FROM_FLOOR, double MOUNTED_ANGLE, GAME_TARGET_HEIGHTS TARGET) { + this.HEIGHT_FROM_FLOOR = HEIGHT_FROM_FLOOR; + this.MOUNTED_ANGLE = MOUNTED_ANGLE; + this.HORIZONTAL_OFFSET = 0; + this.REFLECTED_TAPE_HEIGHT = TARGET.get(); + } + + /** + * @param HEIGHT_FROM_FLOOR The limelight lens height from the floor in + * inches. + * @param MOUNTED_ANGLE The current pitch angle of the limelight on its + * mount. + * @param TARGET The reflective tape target we are + * tracking. + * @param HORIZONTAL_OFFSET The angluar offset of the + * limelight from the + * center + * of the robot. + */ + public LimelightConstants(double HEIGHT_FROM_FLOOR, double MOUNTED_ANGLE, GAME_TARGET_HEIGHTS TARGET, + double HORIZONTAL_OFFSET) { + this.HEIGHT_FROM_FLOOR = HEIGHT_FROM_FLOOR; + this.MOUNTED_ANGLE = MOUNTED_ANGLE; + this.HORIZONTAL_OFFSET = HORIZONTAL_OFFSET; + this.REFLECTED_TAPE_HEIGHT = TARGET.get(); + } + + public double getHeightFromFloor() { + return this.HEIGHT_FROM_FLOOR; + } + + public double getMountedAngle() { + return this.MOUNTED_ANGLE; + } + + public double getTargetHeight() { + return this.REFLECTED_TAPE_HEIGHT; + } + + public double getHorizontalOffset() { + return this.HORIZONTAL_OFFSET; + } +}