From fa438f0c48fb8e396ca23fcdfe4d13c8c8218221 Mon Sep 17 00:00:00 2001 From: Danny Lin <75951607+dannynotsmart@users.noreply.github.com> Date: Sat, 11 May 2024 19:32:56 -0400 Subject: [PATCH] features --- README.md | 2 +- .../java/org/sciborgs1155/lib/FakePDH.java | 39 ---------------- .../org/sciborgs1155/lib/FaultLogger.java | 1 - .../java/org/sciborgs1155/robot/Robot.java | 2 - .../org/sciborgs1155/robot/vision/Vision.java | 4 +- .../robot/vision/VisionConstants.java | 45 ++++++++++--------- 6 files changed, 27 insertions(+), 66 deletions(-) delete mode 100644 src/main/java/org/sciborgs1155/lib/FakePDH.java diff --git a/README.md b/README.md index 3a7141d..78fff7d 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ Our robot code is centered around [Robot.java](src/main/java/org/sciborgs1155/ro This project currently contains drive, vision, and autos. You are expected to add/modify code supporting existing files and new subsystems when using this template. Some of these files include but are not limited to: - **[Autos.java](src/main/java/org/sciborgs1155/robot/commands/Autos.java)** Add code for new subsystems in `configureAutos`, such as commands for `NamedCommands` -- **[VisionConstants.java](src/main/java/org/sciborgs1155/robot/vision/VisionConstants.java)** Add new `CameraConfig` objects representing cameras on the robot in the field `CAMERAS`. Also modify any camera configurations or constants if needed. +- **[VisionConstants.java](src/main/java/org/sciborgs1155/robot/vision/VisionConstants.java)** Add new `CameraConfig` fields representing cameras on the robot, and change the `create` method in [Vision](src/main/java/org/sciborgs1155/robot/vision/Vision.java). Also modify any camera configurations or constants if needed. - **[Constants.java](src/main/java/org/sciborgs1155/robot/Constants.java)** Modify the class `Field` to be updated for each year's game, and any other constants if needed. - **[Ports.java](src/main/java/org/sciborgs1155/robot/Ports.java)** Modify existing OI and drive ports, as well as adding new ports. - **[Robot.java](src/main/java/org/sciborgs1155/robot/Robot.java)** A lot needs to be added/modified in this file, self-explanatory. diff --git a/src/main/java/org/sciborgs1155/lib/FakePDH.java b/src/main/java/org/sciborgs1155/lib/FakePDH.java deleted file mode 100644 index b3a5899..0000000 --- a/src/main/java/org/sciborgs1155/lib/FakePDH.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.sciborgs1155.lib; - -import com.revrobotics.CANSparkBase; -import edu.wpi.first.wpilibj.RobotController; -import java.util.ArrayList; -import java.util.List; - -/** - * Our actual PDH can't connect to CAN, so this is a workaround to (poorly) estimate our total used - * current based on the stator currents of all our motors with the formula: - * - *

supply current = battery voltage * applied output * stator current - */ -public class FakePDH { - private static final List sparks = new ArrayList<>(); - private static double statorCurrent; - private static double supplyCurrent; - - public static void register(CANSparkBase spark) { - sparks.add(spark); - } - - public static double update() { - double voltage = RobotController.getBatteryVoltage(); - double output = sparks.stream().mapToDouble(CANSparkBase::getAppliedOutput).sum(); - statorCurrent = - sparks.stream().mapToDouble(CANSparkBase::getOutputCurrent).map(Math::abs).sum(); - supplyCurrent = voltage * output * statorCurrent; - return supplyCurrent; - } - - public static double supplyCurrent() { - return supplyCurrent; - } - - public static double statorCurrent() { - return statorCurrent; - } -} diff --git a/src/main/java/org/sciborgs1155/lib/FaultLogger.java b/src/main/java/org/sciborgs1155/lib/FaultLogger.java index 9a26825..176b94c 100644 --- a/src/main/java/org/sciborgs1155/lib/FaultLogger.java +++ b/src/main/java/org/sciborgs1155/lib/FaultLogger.java @@ -200,7 +200,6 @@ public static void register(CANSparkBase spark) { "motor above 100°C", FaultType.WARNING); // TODO actually fix PDH (this is cursed) - FakePDH.register(spark); } /** diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index 40bd571..ea09816 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -23,7 +23,6 @@ import monologue.Monologue; import org.littletonrobotics.urcl.URCL; import org.sciborgs1155.lib.CommandRobot; -import org.sciborgs1155.lib.FakePDH; import org.sciborgs1155.lib.FaultLogger; import org.sciborgs1155.lib.InputStream; import org.sciborgs1155.lib.SparkUtils; @@ -71,7 +70,6 @@ private void configureGameBehavior() { SmartDashboard.putData(CommandScheduler.getInstance()); // Log PDH // SmartDashboard.putData("PDH", new PowerDistribution()); - addPeriodic(() -> log("current", FakePDH.update()), PERIOD.in(Seconds)); // Configure pose estimation updates every tick addPeriodic(() -> drive.updateEstimates(vision.getEstimatedGlobalPoses()), PERIOD.in(Seconds)); diff --git a/src/main/java/org/sciborgs1155/robot/vision/Vision.java b/src/main/java/org/sciborgs1155/robot/vision/Vision.java index 081a41a..665f253 100644 --- a/src/main/java/org/sciborgs1155/robot/vision/Vision.java +++ b/src/main/java/org/sciborgs1155/robot/vision/Vision.java @@ -1,5 +1,6 @@ package org.sciborgs1155.robot.vision; +import static org.sciborgs1155.robot.Constants.*; import static org.sciborgs1155.robot.vision.VisionConstants.*; import edu.wpi.first.math.Matrix; @@ -25,7 +26,6 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.sciborgs1155.lib.FaultLogger; -import org.sciborgs1155.robot.Constants.Field; import org.sciborgs1155.robot.Robot; public class Vision implements Logged { @@ -41,7 +41,7 @@ public static record PoseEstimate(EstimatedRobotPose estimatedPose, Matrix