diff --git a/src/main/java/com/team4099/utils/LimelightHelpers.java b/src/main/java/com/team4099/utils/LimelightHelpers.java index 0cb199a3..9ef9ec52 100644 --- a/src/main/java/com/team4099/utils/LimelightHelpers.java +++ b/src/main/java/com/team4099/utils/LimelightHelpers.java @@ -1,4 +1,5 @@ -package com.team4099.utils;//com.team4099.utils.LimelightHelpers v1.2.1 (March 1, 2023) +package com.team4099.utils;//com.team4099.utils.LimelightHelpers +//LimelightHelpers v1.5.0 (March 27, 2024) import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; @@ -301,6 +302,18 @@ public static class Results { @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + @JsonProperty("t6c_rs") public double[] camerapose_robotspace; @@ -361,8 +374,59 @@ public static class LimelightResults { @JsonProperty("Results") public Results targetingResults; + public String error; + public LimelightResults() { targetingResults = new Results(); + error = ""; + } + + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -383,7 +447,7 @@ static final String sanitizeName(String name) { private static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 3D Pose Data!"); + //System.err.println("Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( @@ -395,7 +459,7 @@ private static Pose3d toPose3D(double[] inData){ private static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + //System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -403,6 +467,86 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } + private static double extractBotPoseEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray,6); + int tagCount = (int)extractBotPoseEntry(poseArray,7); + double tagSpan = extractBotPoseEntry(poseArray,8); + double tagDist = extractBotPoseEntry(poseArray,9); + double tagArea = extractBotPoseEntry(poseArray,10); + //getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); + + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial*tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } + else{ + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } @@ -542,8 +686,8 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); } ///// @@ -602,6 +746,28 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { return toPose2D(result); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -616,6 +782,26 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -641,6 +827,11 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + /** * The LEDs will be controlled by Limelight pipeline settings, and not by robot * code. @@ -694,6 +885,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; @@ -763,7 +976,7 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime(); @@ -775,5 +988,4 @@ public static LimelightResults getLatestResults(String limelightName) { return results; } -} - +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 9c96230a..a4b43c40 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -15,7 +15,7 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 import com.team4099.robot2023.subsystems.elevator.Elevator -import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO +import com.team4099.robot2023.subsystems.elevator.ElevatorIO import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.feeder.Feeder import com.team4099.robot2023.subsystems.feeder.FeederIONeo @@ -28,6 +28,7 @@ import com.team4099.robot2023.subsystems.intake.IntakeIOFalconNEO import com.team4099.robot2023.subsystems.intake.IntakeIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO +import com.team4099.robot2023.subsystems.limelight.LimelightVisionIOReal import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision @@ -46,6 +47,7 @@ import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest +import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO object RobotContainer { private val drivetrain: Drivetrain @@ -77,7 +79,7 @@ object RobotContainer { drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal) vision = Vision(object : CameraIO {}, CameraIOPhotonvision("parakeet_2")) - limelight = LimelightVision(object : LimelightVisionIO {}) + limelight = LimelightVision(LimelightVisionIOReal) intake = Intake(IntakeIOFalconNEO) feeder = Feeder(FeederIONeo) elevator = Elevator(ElevatorIONEO) @@ -95,7 +97,8 @@ object RobotContainer { wrist = Wrist(WristIOSim) } - superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel, drivetrain, vision) + superstructure = + Superstructure(intake, feeder, elevator, wrist, flywheel, drivetrain, vision, limelight) vision.setDataInterfaces( { drivetrain.fieldTRobot }, { drivetrain.addVisionData(it) }, @@ -161,6 +164,7 @@ object RobotContainer { } fun mapTeleopControls() { + limelight.limelightState = LimelightVision.LimelightStates.TARGETING_GAME_PIECE ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain)) ControlBoard.intake.whileTrue( diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt index 4dadbafc..b22ed663 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.controller.PIDController import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees @@ -22,8 +23,10 @@ import org.team4099.lib.units.derived.perDegreePerSecond import org.team4099.lib.units.derived.perDegreeSeconds import org.team4099.lib.units.derived.radians import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.perSecond import kotlin.math.PI +import kotlin.math.hypot class TargetNoteCommand( val driver: DriverProfile, @@ -98,9 +101,12 @@ class TargetNoteCommand( override fun initialize() { thetaPID.reset() + /* if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) { thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get()) } + + */ } override fun execute() { @@ -112,10 +118,16 @@ class TargetNoteCommand( DebugLogger.recordDebugOutput("NoteAlignment/error", thetaPID.error.inDegrees) DebugLogger.recordDebugOutput("NoteAlignment/thetaFeedback", thetaFeedback.inDegreesPerSecond) + val driveVector = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( thetaFeedback, - driver.driveSpeedClampedSupplier(driveX, driveY, slowMode), + Pair( + -hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond) + .meters + .perSecond, + 0.0.meters.perSecond + ), fieldOriented = false ) } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 41089e90..bad9aaaa 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -53,7 +53,6 @@ object Constants { } object Tuning { - const val TUNING_MODE = false const val DEBUGING_MODE = false const val SIMULATE_DRIFT = false diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 41ec3a85..f455d768 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -128,10 +128,10 @@ object DrivetrainConstants { } } - val LIMELIGHT_THETA_KP = 0.0.degrees.perSecond / 1.degrees + val LIMELIGHT_THETA_KP = 4.0.degrees.perSecond / 1.degrees val LIMELIGHT_THETA_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val LIMELIGHT_THETA_KD = - (0.0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (0.1.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond val AUTO_THETA_ALLOWED_ERROR = 3.degrees val AUTO_THETA_PID_KP = (1.05.radians.perSecond / 1.radians) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index 0682b554..47b13607 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -4,6 +4,7 @@ import org.team4099.lib.geometry.Transform2d import org.team4099.lib.geometry.Translation2d import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.volts @@ -17,7 +18,11 @@ object IntakeConstants { val INTAKE_TRANSFORM = Transform2d(Translation2d(-18.0.inches, 0.0.inches), 0.0.degrees) // TODO: Change gear ratio according to robot - val ROLLER_CURRENT_LIMIT = 80.0.amps + val ROLLER_CURRENT_LIMIT = 80.amps + val ROLLER_SUPPLY_CURRENT_LIMIT = 120.0.amps + val ROLLER_STATOR_CURRENT_LIMIT = 200.0.amps + val ROLLER_CURRENT_TIME_THRESHOLD = 1.5.seconds + val ROLLER_SUPPLY_TRIGGER_THRESHOLD = 65.amps const val ROLLER_MOTOR_INVERTED = true const val ROLLER_GEAR_RATIO = 24.0 / 12.0 // this one has been updated const val CENTER_WHEEL_GEAR_RATIO = 34.0 / 14.0 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt index 4d135228..aafe3014 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt @@ -1,29 +1,45 @@ package com.team4099.robot2023.config.constants import com.ctre.phoenix.led.Animation +import com.ctre.phoenix.led.StrobeAnimation import org.team4099.lib.units.base.amps import org.team4099.lib.units.derived.volts object LEDConstants { val INTAKE_CURRENT_THRESHOLD = 15.amps val OUTAKE_CURRENT_THRESHOLD = 20.amps + val BATTERY_FULL_THRESHOLD = 12.5.volts val LED_COUNT = 50 - val BATTERY_WARNING_THRESHOLD = 12.3.volts - - enum class CandleState(val animation: Animation?, val r: Int, val g: Int, val b: Int) { + enum class CandleState( + val animation: Animation?, + val r: Int, + val g: Int, + val b: Int, + var attachedValue: Any? = null + ) { // Gold NO_NOTE(null, 0, 0, 0), NOTHING(null, 0, 0, 0), + GOLD(null, 255, 105, 0), RED(null, 255, 0, 0), + LIGHT_RED(null, 255, 67, 36), BLUE(null, 0, 0, 255), + PURPLE(null, 67, 36, 255), + GREEN(null, 0, 255, 0), + MAGENTA(null, 255, 0, 255), // Blue HAS_NOTE(null, 0, 0, 255), - - // Red - LOW_BATTERY(null, 255, 105, 0), + NO_TAG(null, 255, 0, 0), + SEES_TAG(null, 255, 105, 0), + SEES_NOTE(null, 255, 87, 51), + + // Yellow + BATTERY_DISPLAY(null, 255, 105, 0), + LOW_BATTERY_WARNING(StrobeAnimation(67, 36, 255), 0, 0, 0), + WHITE(null, 255, 255, 255), // Green diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt index 2903f1aa..93715b3d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt @@ -77,15 +77,15 @@ object VisionConstants { } object Limelight { - val LIMELIGHT_NAME = "limelight-zapdos" + val LIMELIGHT_NAME = "limelight-owl" val HORIZONTAL_FOV = 59.6.degrees val VERITCAL_FOV = 45.7.degrees val HIGH_TAPE_HEIGHT = 43.875.inches + 1.inches val MID_TAPE_HEIGHT = 23.905.inches + 1.inches val LL_TRANSFORM = Transform3d( - Translation3d(1.1438.inches, 10.3966.inches, 12.9284.inches), - Rotation3d(8.159.degrees, -90.degrees + 61.610.degrees, -14.1254.degrees) + Translation3d(-14.655.inches, 0.inches, 23.316.inches), + Rotation3d(0.degrees, 143.degrees, 180.degrees) ) const val RES_WIDTH = 320 const val RES_HEIGHT = 240 // no clue what these numbers should be but usnig these for now diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index 8a193c77..37e35967 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -31,7 +31,7 @@ object WristConstants { val ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO = 1.06488 / 1.0 val MOTOR_TO_ABSOLUTE_ENCODER_GEAR_RATIO = - 5.0 / 1.0 * 4.0 / 1.0 * 3.0 / 1.0 * 46.0 / 42.0 * 90.0 / 33.0 * 1.0 / 1.06488 + 5.0 / 1.0 * 4.0 / 1.0 * 54.0 / 34.0 * 90.0 / 33.0 * 1.0 / 1.06488 val VOLTAGE_COMPENSATION = 12.0.volts val ABSOLUTE_ENCODER_OFFSET = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt index a5e20665..28013a0a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt @@ -37,7 +37,7 @@ interface ElevatorIO { var leaderTempCelcius = 0.0.celsius var followerTempCelcius = 0.0.celsius - var isSimulating = false + var isSimulating = true override fun toLog(table: LogTable) { table?.put("elevatorPositionInches", elevatorPosition.inInches) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt index 50c171c3..3bf87ce7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt @@ -56,11 +56,15 @@ object IntakeIOFalconNEO : IntakeIO { var rollerFalconConfiguration = TalonFXConfiguration() rollerFalconConfiguration.CurrentLimits.StatorCurrentLimit = - IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes - rollerFalconConfiguration.CurrentLimits.StatorCurrentLimit = - IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes + IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes + rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimit = + IntakeConstants.ROLLER_SUPPLY_CURRENT_LIMIT.inAmperes + // rollerFalconConfiguration.CurrentLimits.SupplyTimeThreshold = + // IntakeConstants.ROLLER_CURRENT_TIME_THRESHOLD.inSeconds + // rollerFalconConfiguration.CurrentLimits.SupplyCurrentThreshold = + // IntakeConstants.ROLLER_SUPPLY_TRIGGER_THRESHOLD.inAmperes rollerFalconConfiguration.CurrentLimits.StatorCurrentLimitEnable = true - rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimitEnable = false + rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true rollerFalconConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive rollerFalconConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast @@ -76,7 +80,7 @@ object IntakeIOFalconNEO : IntakeIO { "Roller", MotorCollection( mutableListOf(Falcon500(rollerFalcon, "Roller Motor")), - IntakeConstants.ROLLER_CURRENT_LIMIT, + IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT, 60.celsius, 50.amps, 120.celsius @@ -87,7 +91,7 @@ object IntakeIOFalconNEO : IntakeIO { centerWheelSparkMax.clearFaults() centerWheelSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts) - centerWheelSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt()) + // centerWheelSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt()) centerWheelSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED centerWheelSparkMax.idleMode = CANSparkMax.IdleMode.kCoast @@ -99,7 +103,7 @@ object IntakeIOFalconNEO : IntakeIO { "Center Wheel", MotorCollection( mutableListOf(Neo(centerWheelSparkMax, "Center Wheel Motor")), - IntakeConstants.ROLLER_CURRENT_LIMIT, + IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT, 60.celsius, 50.amps, 120.celsius diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt index 26ac3c09..0ede25f5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt @@ -3,8 +3,11 @@ package com.team4099.robot2023.subsystems.led import com.team4099.robot2023.config.constants.LEDConstants import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.units.derived.ElectricalPotential interface LedIO { + var batteryVoltage: ElectricalPotential + class LedIOInputs : LoggableInputs { var ledState = LEDConstants.CandleState.NO_NOTE.toString() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt index e14e188b..58b1808d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt @@ -4,11 +4,20 @@ import com.ctre.phoenix.led.CANdle import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.LEDConstants import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import kotlin.math.absoluteValue object LedIOCandle : LedIO { private val ledController = CANdle(Constants.LED.LED_CANDLE_ID) private var lastState: LEDConstants.CandleState = LEDConstants.CandleState.NO_NOTE + private var waveRuns = 0 + private var loopCycles = 0 + private var reverseLEDS = false + private var finishedFade = false + override var batteryVoltage: ElectricalPotential = 12.0.volts override fun updateInputs(inputs: LedIO.LedIOInputs) { inputs.ledState = lastState.name @@ -20,8 +29,224 @@ object LedIOCandle : LedIO { setCANdleState(newState) } + private fun wave( + state: LEDConstants.CandleState, + defaultState: LEDConstants.CandleState, + lengthOfWave: Int = 10, + lengthOfGradient: Double = 5.0, + ) { + ledController.setLEDs(state.r, state.g, state.b, 0, waveRuns, lengthOfWave) + + // Set outer colors + ledController.setLEDs( + defaultState.r, defaultState.g, defaultState.b, 0, 0, waveRuns - lengthOfGradient.toInt() + ) + ledController.setLEDs( + defaultState.r, + defaultState.g, + defaultState.b, + 0, + waveRuns + lengthOfWave + lengthOfGradient.toInt(), + LEDConstants.LED_COUNT - waveRuns - lengthOfWave + ) + + // Set gradient colors + if (lengthOfGradient > 0.0) { + for ((step, ledIdx) in ((waveRuns - lengthOfGradient.toInt())..waveRuns).withIndex()) { + ledController.setLEDs( + (defaultState.r + (state.r - defaultState.r) * ((step + 1) / lengthOfGradient)).toInt(), + (defaultState.g + (state.g - defaultState.g) * ((step + 1) / lengthOfGradient)).toInt(), + (defaultState.b + (state.b - defaultState.b) * ((step + 1) / lengthOfGradient)).toInt(), + 0, + ledIdx, + 1 + ) + } + + for ((step, ledIdx) in (waveRuns..(waveRuns + lengthOfGradient).toInt()).withIndex()) { + ledController.setLEDs( + ( + defaultState.r + + (state.r - defaultState.r) * + ((lengthOfGradient - (step + 1)) / lengthOfGradient) + ) + .toInt(), + ( + defaultState.g + + (state.g - defaultState.g) * + ((lengthOfGradient - (step + 1)) / lengthOfGradient) + ) + .toInt(), + ( + defaultState.b + + (state.b - defaultState.b) * + ((lengthOfGradient - (step + 1)) / lengthOfGradient) + ) + .toInt(), + 0, + ledIdx, + 1 + ) + } + } + + if (waveRuns >= LEDConstants.LED_COUNT - lengthOfWave) { + reverseLEDS = true + } else if (waveRuns < lengthOfWave) { + reverseLEDS = false + } + + waveRuns += if (!reverseLEDS) 1 else -1 + } + + private fun gradient( + state: LEDConstants.CandleState, + otherState: LEDConstants.CandleState, + lengthOfSolidEnds: Int = 10 + ) { + ledController.setLEDs(state.r, state.g, state.b, 0, 0, lengthOfSolidEnds) + ledController.setLEDs( + otherState.r, + otherState.g, + otherState.b, + 0, + LEDConstants.LED_COUNT - lengthOfSolidEnds, + lengthOfSolidEnds + ) + + for ( + (step, ledIdx) in + (lengthOfSolidEnds until (LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex() + ) { + ledController.setLEDs( + ( + state.r + + (otherState.r - state.r) * + ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0)) + ) + .toInt(), + ( + state.g + + (otherState.g - state.g) * + ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0)) + ) + .toInt(), + ( + state.b + + (otherState.b - state.b) * + ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0)) + ) + .toInt(), + 0, + ledIdx, + 1 + ) + } + } + + private fun progressBar( + state: LEDConstants.CandleState, + defaultState: LEDConstants.CandleState, + percent: Double + ) { + ledController.setLEDs( + state.r, state.g, state.b, 0, 0, (percent * LEDConstants.LED_COUNT).toInt() + ) + ledController.setLEDs( + defaultState.r, + defaultState.g, + defaultState.b, + 0, + (percent * LEDConstants.LED_COUNT).toInt(), + LEDConstants.LED_COUNT + ) + } + + private fun fadeBetweenColors( + state: LEDConstants.CandleState, + otherState: LEDConstants.CandleState, + loopCyclesToConverge: Int = 10 + ) { + val stepUpInLoopCycles = 2 + var reachedStartColor = true + var reachedEndColor = true + + for ( + (ledIdx, numberOfLoopCycles) in + ( + loopCyclesToConverge..loopCyclesToConverge + + LEDConstants.LED_COUNT * stepUpInLoopCycles step stepUpInLoopCycles + ) + .withIndex() + ) { + val calculatedR = + ( + state.r + + (otherState.r - state.r) * + ( + if (loopCycles > numberOfLoopCycles) 1.0 + else (loopCycles / numberOfLoopCycles.toDouble()) + ) + ) + .toInt() + val calculatedG = + ( + state.g + + (otherState.g - state.g) * + ( + if (loopCycles > numberOfLoopCycles) 1.0 + else (loopCycles / numberOfLoopCycles.toDouble()) + ) + ) + .toInt() + val calculatedB = + ( + state.b + + (otherState.b - state.b) * + ( + if (loopCycles > numberOfLoopCycles) 1.0 + else (loopCycles / numberOfLoopCycles.toDouble()) + ) + ) + .toInt() + + reachedStartColor = + (calculatedR == state.r && calculatedG == state.g && calculatedB == state.b) + reachedEndColor = + ( + (calculatedR - otherState.r).absoluteValue < 10 && + (calculatedG - otherState.g).absoluteValue < 10 && + (calculatedB - otherState.b).absoluteValue < 10 + ) + + ledController.setLEDs(calculatedR, calculatedG, calculatedB, 0, ledIdx, 1) + } + + loopCycles += if (finishedFade) -1 else 1 + + if (reachedStartColor) { + finishedFade = false + } + + if (reachedEndColor) { // All LEDs are at the end color + finishedFade = true + } + } + private fun setCANdleState(state: LEDConstants.CandleState) { - if (state.animation == null) { + if (state == LEDConstants.CandleState.BATTERY_DISPLAY) { + progressBar( + LEDConstants.CandleState.BATTERY_DISPLAY, + LEDConstants.CandleState.NOTHING, + (batteryVoltage.inVolts - 11.5) / (LEDConstants.BATTERY_FULL_THRESHOLD.inVolts - 11.5) + ) + } else if (state == LEDConstants.CandleState.BLUE) { + ledController.clearAnimation(0) + fadeBetweenColors(state, LEDConstants.CandleState.MAGENTA, loopCyclesToConverge = 2) + } else if (state == LEDConstants.CandleState.RED) { + ledController.clearAnimation(0) + fadeBetweenColors(state, LEDConstants.CandleState.LIGHT_RED, loopCyclesToConverge = 2) + } else if (state.animation == null) { ledController.clearAnimation(0) ledController.setLEDs(state.r, state.g, state.b) } else { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt index 7dd0f7b9..254d2b3a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -3,15 +3,19 @@ package com.team4099.robot2023.subsystems.led import com.team4099.robot2023.config.constants.LEDConstants import com.team4099.robot2023.util.FMSData import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.RobotController import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.derived.volts class Leds(val io: LedIO) { var inputs = LedIO.LedIOInputs() var hasNote = false var subsystemsAtPosition = false - var isIdle = true - var batteryIsLow = false + var isAutoAiming = false + var isAmping = false + var seesGamePiece = false + var seesTag = true var state = LEDConstants.CandleState.NO_NOTE set(value) { @@ -21,24 +25,38 @@ class Leds(val io: LedIO) { fun periodic() { io.updateInputs(inputs) - if (batteryIsLow && DriverStation.isDisabled()) { - state = LEDConstants.CandleState.LOW_BATTERY - } else if (DriverStation.isDisabled()) { - if (DriverStation.getAlliance().isPresent) { - if (FMSData.isBlue) { - state = LEDConstants.CandleState.BLUE - } else { - state = LEDConstants.CandleState.RED - } + if (DriverStation.getAlliance().isEmpty) { + io.batteryVoltage = RobotController.getBatteryVoltage().volts + state = LEDConstants.CandleState.GOLD + if (io.batteryVoltage < 12.3.volts) { + state = LEDConstants.CandleState.LOW_BATTERY_WARNING } else { - state = LEDConstants.CandleState.NOTHING + state = LEDConstants.CandleState.GOLD } - } else if (hasNote) { - if (subsystemsAtPosition && !isIdle) { - state = LEDConstants.CandleState.CAN_SHOOT + } else if (DriverStation.isDisabled() && DriverStation.getAlliance().isPresent) { + if (FMSData.isBlue) { + state = LEDConstants.CandleState.BLUE } else { - state = LEDConstants.CandleState.HAS_NOTE + state = LEDConstants.CandleState.RED + } + } else if (hasNote) { + if (isAutoAiming) { + if (!seesTag) { + state = LEDConstants.CandleState.NO_TAG + } else if (!subsystemsAtPosition) { + state = LEDConstants.CandleState.SEES_TAG + } else { + state = LEDConstants.CandleState.CAN_SHOOT + } + } else if (isAmping) { + if (subsystemsAtPosition) { + state = LEDConstants.CandleState.CAN_SHOOT + } else { + state = LEDConstants.CandleState.HAS_NOTE + } } + } else if (seesGamePiece) { + state = LEDConstants.CandleState.SEES_NOTE } else { state = LEDConstants.CandleState.NO_NOTE } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt index 00d19361..6e9ac8c0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt @@ -63,6 +63,10 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { init {} override fun periodic() { + + visibleGamePieces.clear() + targetGamePieceTx = null + val startTime = Clock.realTimestamp io.updateInputs(inputs) Logger.processInputs("LimelightVision", inputs) @@ -89,6 +93,11 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { } } + Logger.recordOutput( + "Limelight/poseRelativeToRobot", + ((targetGamePiecePose?.toPose2d() ?: Pose2d()) - poseSupplier.invoke()).transform2d + ) + Logger.recordOutput( "LimelightVision/RawLimelightReadingsTx", inputs.gamePieceTargets.map { it.tx.inDegrees }.toDoubleArray() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index 0b60eca0..219f41ca 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -1,10 +1,8 @@ package com.team4099.robot2023.subsystems.superstructure import com.team4099.lib.hal.Clock -import com.team4099.robot2023.config.constants.FeederConstants import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.FlywheelConstants -import com.team4099.robot2023.config.constants.LEDConstants import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.elevator.Elevator @@ -13,12 +11,12 @@ import com.team4099.robot2023.subsystems.flywheel.Flywheel import com.team4099.robot2023.subsystems.intake.Intake import com.team4099.robot2023.subsystems.led.LedIOCandle import com.team4099.robot2023.subsystems.led.Leds +import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.util.NoteSimulation import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase -import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger @@ -35,7 +33,6 @@ import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees -import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inRotationsPerMinute @@ -48,7 +45,8 @@ class Superstructure( private val wrist: Wrist, private val flywheel: Flywheel, private val drivetrain: Drivetrain, - private val vision: Vision + private val vision: Vision, + private val limelight: LimelightVision ) : SubsystemBase() { var wristPushDownVoltage = Wrist.TunableWristStates @@ -140,11 +138,13 @@ class Superstructure( override fun periodic() { leds.hasNote = feeder.hasNote - leds.isIdle = currentState == SuperstructureStates.IDLE + leds.isAutoAiming = currentState == SuperstructureStates.AUTO_AIM + leds.isAmping = + (currentState == SuperstructureStates.WRIST_AMP_PREP) || + (currentState == SuperstructureStates.ELEVATOR_AMP_PREP) leds.subsystemsAtPosition = wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && elevator.isAtTargetedPosition - leds.batteryIsLow = - RobotController.getBatteryVoltage() < LEDConstants.BATTERY_WARNING_THRESHOLD.inVolts + leds.seesGamePiece = limelight.inputs.gamePieceTargets.size > 0 val ledLoopStartTime = Clock.realTimestamp leds.periodic() @@ -400,13 +400,7 @@ class Superstructure( ) if (feeder.hasNote || (!RobotBase.isReal() && noteHoldingID != -1)) { - if (DriverStation.isTeleop()) { - cleanupStartTime = Clock.fpgaTime - nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP - } else { - currentRequest = Request.SuperstructureRequest.Idle() - nextState = SuperstructureStates.IDLE - } + nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK } when (currentRequest) { is Request.SuperstructureRequest.Idle -> { @@ -423,13 +417,29 @@ class Superstructure( } } } - SuperstructureStates.GROUND_INTAKE_CLEAN_UP -> { - feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(-1.volts) + SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK -> { + feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(-1.0.volts) + if (!feeder.hasNote) { + nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD + } - if (Clock.fpgaTime - cleanupStartTime > FeederConstants.CLEAN_UP_TIME) { + when (currentRequest) { + is Request.SuperstructureRequest.Idle -> { + nextState = SuperstructureStates.IDLE + } + } + } + SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD -> { + feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(1.volts) + if (feeder.hasNote) { currentRequest = Request.SuperstructureRequest.Idle() nextState = SuperstructureStates.IDLE } + when (currentRequest) { + is Request.SuperstructureRequest.Idle -> { + nextState = SuperstructureStates.IDLE + } + } } SuperstructureStates.AUTO_AIM -> { val targetFlywheelSpeed = aimer.calculateFlywheelSpeed() @@ -1024,7 +1034,8 @@ class Superstructure( HOME, GROUND_INTAKE_PREP, GROUND_INTAKE, - GROUND_INTAKE_CLEAN_UP, + GROUND_INTAKE_CLEAN_UP_PUSH_BACK, + GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD, ELEVATOR_AMP_PREP, WRIST_AMP_PREP, SCORE_ELEVATOR_AMP, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index b98b9ea8..251217cd 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -111,7 +111,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { PhotonUtils.calculateDistanceToTargetMeters( cameraPoses[instance].translation.z.inMeters, 57.125.inches.inMeters, - 23.25.degrees.inRadians, + 22.42.degrees.inRadians, tag.pitch.degrees.inRadians ) .meters diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt index d8f031a0..4607448c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt @@ -37,7 +37,7 @@ interface WristIO { var wristAcceleration = 0.0.degrees.perSecond.perSecond - var isSimulated = false + var isSimulated = true override fun toLog(table: LogTable) { table.put("wristPosition", wristPosition.inDegrees)