From af749825edd941c1dbc7189394cacc9fe7efa44a Mon Sep 17 00:00:00 2001 From: Mark Waldron Date: Tue, 26 May 2020 18:32:49 +1000 Subject: [PATCH] Move to a much simpler, static config class and remove using constants for defaults. (#97) * Beginnings of a more simple config file. * Getting closer to unit testing and better config layout. * Swap to Config from RobotConfiguration Move more parameters to config so that it can be removed from Constants. Put Constaints on a diet to remove redundant values. Add ConfigReader unit test. Stop passing config everywhere, using a static instance instead. Use namespaces in Config class for better grouping and more consistency. * Ensure example config is written out. * Separate logging and charting. Move to static methods so that they don't need to be passed around. * Rename method to make it consistent * Add unit test for TimestampedLogWriter. * Move things around to reove a class as per pull request. * Changes from reivew. Renamed Logger->Log and other minor changes. --- src/main/java/frc/robot/Auto.java | 11 +- src/main/java/frc/robot/Config.java | 227 +++++++ src/main/java/frc/robot/Constants.java | 115 +--- src/main/java/frc/robot/OI.java | 14 +- src/main/java/frc/robot/Robot.java | 74 ++- .../java/frc/robot/controller/Controller.java | 67 +- .../java/frc/robot/controller/Sequences.java | 2 +- .../frc/robot/drive/routines/ArcadeDrive.java | 18 +- .../robot/drive/routines/CheesyDpadDrive.java | 5 +- .../robot/drive/routines/ConstantDrive.java | 5 +- .../robot/drive/routines/CurvatureDrive.java | 6 +- .../robot/drive/routines/DriveRoutine.java | 5 +- .../drive/routines/PositionalPIDDrive.java | 22 +- .../robot/drive/routines/TrajectoryDrive.java | 51 +- .../frc/robot/drive/util/PositionCalc.java | 12 +- .../frc/robot/drive/util/PositionPID.java | 12 +- src/main/java/frc/robot/interfaces/Log.java | 192 ------ .../java/frc/robot/interfaces/LogWriter.java | 33 + src/main/java/frc/robot/lib/ConfigReader.java | 262 ++++++++ src/main/java/frc/robot/lib/Jevois.java | 34 +- src/main/java/frc/robot/lib/LEDColour.java | 6 +- src/main/java/frc/robot/lib/LogGraph.java | 607 ------------------ .../frc/robot/lib/LoggingInputDevice.java | 11 +- src/main/java/frc/robot/lib/MotorFactory.java | 89 +-- .../java/frc/robot/lib/NavXGyroscope.java | 43 +- src/main/java/frc/robot/lib/PowerMonitor.java | 21 +- .../java/frc/robot/lib/RedundantTalonSRX.java | 32 +- .../frc/robot/lib/RobotConfiguration.java | 400 ------------ src/main/java/frc/robot/lib/RobotName.java | 17 + src/main/java/frc/robot/lib/Subsystem.java | 9 +- src/main/java/frc/robot/lib/chart/Chart.java | 309 +++++++++ src/main/java/frc/robot/lib/log/Log.java | 170 +++++ .../java/frc/robot/lib/log/LogFileNumber.java | 81 +++ .../java/frc/robot/lib/log/NullLogWriter.java | 26 + .../robot/lib/log/TimestampedLogWriter.java | 147 +++++ .../java/frc/robot/mock/MockBuddyClimb.java | 3 +- .../java/frc/robot/mock/MockColourWheel.java | 3 +- .../java/frc/robot/mock/MockDrivebase.java | 6 +- src/main/java/frc/robot/mock/MockIntake.java | 3 +- src/main/java/frc/robot/mock/MockLoader.java | 3 +- src/main/java/frc/robot/mock/MockLog.java | 121 ---- src/main/java/frc/robot/mock/MockShooter.java | 3 +- .../java/frc/robot/subsystems/BuddyClimb.java | 13 +- .../frc/robot/subsystems/ColourWheel.java | 31 +- .../java/frc/robot/subsystems/Drivebase.java | 61 +- .../java/frc/robot/subsystems/Intake.java | 27 +- .../java/frc/robot/subsystems/LEDStrip.java | 6 +- .../java/frc/robot/subsystems/Loader.java | 42 +- .../java/frc/robot/subsystems/Location.java | 33 +- .../subsystems/OverridableSubsystem.java | 17 +- .../java/frc/robot/subsystems/Shooter.java | 25 +- .../java/frc/robot/subsystems/Subsystems.java | 205 +++--- .../java/frc/robot/subsystems/Vision.java | 42 +- src/test/java/frc/robot/TestSuite.java | 4 +- .../frc/robot/controller/TestController.java | 15 +- .../robot/drive/util/TestPositionCalc.java | 5 +- .../frc/robot/lib/RobotConfigurationTest.java | 78 --- .../java/frc/robot/lib/TestConfigReader.java | 69 ++ .../lib/log/TestTimestampedLogWriter.java | 43 ++ .../frc/robot/subsystems/TestColourwheel.java | 3 +- .../frc/robot/subsystems/TestDrivebase.java | 5 +- .../java/frc/robot/subsystems/TestLoader.java | 5 +- .../frc/robot/subsystems/TestLocation.java | 3 +- .../subsystems/TestOverridableSubsystem.java | 11 +- .../frc/robot/subsystems/TestShooter.java | 3 +- .../java/frc/robot/subsystems/TestVision.java | 5 +- 66 files changed, 1925 insertions(+), 2103 deletions(-) create mode 100644 src/main/java/frc/robot/Config.java delete mode 100644 src/main/java/frc/robot/interfaces/Log.java create mode 100644 src/main/java/frc/robot/interfaces/LogWriter.java create mode 100644 src/main/java/frc/robot/lib/ConfigReader.java delete mode 100644 src/main/java/frc/robot/lib/LogGraph.java delete mode 100644 src/main/java/frc/robot/lib/RobotConfiguration.java create mode 100644 src/main/java/frc/robot/lib/chart/Chart.java create mode 100644 src/main/java/frc/robot/lib/log/Log.java create mode 100644 src/main/java/frc/robot/lib/log/LogFileNumber.java create mode 100644 src/main/java/frc/robot/lib/log/NullLogWriter.java create mode 100644 src/main/java/frc/robot/lib/log/TimestampedLogWriter.java delete mode 100644 src/main/java/frc/robot/mock/MockLog.java delete mode 100644 src/test/java/frc/robot/lib/RobotConfigurationTest.java create mode 100644 src/test/java/frc/robot/lib/TestConfigReader.java create mode 100644 src/test/java/frc/robot/lib/log/TestTimestampedLogWriter.java diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java index 0456e58..9385aa7 100644 --- a/src/main/java/frc/robot/Auto.java +++ b/src/main/java/frc/robot/Auto.java @@ -4,7 +4,8 @@ import frc.robot.controller.Sequence; import frc.robot.controller.Sequence.SequenceBuilder; import frc.robot.controller.Sequences; -import frc.robot.interfaces.Log; +import frc.robot.lib.log.Log; + import static frc.robot.lib.PoseHelper.*; import static frc.robot.Constants.*; @@ -22,11 +23,9 @@ * Auto routines should be defined in Sequences.java */ public class Auto { - private final Log log; private SendableChooser autoProgram = new SendableChooser(); private SendableChooser initBallSelector = new SendableChooser(); - public Auto(Log log) { - this.log = log; + public Auto() { addAutoOptions(); initAutoChooser(); addBallOptions(); @@ -44,13 +43,13 @@ private void addBallOptions() { public void executedSelectedSequence(Controller controller) { Sequence seq = autoProgram.getSelected(); - log.info("Starting selected auto program %s", seq.getName()); + Log.info("Starting selected auto program %s", seq.getName()); controller.doSequence(seq); } public int getSelectedBallAmount() { Integer numBalls = initBallSelector.getSelected(); - log.info("Starting with %s balls", numBalls); + Log.info("Starting with %s balls", numBalls); return numBalls; } diff --git a/src/main/java/frc/robot/Config.java b/src/main/java/frc/robot/Config.java new file mode 100644 index 0000000..b148b0d --- /dev/null +++ b/src/main/java/frc/robot/Config.java @@ -0,0 +1,227 @@ +package frc.robot; + +import frc.robot.lib.ConfigReader; +import frc.robot.lib.PIDF; +import static frc.robot.Constants.*; + +/** + * Class responsible for updating values which are dependent on robot hardware. + * (e.g. if subsystems are present or not) It reads from a text file Currently + * the supported types are String, int, double, boolean and int array. + * + * Example lines: + * drivebase/present = true + * drivebase/rampRate = 0.13125 +* drivebase/right/canIDs/withEncoders = 7,6 + * drivebase/right/canIDs/withoutEncoders = 5 + * + * The configuration can be overridden on each robot by changing a text file + * stored on the robot allowing different robots to have different + * configuration preventing having to modify the code each time it's pushed + * to a different bit of hardware. + * + * This is very useful for testing when parts of the hardware are not + * attached, delivered or even broken. + */ +public class Config { + + /* + * Drivebase parameters + * + * The robot has motors on each side. This is the information that defines these + * motors and their behaviour + */ + public static class drivebase { + public static final boolean present = getBoolean("drivebase/present", false); + public static final String motorControllerType = getMotorControllerType("drivebase/motorControllerType", DEFAULT_MOTOR_CONTROLLER_TYPE); + public static final int[] canIdsLeftWithEncoders = getIntArray("drivebase/left/canIDs/withEncoders", new int[] { 4, 5 }); + public static final int[] canIdsLeftWithoutEncoders = getIntArray("drivebase/left/canIDs/withoutEncoders", new int[] {}); + public static final int[] canIdsRightWithEncoders = getIntArray("drivebase/right/canIDs/withEncoders", new int[] { 1, 2 }); + public static final int[] canIdsRightWithoutEncoders = getIntArray("drivebase/right/canIDs/withoutEncoders", new int[] {}); + public static final boolean currentLimiting = getBoolean("drivebase/currentLimiting", true); + public static final int contCurrent = getInt("drivebase/contCurrent", 38); + public static final int peakCurrent = getInt("drivebase/peakCurrent", 80); + public static final double rampRate = getDouble("drivebase/rampRate", 0.01); + public static final PIDF pidf = getPIDF("drivebase", new PIDF(0, 0, 0, 0.7)); + public static final String mode = getString("drivebase/mode", DRIVE_MODE_ARCADE); + public static final double maxSpeed = getDouble("drivebase/maxSpeed", 4.0); + public static final boolean swapLeftRight = getBoolean("drivebase/swapLeftRight", false); + public static final boolean sensorPhase = getBoolean("drivebase/sensor/phase", false); + } + + /** + * NavX + * + * Using the gyro for autonomous routines. + */ + public static class navx { + public static final boolean present = getBoolean("navx/present", true); + } + + /* + * Climber parameters. + * + * The climber is a PTO from the drivebase. There is a solenoid to release the + * brake. + */ + public static class climber { + public static final int ptoPort = 6; + public static final int brakePort = 0; + } + + /* + * Intake parameters. + * + * Uses a pneumatic to deploy and a motor to run mecanum wheels. + */ + public static class intake { + public static final boolean present = getBoolean("intake/present", false); + public static final int canID = getInt("intake/canID", 10); + public static final PIDF pidf = getPIDF("intake", new PIDF(0.015, 0.0, 15.0, 0.019)); + public static final int solenoidPort = getInt("intake/solenoidPort", 1); + } + + /** + * Colour wheel parameters. + * + * Unknown deployment method. Single motor to spin wheel. + */ + public static class colourWheel { + public static final boolean present = getBoolean("colourWheel/present", false); + public static final int canID = getInt("colourWheel/canID", 7); + public static final PIDF pidf = getPIDF("colourWheel", new PIDF(0, 0, 0, 0)); + public static final int solenoidPort = getInt("colourWheel/solenoidPort", 5); + } + + /** + * Loader parameters. + * + * A hooper containing a spinner motor for pushing the balls into the shooter. + */ + public static class loader { + public static final boolean present = getBoolean("loader/present", false); + public static final int spinnerCanID = getInt("loader/spinner/canID", 12); + public static final PIDF spinnderPIDF = getPIDF("loader/spinner/", new PIDF(0.3, 0.0, 30.0, 0.1)); + public static final int passthroughCanID = getInt("loader/passthrough/canID", 11); + public static final PIDF passthroughPIDF = getPIDF("loader/passthrough/", new PIDF(0, 0, 0, 0)); + public static final int ballInDetectorPort = getInt("loader/ballInDetectorPort", 0); + public static final int ballOutDetectorPort = getInt("loader/ballOutDetectorPort", 1); + public static final int solenoidPort = getInt("loader/solenoidPort", 2); + } + + /** + * Shooter parameters. + * + * A single shooter wheel powered by three motors. + */ + public static class shooter { + public static final boolean present = getBoolean("shooter/present", false); + public static final int[] canIds = getIntArray("shooter/shooterCanIDs", new int[] { 30, 31, 32 });; + public static final PIDF pidf = getPIDF("shooter/", new PIDF(0.7, 0.0, 0.0, 0.08)); + public static final int solenoidPort = getInt("shooter/solenoidPort", 3); + } + + /** + * PDP parameters + * + * The motor controller wrappers also monitor current, so this is normally off. + */ + public static class pdp { + public static final boolean present = getBoolean("pdp/present", false); + public static final int canId = getInt("pdp/canID", 62); + public static final boolean monitor = getBoolean("pdp/monitor", false); // by default we do NOT monitor the PDP + public static final int[] channelsToMonitor = getIntArray("pdp/channels", new int[0]); // by default we do NOT monitor any channels + } + + /** + * PCM parameters + * + * Pneumatic control module controls intake, the climber brake, buddy climb and + * the shooter hood. + */ + public static class pcm { + public static final boolean present = getBoolean("pcm/present", false); + public static final int canId = getInt("pcm/canID", 61); + } + + /** + * Vision parameters + * + * A jevois camera connected via USB for detecting the vision target on the + * goal. + */ + public static class vision { + public static final boolean present = getBoolean("vision/present", false); + public static final double hMin = getDouble("vision/hsvFilter/h/min", 40.0); + public static final double hMax = getDouble("vision/hsvFilter/h/max", 100.0); + public static final double sMin = getDouble("vision/hsvFilter/s/min", 20.0); + public static final double sMax = getDouble("vision/hsvFilter/s/max", 225.0); + public static final double vMin = getDouble("vision/hsvFilter/v/min", 40.0); + public static final double vMax = getDouble("vision/hsvFilter/v/max", 255.0); + } + + /** + * Buddy climb + * + * Not currently used or read from the config file. + */ + public static class buddyClimb { + public static final boolean present = getBoolean("buddyClimb/present", false); + public static final int solenoidPort = getInt("buddyClimb/solenoidPort", 7); + } + + /** + * LED strip + * + * Used to indicate the state of the robot (balls count, shoot count, issues). + */ + public static class ledStrip { + public static final boolean present = getBoolean("ledStrip/present", false); + public static final int pwmPort = getInt("ledStrip/pwmPort", 0); + public static final int numLEDs = getInt("ledStrip/numLEDs", 30); + } + + public static final boolean doCharting = getBoolean("charting/enabled", true); + + + + // Only implementation from here onwards. + + private final static ConfigReader reader = new ConfigReader(); + + /** + * Needs to be called after the config is loaded to write out an example config + * file and to print out details about the config file. + */ + public static void finishLoadingConfig() { + reader.finishLoadingConfig(); + } + + protected static String getMotorControllerType(final String parameterName, final String defaultValue) { + return reader.getMotorControllerType(parameterName, defaultValue); + } + + protected static int getInt(final String key, final int defaultValue) { + return reader.getInt(key, defaultValue); + } + + protected static double getDouble(final String key, final double defaultValue) { + return reader.getDouble(key, defaultValue); + } + + protected static PIDF getPIDF(final String prefix, final PIDF pidf) { + return reader.getPIDF(prefix, pidf); + } + + protected static boolean getBoolean(final String key, final boolean defaultValue) { + return reader.getBoolean(key, defaultValue); + } + + protected static String getString(final String key, final String defaultValue) { + return reader.getString(key, defaultValue); + } + + protected static int[] getIntArray(final String key, final int[] defaultValue) { + return reader.getIntArray(key, defaultValue); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5ef5db4..a604d17 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,8 +26,6 @@ private Constants() { public static final double FULL_CIRCLE = 360.0; // size of a full circle in internal units (degrees) public static final double HALF_CIRCLE = 180.0; // size of a half circle in internal units (degrees) public static final double QUARTER_CIRCLE = 90.0; // size of a quarter circle in internal units (degrees) - public static final int NUM_JOYSTICK_BUTTONS = 32; // maximum number of push buttons on a joystick - public static final int NUM_JOYSTICK_DIRECTIONS = 10; public static final double INCHES_TO_METRES = 0.0254; public static final double JOYSTICK_DEADBAND_MINIMUM_VALUE = 0.05; // below this we deadband the value away @@ -51,16 +49,9 @@ private Constants() { * * The robot has motors on each side. This is the information that defines these motors and their behaviour */ - public static final boolean DRIVE_PRESENT_DEFAULT = false; - public static final int[] DRIVE_LEFT_TALON_WITH_ENCODERS_CAN_ID_LIST = {4,5}; - public static final int[] DRIVE_LEFT_TALON_WITHOUT_ENCODERS_CAN_ID_LIST = {}; - public static final int[] DRIVE_RIGHT_TALON_WITH_ENCODERS_CAN_ID_LIST = {1,2}; - public static final int[] DRIVE_RIGHT_TALON_WITHOUT_ENCODERS_CAN_ID_LIST = {}; - public static final boolean DRIVE_BRAKE_MODE = true; - public static final String MOTOR_CONTROLLER_TYPE_TALONSRX = "TalonSRX"; public static final String MOTOR_CONTROLLER_TYPE_SPARKMAX = "SparkMAX"; - public static final String DRIVE_DEFAULT_CONTROLLER_TYPE = MOTOR_CONTROLLER_TYPE_TALONSRX; + public static final String DEFAULT_MOTOR_CONTROLLER_TYPE = MOTOR_CONTROLLER_TYPE_TALONSRX; // Encoder values public static final double FALCON_ENCODER_TICKS = 2048; // Falon inbuilt encoders. @@ -72,29 +63,10 @@ private Constants() { public static final double DRIVE_WHEEL_DIAMETER_METRES = 6 * INCHES_TO_METRES; // 6" wheels. public static final double DRIVE_METRES_PER_REV = DRIVE_WHEEL_DIAMETER_METRES * Math.PI; public static final double DRIVE_GEABOX_RATIO = 189.0/17.0; - - // This magic number is the "fastest" we want the motor to go. It is calculated - // by running the motor at full speed and observing what the quad encoder - // velocity returns. - // This number is very suspect. - public static final double DRIVE_COUNT_100ms = 13.0; - // A more sensible number. - public static final double DRIVE_MAX_SPEED = 4; - public static final double DRIVE_MAX_ACCELERATION = 2; // Inches/sec/sec - public static final double DRIVE_MAX_JERK = 1; // Inches/sec/sec/sec. - public static final double DRIVE_P = 0.0;//5;//1.0; - public static final double DRIVE_I = 0.0; - public static final double DRIVE_D = 0.0;//0.01; - public static final double DRIVE_F = 0.7;//0.665; - public static final double DRIVE_DEADBAND = 0.05; - public static final double DRIVE_RAMP_RATE = 0.01; //0.175; sluggish but smooth //0.1375; jittered // seconds from neutral to full + public static final String DRIVE_MODE_ARCADE = "arcade"; public static final String DRIVE_MODE_CHEESY = "cheesy"; - public static final String DRIVE_DEFAULT_MODE = DRIVE_MODE_ARCADE; // Joystick teleop mode. - public static final int DRIVE_CONT_CURRENT = 38; // current limit to this value if... - public static final int DRIVE_PEAK_CURRENT = 80; // the current exceeds this value for 100ms - public static final int DRIVE_SCALE_FACTOR = 128; - public static final double DRIVE_OFF_LEVEL_TWO_POWER = 0.3; + public static final double DRIVE_SLOW_POWER = 0.3; /** @@ -127,43 +99,16 @@ private Constants() { public static final Pose2d AUTO_LINE_GOAL = new Pose2d(AUTO_LINE_XPOS - HALF_ROBOT_LENGTH, GOAL_YPOS, new Rotation2d(Math.toRadians(0))); public static final Pose2d AUTO_LINE_OPPOSING_TRENCH = new Pose2d(AUTO_LINE_XPOS - HALF_ROBOT_LENGTH, OPPOSING_TRENCH_BALLS_YPOS, new Rotation2d(Math.toRadians(0))); - /* - * LED constants - */ - // brightness - public static final double LED_PERCENTAGE = 0.2; - /* * Intake constants */ - public static final boolean INTAKE_PRESENT_DEFAULT = false; - public static final int INTAKE_CAN_ID = 10; public static final double INTAKE_ENCODER_GEARBOX_RATIO = 3; public static final double INTAKE_TARGET_RPS = 30; - public static final double INTAKE_F = 0.019; - public static final double INTAKE_P = 0.015; - public static final double INTAKE_I = 0; - public static final double INTAKE_D = 15.0; - - - - public static final int INTAKE_SOLENOID_PORT = 1; - - public static final int[] TEST_SPARK_MOTOR_CAN_ID_LIST = {50, 51}; - - public static final int[] OUTTAKE_MOTOR_TALON_CAN_ID_LIST = {15}; /* * Shooter constants */ - public static final boolean SHOOTER_PRESENT_DEFAULT = false; - public static final int[] SHOOTER_TALON_CAN_ID_LIST = {30, 31, 32}; - public static final int SHOOTER_HOOD_SOLENOID_PORT = 3; public static final double SHOOTER_SPEED_TOLERANCE_RPS = 3.33; - public static final double SHOOTER_F = 0.08;//0.075; - public static final double SHOOTER_P = 0.7; - public static final double SHOOTER_I = 0; - public static final double SHOOTER_D = 0; public static final double SHOOTER_CLOSE_TARGET_SPEED_RPS = 62; public static final double SHOOTER_AUTO_LINE_TARGET_SPEED_RPS = 90; public static final double SHOOTER_FAR_TARGET_SPEED_RPS = 95; @@ -171,29 +116,11 @@ private Constants() { /* * Loader - */ - public static final boolean LOADER_PRESENT_DEFAULT = false; - public static final int LOADER_SPINNER_MOTOR_CAN_ID = 12; - public static final int LOADER_PASSTHROUGH_MOTOR_CAN_ID = 11; - public static final int IN_BALL_DETECTOR_DIO_PORT = 0; - public static final int OUT_BALL_DETECTOR_DIO_PORT = 1; - public static final int PADDLE_SOLENOID_PORT = 2; + */ public static final double LOADER_MOTOR_INTAKING_RPS = 18; public static final double LOADER_MOTOR_SHOOTING_RPS = 8; public static final double PASSTHROUGH_MOTOR_CURRENT = 0.8; public static final double LOADER_MAIN_MOTOR_GEARBOX_RATIO = 1; // Encoder is on output shaft. - public static final double LOADER_SPINNER_P = 0.3; - public static final double LOADER_SPINNER_I = 0; - public static final double LOADER_SPINNER_D = 30; - public static final double LOADER_SPINNER_F = 0.1; - - // Power distribution Panel (PDP) - public static final boolean PDP_PRESENT_DEFAULT = false; - public static final int PDP_CAN_ID = 62; - - // Pneumatic Control Modules (PCM) - public static final boolean PCM_PRESENT_DEFAULT = false; - public static final int PCM_CAN_ID = 61; /* * Camera server constants @@ -203,7 +130,6 @@ private Constants() { public static final int CAMERA_RESOULTION_HEIGHT = 252; public static final int CAMERA_FRAMES_PER_SECOND = 60; // Vision (all need tuning) - public static final boolean VISION_PRESENT_DEFAULT = false; public static final double VISON_MAX_TARGET_AGE_SECS = 2; public static final double VISION_MAX_VELOCITY_JERK = 32; // m/s/s public static final double VISION_SPEED_SCALE = 2.4 * INCHES_TO_METRES; @@ -219,30 +145,13 @@ private Constants() { public static final double VISION_AIM_TIME_COMPLETE = 0.5; // seconds - - // Vision filter parameters - public static final double VISION_H_MIN = 40; - public static final double VISION_H_MAX = 100; - public static final double VISION_S_MIN = 20; - public static final double VISION_S_MAX = 255; - public static final double VISION_V_MIN = 40; - public static final double VISION_V_MAX = 255; - // Vision low pass filter (needs tuning) public static final double GOAL_LOWPASS_ALPHA = 0.2; // Turn to angle (all need tuning) public static final double TURN_TO_ANGLE_MAX_VELOCITY_JERK = 50; public static final double TURN_TO_ANGLE_ANGLE_SCALE = 0.3; - - // Climber - public static final int CLIMBER_PTO_SOLENOID_PORT = 6; - public static final int CLIMBER_BRAKE_SOLENOID_PORT = 0; - // Buddy climb - public static final boolean BUDDYCLIMB_PRESENT_DEFAULT = false; - public static final int BUDDYCLIMB_SOLENOID_PORT = 7; - // logging information constants public static final String USB_FLASH_DRIVE = "/media/sda1/"; public static final String WEB_BASE_PATH = USB_FLASH_DRIVE; // where web server's data lives @@ -252,7 +161,6 @@ private Constants() { public static final String LOG_LATEST_EXTENSION = "latest"; public static final String LOG_EVENT_EXTENSION = "event"; public static final int WEB_PORT = 5800;// first open port for graph/log web server - public static final double LOG_GRAPH_PERIOD = 0.05; // run the graph updater every 50ms // Config WebServer public static final String CONFIG_WEB_ROOT = "/home/lvuser/www"; @@ -262,15 +170,9 @@ private Constants() { public static final int LOCATION_HISTORY_MEMORY_SECONDS = 5; public static final int LOCATION_HISTORY_CYCLE_SPEED = 100; // in hz - // NavX (gyro) - public static final boolean NAVX_PRESENT_DEFAULT = DRIVE_PRESENT_DEFAULT; - /* * Command timings */ - public static final double TIME_COMMAND_RUN_PERIOD = (1.0/50.0); // run the commands 50 times a second - public static final double TIME_LOCATION_PERIOD = (1.0/(double)LOCATION_HISTORY_CYCLE_SPEED); // update the location subsystem 100 times a second - public static final double TIME_DRIVEBASE_PERIOD = (1.0/40.0); // update the drivebase 40 times a second public static final class DriveConstants { public static final double kTrackwidthMeters = 0.71; @@ -305,9 +207,6 @@ public static final class DriveConstants { /* * Colour Wheel */ - public static final boolean COLOUR_WHEEL_PRESENT_DEFAULT = false; - public static final int COLOUR_WHEEL_CAN_ID = 7; - public static final int COLOUR_WHEEL_SOLENOID_PORT = 5; // Values callibrated using vynl sticker for control panel. public static final Color COLOUR_WHEEL_BLUE_TARGET = ColorMatch.makeColor(0.147, 0.437, 0.416); // Values from the colour sensor used to match colours. // public static final Color COLOUR_WHEEL_GREEN_TARGET = ColorMatch.makeColor(0.189, 0.559, 0.250); // This is the real green value. @@ -323,12 +222,10 @@ public static final class DriveConstants { public static final int COLOUR_WHEEL_DELAY = 15; // Time in miliseconds to wait before disabling motor when correct colour found. /* - * LED Strip + * LED constants */ - public static final boolean LED_STRIP_PRESENT_DEFAULT = false; - public static final int LED_STRIP_PWM_PORT = 0; - public static final int LED_STRIP_NUMBER_OF_LEDS = 30; public static final int LED_STRIP_COUNTDOWN = 15; + public static final double LED_BRIGHTNESS_PERCENTAGE = 0.2; /* * Log Syncing diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 508d616..0e9a8a7 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -14,6 +14,7 @@ import frc.robot.lib.GamepadButtonsX; import frc.robot.lib.OperatorBoxButtons; import frc.robot.lib.WheelColour; +import frc.robot.lib.log.Log; import frc.robot.subsystems.*; @@ -21,15 +22,12 @@ public class OI implements OIInterface { private SwitchReactor reactor = Strongback.switchReactor(); private Controller exec; - private Log log; private Subsystems subsystems; - public OI(Controller controller, Subsystems subsystems, Log log) { + public OI(Controller controller, Subsystems subsystems) { this.exec = controller; this.subsystems = subsystems; - this.log = log; - } /* @@ -312,7 +310,7 @@ public ModeSwitch(Switch switchOn, Switch switchOff, String name, Sequence activ if (active) { return; } - log.sub("Activating " + name); + Log.debug("Activating " + name); exec.doSequence(activatedSeq); active = true; }); @@ -320,7 +318,7 @@ public ModeSwitch(Switch switchOn, Switch switchOff, String name, Sequence activ if (!active) { return; } - log.sub("Deactivating " + name); + Log.debug("Deactivating " + name); exec.doSequence(deactivedSeq); active = false; }); @@ -424,10 +422,10 @@ private class ToggleSwitch { public ToggleSwitch(Switch swtch, String name, Sequence onSeq, Sequence offSeq) { Strongback.switchReactor().onTriggered(swtch, () -> { if (!toggled) { - log.sub("Toggling on " + name); + Log.debug("Toggling on " + name); exec.doSequence(onSeq); } else { - log.sub("Toggling off " + name); + Log.debug("Toggling off " + name); exec.doSequence(offSeq); } toggled = !toggled; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7eb3e51..711e594 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,19 +29,16 @@ import frc.robot.interfaces.OIInterface; import frc.robot.lib.ConfigServer; import frc.robot.lib.LEDColour; -import frc.robot.lib.LogGraph; import frc.robot.lib.Position; import frc.robot.lib.PowerMonitor; import frc.robot.lib.RedundantTalonSRX; -import frc.robot.lib.RobotConfiguration; -import frc.robot.lib.RobotName; import frc.robot.lib.WheelColour; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; import frc.robot.subsystems.Subsystems; public class Robot extends IterativeRobot implements Executable { private Clock clock; - private RobotConfiguration config; - private LogGraph log; // User interface. private DriverStation driverStation; @@ -69,13 +66,9 @@ public class Robot extends IterativeRobot implements Executable { @Override public void robotInit() { clock = Strongback.timeSystem(); - var robotName = RobotName.get(Constants.ROBOT_NAME_FILE_PATH); - log = new LogGraph(robotName, Constants.WEB_BASE_PATH, Constants.LOG_BASE_PATH, Constants.LOG_DATA_EXTENSION, - Constants.LOG_DATE_EXTENSION, Constants.LOG_LATEST_EXTENSION, Constants.LOG_EVENT_EXTENSION, false, clock); - config = new RobotConfiguration(Constants.CONFIG_FILE_PATH, log); startWebServer(); startConfigServer(); - log.info("Waiting for driver's station to connect before setting up UI"); + Log.info("Waiting for driver's station to connect before setting up UI"); // Do the reset of the initialization in init(). } @@ -87,7 +80,7 @@ public void maybeInit() { setupCompleted = true; } catch (Exception e) { // Write the exception to the log file. - log.exception("Exception caught while initializing robot", e); + Log.exception("Exception caught while initializing robot", e); throw e; // Cause it to abort the robot startup. } } @@ -99,12 +92,14 @@ public void init() { Strongback.logConfiguration(); Strongback.setExecutionPeriod(Constants.EXECUTOR_CYCLE_INTERVAL_MSEC); - log.info("Robot initialization started"); + Log.info("Robot initialization started"); + // Write out the example config and print any config warnings. + Config.finishLoadingConfig(); createInputDevices(); // Setup the hardware/subsystems. Listed here so can be quickly jumped to. - subsystems = new Subsystems(createDashboard(), config, clock, log); + subsystems = new Subsystems(createDashboard(), clock); subsystems.createLEDStrip(); subsystems.createPneumatics(); subsystems.createDrivebaseLocation(driverLeftJoystick, driverRightJoystick); @@ -131,9 +126,9 @@ public void init() { Strongback.start(); // Setup the auto sequence chooser. - auto = new Auto(log); + auto = new Auto(); - log.info("Robot initialization successful"); + Log.info("Robot initialization successful"); } /** @@ -152,7 +147,7 @@ public void robotPeriodic() { public void disabledInit() { PortForwarder.add(Constants.RSYNC_PORT, Constants.RSYNC_HOSTNAME, 22); // Start forwarding to port 22 (ssh port) for pulling logs using rsync. maybeInit(); // Called before robotPeriodic(). - log.info("disabledInit"); + Log.info("disabledInit"); // Log any failures again on disable. RedundantTalonSRX.printStatus(); // Tell the controller to give up on whatever it was processing. @@ -175,8 +170,9 @@ public void disabledPeriodic() { @Override public void autonomousInit() { PortForwarder.remove(Constants.RSYNC_PORT); // Stop forwarding port to stop rsync and save bandwidth. - log.restartLogs(); - log.info("auto has started"); + Chart.restartCharts(); + Log.restartLogs(); + Log.info("auto has started"); subsystems.enable(); controller.doSequence(Sequences.getStartSequence()); @@ -203,8 +199,9 @@ public void autonomousPeriodic() { @Override public void teleopInit() { PortForwarder.remove(Constants.RSYNC_PORT); // Stop forwarding port to stop rsync and save bandwidth. - log.restartLogs(); - log.info("teleop has started"); + Chart.restartCharts(); + Log.restartLogs(); + Log.info("teleop has started"); subsystems.enable(); controller.doSequence(Sequences.setDrivebaseToArcade()); subsystems.setLEDColour(allianceLEDColour()); @@ -229,7 +226,8 @@ public void teleopPeriodic() { */ @Override public void testInit() { - log.restartLogs(); + Chart.restartCharts(); + Log.restartLogs(); subsystems.enable(); } @@ -265,8 +263,8 @@ public void putBoolean(String key, Boolean value) { * Create the camera servers so the driver & operator can see what the robot can see. */ public void createCameraServers() { - if (!config.visionIsPresent) { - log.sub("Vision not enabled, not creating a camera server"); + if (!Config.vision.present) { + Log.debug("Vision not enabled, not creating a camera server"); return; } UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(0); @@ -282,8 +280,8 @@ public void createCameraServers() { */ private void createPowerMonitor() { // Do not monitor if not present, or we have been asked not to monitor - boolean enabled = config.pdpIsPresent || config.pdpMonitor; - pdp = new PowerMonitor(new PowerDistributionPanel(config.pdpCanId), config.pdpChannelsToMonitor, enabled, log); + boolean enabled = Config.pdp.present || Config.pdp.monitor; + pdp = new PowerMonitor(new PowerDistributionPanel(Config.pdp.canId), Config.pdp.channelsToMonitor, enabled); } /** @@ -296,9 +294,9 @@ private void startWebServer() { File fileDir = new File(Constants.WEB_BASE_PATH); try { new SimpleWebServer(fileDir, Constants.WEB_PORT); - log.sub("WebServer started at port: " + Constants.WEB_PORT); + Log.debug("WebServer started at port: " + Constants.WEB_PORT); } catch (Exception e) { - log.sub("Failed to start webserver on directory " + fileDir.getAbsolutePath()); + Log.debug("Failed to start webserver on directory " + fileDir.getAbsolutePath()); e.printStackTrace(); } @@ -310,9 +308,9 @@ private void startWebServer() { private void startConfigServer() { try { new ConfigServer(Constants.CONFIG_WEB_ROOT, Constants.CONFIG_FILE_PATH, Constants.ROBOT_NAME_FILE_PATH, Constants.CONFIG_WEB_PORT); - log.sub("Config webserver started at port: " + Constants.WEB_PORT); + Log.debug("Config webserver started at port: " + Constants.WEB_PORT); } catch (Exception e) { - log.sub("Failed to start config webserver."); + Log.debug("Failed to start config webserver."); e.printStackTrace(); } } @@ -333,13 +331,13 @@ private void createInputDevices() { * box if it's attached. */ private void setupUserInterface() { - oi = new OI(controller, subsystems, log); + oi = new OI(controller, subsystems); oi.configureJoysticks(driverLeftJoystick, driverRightJoystick, operatorJoystick); if (operatorBox.getButtonCount() > 0) { - log.info("Operator box detected"); + Log.info("Operator box detected"); oi.configureDiagBox(operatorBox); } - log.register(false, driverStation::getMatchTime, "DriverStation/MatchTime"); + Chart.register(driverStation::getMatchTime, "DriverStation/MatchTime"); } /** @@ -352,19 +350,19 @@ private void startLogging() { driverStation.getEventName(), driverStation.getMatchType().toString(), driverStation.getMatchNumber(), driverStation.getReplayNumber(), driverStation.getAlliance().toString(), driverStation.getLocation()); - log.logCompletedElements(matchDescription); - if (config.doLogging) { + Chart.registrationComplete(matchDescription); + if (Config.doCharting) { // Low priority means run every 20 * 4 = 80ms, or at 12.5Hz // It polls almost everything on the CAN bus, so don't want it to be too fast. - Strongback.executor().register(log, Priority.LOW); + Strongback.executor().register(new Chart(), Priority.LOW); } else { - log.error("Logging: Dygraph logging disabled"); + Log.error("Logging: Dygraph logging disabled"); } } @Override public void execute(long timeInMillis) { - //log.sub("Updating smartDashboard"); + //Logger.debug("Updating smartDashboard"); maybeUpdateSmartDashboard(); } @@ -388,7 +386,7 @@ private void maybeUpdateSmartDashboard() { public WheelColour getFMSColour() { String fmsColour = driverStation.getGameSpecificMessage(); if (!fmsColour.equals(lastColour)) { - log.info("FMS Colour: %s", fmsColour); + Log.info("FMS Colour: %s", fmsColour); lastColour = fmsColour; } if (fmsColour.length() == 0) { diff --git a/src/main/java/frc/robot/controller/Controller.java b/src/main/java/frc/robot/controller/Controller.java index 60b2775..a3af468 100644 --- a/src/main/java/frc/robot/controller/Controller.java +++ b/src/main/java/frc/robot/controller/Controller.java @@ -1,22 +1,21 @@ package frc.robot.controller; - + import java.util.Iterator; import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.strongback.components.Clock; +import frc.robot.controller.Sequence.SequenceBuilder; import frc.robot.interfaces.ColourWheelInterface.ColourAction; import frc.robot.interfaces.ColourWheelInterface.ColourAction.ColourWheelType; import frc.robot.interfaces.DashboardInterface; import frc.robot.interfaces.DashboardUpdater; -import frc.robot.interfaces.Log; import frc.robot.lib.LEDColour; import frc.robot.lib.WheelColour; +import frc.robot.lib.log.Log; import frc.robot.subsystems.Subsystems; -import frc.robot.controller.Sequence.SequenceBuilder; - /** * The controller of State Sequences while ensuring the robot is safe at every @@ -41,7 +40,6 @@ public class Controller implements Runnable, DashboardUpdater { private final Subsystems subsystems; private final Clock clock; private final DashboardInterface dashboard; - private final Log log; private Sequence sequence = new SequenceBuilder("idle",false).build(); // Current sequence we are working through. private boolean sequenceHasChanged = true; private boolean sequenceHasFinished = true; @@ -55,7 +53,6 @@ public Controller(Subsystems subsystems, Supplier fmsColour) { this.subsystems = subsystems; this.clock = subsystems.clock; this.dashboard = subsystems.dashboard; - this.log = subsystems.log; this.fmsColour = fmsColour; (new Thread(this)).start(); } @@ -71,7 +68,7 @@ synchronized public void doSequence(Sequence sequence) { endState = this.sequence.getEndState(); this.sequence = sequence; sequenceHasChanged = true; - logSub("Sequence has changed to %s sequence", sequence.getName()); + debug("Sequence has changed to %s sequence", sequence.getName()); notifyAll(); // Tell the run() method that there is a new sequence. } @@ -89,24 +86,24 @@ public void run() { State desiredState = null; synchronized (this) { if (!sequenceHasFinished && sequenceHasChanged) { - logSub("Sequence interrupted, applying end state."); + debug("Sequence interrupted, applying end state."); applyState(endState); } if (sequenceHasChanged || iterator == null) { - logSub("State sequence has changed, now executing %s sequence", sequence.getName()); + debug("State sequence has changed, now executing %s sequence", sequence.getName()); iterator = sequence.iterator(); sequenceHasChanged = false; sequenceHasFinished = false; } if (!iterator.hasNext()) { - logSub("Sequence %s is complete", sequence.getName()); + debug("Sequence %s is complete", sequence.getName()); sequenceHasFinished = true; try { - logSub("Controller waiting for a new sequence to run"); + debug("Controller waiting for a new sequence to run"); wait(); - // logSub("Have a new sequence to run"); + // debug("Have a new sequence to run"); } catch (InterruptedException e) { - logSub("Waiting interrupted %s", e); + debug("Waiting interrupted %s", e); } continue; // Restart from the beginning. } @@ -116,7 +113,7 @@ public void run() { } } catch (Exception e) { // The controller is dying, write the exception to the logs. - log.exception("Controller caught an unhandled exception", e); + Log.exception("Controller caught an unhandled exception", e); // Used by the unit tests to detect if the controller thread is still running // see isAlive() @@ -148,18 +145,18 @@ private void applyState(State desiredState) { System.out.println("Desired state is null!!!"); } - logSub("Applying requested state: %s", desiredState); - //logSub("Waiting subsystems to finish moving before applying state"); + debug("Applying requested state: %s", desiredState); + //debug("Waiting subsystems to finish moving before applying state"); // Get the current state of the subsystems. State currentState = new State(subsystems, clock); - logSub("Current state: %s", currentState); + debug("Current state: %s", currentState); // Fill in the blanks in the desired state. desiredState = State.calculateUpdatedState(desiredState, currentState); if (desiredState.colourAction.movingToUnknownColour()) { // If the colour wheel is set to positional but the colour is unknown, work out the desired colour using FMS. desiredState.colourAction = new ColourAction(ColourWheelType.POSITION, fmsColour.get()); } - logSub("Calculated new 'safe' state: %s", desiredState); + debug("Calculated new 'safe' state: %s", desiredState); // The time beyond which we are allowed to move onto the next state double endTime = desiredState.timeAction.calculateEndTime(clock.currentTime()); @@ -220,7 +217,7 @@ private void maybeWaitForAutoDriving() { try { waitUntilOrAbort(() -> subsystems.drivebase.hasFinished(), "auto driving", LEDColour.RED); } catch (SequenceChangedException e) { - logSub("Sequence changed while driving, switching drivebase back to arcade"); + debug("Sequence changed while driving, switching drivebase back to arcade"); subsystems.drivebase.setArcadeDrive(); } } @@ -233,7 +230,7 @@ private void maybeWaitForIntake() { try { waitUntilOrAbort(() -> subsystems.intake.isRetracted() || subsystems.intake.isExtended(), "intake to finish moving", LEDColour.YELLOW); } catch (SequenceChangedException e) { - logSub("Sequence changed while deploying/intaking"); + debug("Sequence changed while deploying/intaking"); } } /** @@ -244,7 +241,7 @@ private void maybeWaitForBlocker() { try { waitUntilOrAbort(() -> subsystems.loader.isPaddleBlocking() || subsystems.loader.isPaddleNotBlocking(), "blocking to finish moving", LEDColour.BLUE); } catch (SequenceChangedException e) { - logSub("Sequence changed while blocking/not blocking with paddle"); + debug("Sequence changed while blocking/not blocking with paddle"); } } @@ -256,7 +253,7 @@ private void maybeWaitForShooterHood() { try { waitUntilOrAbort(() -> subsystems.shooter.isHoodExtended() || subsystems.shooter.isHoodRetracted(), "hood to finish moving", LEDColour.GREEN); } catch (SequenceChangedException e) { - logSub("Sequence changed while Shooter Hood extends/retracts"); + debug("Sequence changed while Shooter Hood extends/retracts"); } } /** @@ -270,14 +267,14 @@ private void maybeWaitForShooter(Boolean shooterUpToSpeed) { } // set the LEDs to purple if we are trying to wait for the shooter to reach 0 rps if (shooterUpToSpeed && subsystems.shooter.getTargetRPS() == 0) { - logSub("Should never be waiting for the shooter to reach 0 RPS. Running the empty sequence"); + debug("Should never be waiting for the shooter to reach 0 RPS. Running the empty sequence"); doSequence(Sequences.setLEDColour(LEDColour.PURPLE)); } try { waitUntilOrAbort(() -> subsystems.shooter.isAtTargetSpeed(), "shooter", LEDColour.CYAN); } catch (SequenceChangedException e) { - logSub("Sequence changed while spinning up shooter, stopping shooter"); + debug("Sequence changed while spinning up shooter, stopping shooter"); subsystems.shooter.setTargetRPS(0); } } @@ -287,7 +284,7 @@ private void maybeWaitForColourWheel() { try { waitUntilOrAbort(() -> subsystems.colourWheel.isFinished(), "colour wheel finished", LEDColour.ORANGE); } catch (SequenceChangedException e) { - logSub("Sequence changed while moving colour wheel"); + debug("Sequence changed while moving colour wheel"); // The sequence has changed, setting action to null. subsystems.colourWheel.setDesiredAction(new ColourAction(ColourWheelType.NONE, WheelColour.UNKNOWN)); subsystems.colourWheel.setArmExtended(false); @@ -298,7 +295,7 @@ private void maybeWaitForBuddyClimb() { try { waitUntilOrAbort(() -> subsystems.buddyClimb.isExtended() || subsystems.buddyClimb.isRetracted(), "buddy climb to finish moving", LEDColour.BROWN); } catch (SequenceChangedException e) { - logSub("Sequence changed while moving buddy climb"); + debug("Sequence changed while moving buddy climb"); } } @@ -314,7 +311,7 @@ private void maybeWaitForBalls(Integer expectBalls) { if (subsystems.loader.getCurrentBallCount() == expectBalls) return; - logSub("Waiting for balls"); + debug("Waiting for balls"); try { waitUntilOrAbort(() -> subsystems.loader.getCurrentBallCount() == expectBalls, "numBalls", LEDColour.MAGENTA); } catch (SequenceChangedException e) { @@ -329,7 +326,7 @@ private void maybeWaitForBalls(Integer expectBalls) { */ private void waitForTime(double endTimeSec) { if (clock.currentTime() < endTimeSec) { - //logSub("Waiting for %.1f seconds", endTimeSec - clock.currentTime()); + //debug("Waiting for %.1f seconds", endTimeSec - clock.currentTime()); } waitUntil(() -> clock.currentTime() > endTimeSec, "time", LEDColour.WHITE); } @@ -355,7 +352,7 @@ private void waitUntilOrAbort(BooleanSupplier func, String name, LEDColour debug } double now = clock.currentTime(); if (now > nextLogTimeSec) { - logSub("Controller waiting on %s, has waited %fs so far", name, now - startTimeSec); + debug("Controller waiting on %s, has waited %fs so far", name, now - startTimeSec); blockedBy = name; // Update the dashboard with what the controller is waiting for. waitDurationSec *= 2; nextLogTimeSec = now + waitDurationSec; @@ -365,7 +362,7 @@ private void waitUntilOrAbort(BooleanSupplier func, String name, LEDColour debug blockedBy = ""; if (clock.currentTime() - nextLogTimeSec > 1) { // Print a final message. - logSub("Controller done waiting on %s, after %fs", name, clock.currentTime() - startTimeSec); + debug("Controller done waiting on %s, after %fs", name, clock.currentTime() - startTimeSec); } } @@ -384,7 +381,7 @@ private void waitUntil(BooleanSupplier func, String name, LEDColour debugColour) while (!func.getAsBoolean()) { double now = clock.currentTime(); if (now > nextLogTimeSec) { - logSub("Controller waiting on %s, has waited %fs so far", name, now - startTimeSec); + debug("Controller waiting on %s, has waited %fs so far", name, now - startTimeSec); blockedBy = name; // Update the dashboard with what the controller is waiting for. waitDurationSec *= 2; nextLogTimeSec = now + waitDurationSec; @@ -394,7 +391,7 @@ private void waitUntil(BooleanSupplier func, String name, LEDColour debugColour) blockedBy = ""; if (clock.currentTime() - nextLogTimeSec > 1) { // Print a final message. - logSub("Controller done waiting on %s, after %fs", name, clock.currentTime() - startTimeSec); + debug("Controller done waiting on %s, after %fs", name, clock.currentTime() - startTimeSec); } } @@ -405,15 +402,15 @@ public SequenceChangedException() { } } - private void logSub(String message, Object... args) { + private void debug(String message, Object... args) { String time_str = String.format("%.3f controller: ", clock.currentTime()); - log.sub(time_str + message, args); + Log.debug(time_str + message, args); } @SuppressWarnings("unused") private void logErr(String message, Object... args) { String time_str = String.format("%.3f controller: ", clock.currentTime()); - log.error(time_str + message, args); + Log.error(time_str + message, args); } public synchronized void updateDashboard() { diff --git a/src/main/java/frc/robot/controller/Sequences.java b/src/main/java/frc/robot/controller/Sequences.java index 23de8b7..b2501b6 100644 --- a/src/main/java/frc/robot/controller/Sequences.java +++ b/src/main/java/frc/robot/controller/Sequences.java @@ -90,7 +90,7 @@ public static Sequence getDriveToWaypointSequence(double x, double y, double ang public static Sequence startSlowDriveForward() { SequenceBuilder builder = new SequenceBuilder("Slow drive forward"); - builder.then().setDrivebasePower(DRIVE_OFF_LEVEL_TWO_POWER); + builder.then().setDrivebasePower(DRIVE_SLOW_POWER); return builder.build(); } diff --git a/src/main/java/frc/robot/drive/routines/ArcadeDrive.java b/src/main/java/frc/robot/drive/routines/ArcadeDrive.java index b767c66..4573399 100644 --- a/src/main/java/frc/robot/drive/routines/ArcadeDrive.java +++ b/src/main/java/frc/robot/drive/routines/ArcadeDrive.java @@ -2,8 +2,8 @@ import org.strongback.components.Motor.ControlMode; import org.strongback.components.ui.ContinuousRange; -import frc.robot.interfaces.Log; import frc.robot.interfaces.DrivebaseInterface.DriveMotion; +import frc.robot.lib.chart.Chart; public class ArcadeDrive extends DriveRoutine { private double scale = 1; @@ -11,27 +11,25 @@ public class ArcadeDrive extends DriveRoutine { private ContinuousRange turn; private boolean squaredInputs; - public ArcadeDrive(String name, ControlMode mode, double scale, ContinuousRange move, ContinuousRange turn, - Log log) { - this(name, mode, scale, move, turn, true, log); + public ArcadeDrive(String name, ControlMode mode, double scale, ContinuousRange move, ContinuousRange turn) { + this(name, mode, scale, move, turn, true); } - public ArcadeDrive(String name, ControlMode mode, double scale, ContinuousRange move, ContinuousRange turn, boolean squaredInputs, - Log log) { - super(name, mode, log); + public ArcadeDrive(String name, ControlMode mode, double scale, ContinuousRange move, ContinuousRange turn, boolean squaredInputs) { + super(name, mode); this.scale = scale; this.move = move; this.turn = turn; this.squaredInputs = squaredInputs; - log.register(false, () -> move.read(), "UI/%s/Move", name).register(false, () -> turn.read(), "UI/%s/Turn", - name); + Chart.register(() -> move.read(), "UI/%s/Move", name); + Chart.register(() -> turn.read(), "UI/%s/Turn", name); } @Override public DriveMotion getMotion(double leftSpeed, double rightSpeed) { double m = move.read(); double t = turn.read(); - // log.sub("%s: Move: %f, Turn: %f\n", name, m, t); + // Logger.debug("%s: Move: %f, Turn: %f\n", name, m, t); return arcadeToTank(m, t, scale, squaredInputs); } } diff --git a/src/main/java/frc/robot/drive/routines/CheesyDpadDrive.java b/src/main/java/frc/robot/drive/routines/CheesyDpadDrive.java index 149bc73..9107a8f 100644 --- a/src/main/java/frc/robot/drive/routines/CheesyDpadDrive.java +++ b/src/main/java/frc/robot/drive/routines/CheesyDpadDrive.java @@ -5,7 +5,6 @@ import org.strongback.components.Motor.ControlMode; import org.strongback.components.ui.ContinuousRange; import org.strongback.components.ui.DirectionalAxis; -import frc.robot.interfaces.Log; import frc.robot.interfaces.DrivebaseInterface.DriveMotion; import frc.robot.lib.GamepadButtonsX; @@ -36,8 +35,8 @@ public class CheesyDpadDrive extends DriveRoutine { static final double kTopHatSpeed = 0.7; public CheesyDpadDrive(String name, DirectionalAxis dPad, ContinuousRange throttle, ContinuousRange wheel, - Switch isQuickTurn, Log log) { - super(name, ControlMode.DutyCycle, log); + Switch isQuickTurn) { + super(name, ControlMode.DutyCycle); this.dPad = dPad; this.throttleCR = throttle; this.wheelCR = wheel; diff --git a/src/main/java/frc/robot/drive/routines/ConstantDrive.java b/src/main/java/frc/robot/drive/routines/ConstantDrive.java index ea7dcb3..f95a8b9 100644 --- a/src/main/java/frc/robot/drive/routines/ConstantDrive.java +++ b/src/main/java/frc/robot/drive/routines/ConstantDrive.java @@ -1,6 +1,5 @@ package frc.robot.drive.routines; -import frc.robot.interfaces.Log; import frc.robot.interfaces.DrivebaseInterface.DriveMotion; import frc.robot.interfaces.DrivebaseInterface.DriveRoutineParameters; import org.strongback.components.Motor.ControlMode; @@ -11,8 +10,8 @@ public class ConstantDrive extends DriveRoutine { private DriveMotion motion = new DriveMotion(0, 0); - public ConstantDrive(String name, ControlMode mode, Log log) { - super(name, mode, log); + public ConstantDrive(String name, ControlMode mode) { + super(name, mode); } @Override diff --git a/src/main/java/frc/robot/drive/routines/CurvatureDrive.java b/src/main/java/frc/robot/drive/routines/CurvatureDrive.java index f321f71..dced9ba 100644 --- a/src/main/java/frc/robot/drive/routines/CurvatureDrive.java +++ b/src/main/java/frc/robot/drive/routines/CurvatureDrive.java @@ -3,7 +3,6 @@ import org.strongback.components.Switch; import org.strongback.components.Motor.ControlMode; import org.strongback.components.ui.ContinuousRange; -import frc.robot.interfaces.Log; import frc.robot.interfaces.DrivebaseInterface.DriveMotion; import frc.robot.lib.MathUtil; @@ -20,9 +19,8 @@ public class CurvatureDrive extends DriveRoutine { public static final double DEFAULT_MAXIMUM_SPEED = 1.0; private static final double SENSITIVITY_TURN = 1.0; - public CurvatureDrive(ContinuousRange throttle, ControlMode mode, ContinuousRange wheel, Switch isQuickTurn, - Log log) { - super("Curvature", mode, log); + public CurvatureDrive(ContinuousRange throttle, ControlMode mode, ContinuousRange wheel, Switch isQuickTurn) { + super("Curvature", mode); this.throttleCR = throttle; this.wheelCR = wheel; this.isQuickTurn = isQuickTurn; diff --git a/src/main/java/frc/robot/drive/routines/DriveRoutine.java b/src/main/java/frc/robot/drive/routines/DriveRoutine.java index 10a97aa..c2e751c 100644 --- a/src/main/java/frc/robot/drive/routines/DriveRoutine.java +++ b/src/main/java/frc/robot/drive/routines/DriveRoutine.java @@ -2,7 +2,6 @@ import org.strongback.components.Motor.ControlMode; -import frc.robot.interfaces.Log; import frc.robot.interfaces.DrivebaseInterface.DriveMotion; import frc.robot.interfaces.DrivebaseInterface.DriveRoutineParameters; import frc.robot.lib.MathUtil; @@ -15,11 +14,9 @@ public abstract class DriveRoutine { protected String name; protected ControlMode mode; - protected Log log; - protected DriveRoutine(String name, ControlMode mode, Log log) { + protected DriveRoutine(String name, ControlMode mode) { this.name = name; - this.log = log; } /* diff --git a/src/main/java/frc/robot/drive/routines/PositionalPIDDrive.java b/src/main/java/frc/robot/drive/routines/PositionalPIDDrive.java index 2fc1b7c..d22d623 100644 --- a/src/main/java/frc/robot/drive/routines/PositionalPIDDrive.java +++ b/src/main/java/frc/robot/drive/routines/PositionalPIDDrive.java @@ -6,9 +6,9 @@ import org.strongback.components.Clock; import org.strongback.components.Motor.ControlMode; -import frc.robot.interfaces.Log; import frc.robot.interfaces.DrivebaseInterface.DriveMotion; import frc.robot.interfaces.DrivebaseInterface.DriveRoutineParameters; +import frc.robot.lib.log.Log; import frc.robot.Constants; import frc.robot.drive.util.LowPassFilter; import frc.robot.drive.util.PositionPID; @@ -31,11 +31,11 @@ public class PositionalPIDDrive extends DriveRoutine { public PositionalPIDDrive(String name, BooleanSupplier finished, DoubleSupplier targetSpeed, DoubleSupplier targetTurn, double speedScale, double turnScale, double maxJerk, DoubleSupplier leftDistance, - DoubleSupplier leftSpeed, DoubleSupplier rightDistance, DoubleSupplier rightSpeed, Clock clock, Log log) { - super(name, ControlMode.DutyCycle, log); + DoubleSupplier leftSpeed, DoubleSupplier rightDistance, DoubleSupplier rightSpeed, Clock clock) { + super(name, ControlMode.DutyCycle); this.finished = finished; this.clock = clock; - log.info("Starting to drive positional PID"); + Log.info("Starting to drive positional PID"); /* * There is an issue here. If the targetSpeed increases too rapidly, then even @@ -48,22 +48,22 @@ public PositionalPIDDrive(String name, BooleanSupplier finished, DoubleSupplier LowPassFilter filteredSpeed = new LowPassFilter(targetSpeed, 0.2); leftPID = createPID("Drive/" + name + "/left", () -> { return speedScale * filteredSpeed.getAsDouble() + turnScale * targetTurn.getAsDouble(); - }, maxJerk, leftDistance, leftSpeed, clock, log); + }, maxJerk, leftDistance, leftSpeed, clock); rightPID = createPID("Drive/" + name + "/right", () -> { return speedScale * filteredSpeed.getAsDouble() - turnScale * targetTurn.getAsDouble(); - }, maxJerk, rightDistance, rightSpeed, clock, log); + }, maxJerk, rightDistance, rightSpeed, clock); } public PositionalPIDDrive(String name, DoubleSupplier targetSpeed, DoubleSupplier targetTurn, double speedScale, double turnScale, double maxJerk, DoubleSupplier leftDistance, DoubleSupplier leftSpeed, - DoubleSupplier rightDistance, DoubleSupplier rightSpeed, Clock clock, Log log) { + DoubleSupplier rightDistance, DoubleSupplier rightSpeed, Clock clock) { this(name, () -> true, targetSpeed, targetTurn, speedScale, turnScale, maxJerk, leftDistance, leftSpeed, - rightDistance, rightSpeed, clock, log); + rightDistance, rightSpeed, clock); } private PositionPID createPID(String name, DoubleSupplier targetSpeed, double maxJerk, DoubleSupplier distance, - DoubleSupplier speed, Clock clock, Log log) { - PositionPID pid = new PositionPID(name, targetSpeed, maxJerk, distance, speed, clock, log); + DoubleSupplier speed, Clock clock) { + PositionPID pid = new PositionPID(name, targetSpeed, maxJerk, distance, speed, clock); double kV = 0.3; double kA = 0, kI = 0, kD = 0;// 0.5; double kP = 0.64; @@ -98,7 +98,7 @@ public DriveMotion getMotion(double leftSpeed, double rightSpeed) { if (!finished.getAsBoolean()) { timestamp = clock.currentTime(); } - log.sub("%s: left=%f right=%f", name, leftPower, rightPower); + Log.debug("%s: left=%f right=%f", name, leftPower, rightPower); return new DriveMotion(leftPower, rightPower); } diff --git a/src/main/java/frc/robot/drive/routines/TrajectoryDrive.java b/src/main/java/frc/robot/drive/routines/TrajectoryDrive.java index cbb8ca7..a93da1c 100644 --- a/src/main/java/frc/robot/drive/routines/TrajectoryDrive.java +++ b/src/main/java/frc/robot/drive/routines/TrajectoryDrive.java @@ -18,8 +18,9 @@ import frc.robot.Constants; import frc.robot.interfaces.DrivebaseInterface.DriveMotion; import frc.robot.interfaces.DrivebaseInterface.DriveRoutineParameters; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; import frc.robot.interfaces.LocationInterface; -import frc.robot.interfaces.Log; /** * Walks the drivebase through a supplied list of waypoints. @@ -57,11 +58,9 @@ public class TrajectoryDrive extends DriveRoutine { private Boolean enabled = false; private Clock clock; - private Log log; - public TrajectoryDrive(LocationInterface location, Clock clock, Log log) { - super("Trajectory Drive", ControlMode.Voltage, log); - this.log = log; + public TrajectoryDrive(LocationInterface location, Clock clock) { + super("Trajectory Drive", ControlMode.Voltage); this.clock = clock; m_startTime = clock.currentTime(); m_pose = location::getPose; @@ -75,26 +74,26 @@ public TrajectoryDrive(LocationInterface location, Clock clock, Log log) { m_leftController = new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0); m_rightController = new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0); - log.register(true, () -> clock.currentTime() - m_startTime, "TrajectoryDrive/elapsedTime") - .register(true, () -> m_trajectory == null ? 0 : m_trajectory.getTotalTimeSeconds(), "TrajectoryDrive/totalTime") - .register(true, () -> m_leftSpeedSetpoint, "TrajectoryDrive/trajectory/speed/leftSepoint") - .register(true, () -> m_rightSpeedSetpoint, "TrajectoryDrive/trajectory/speed/rightSepoint") - .register(true, () -> m_leftSpeedError, "TrajectoryDrive/trajectory/speed/leftError") - .register(true, () -> m_rightSpeedError, "TrajectoryDrive/trajectory/speed/rightError") - .register(true, () -> m_leftPIDResult, "TrajectoryDrive/trajectory/speed/leftPIDResult") - .register(true, () -> m_rightPIDResult, "TrajectoryDrive/trajectory/speed/rightPIDResult") - .register(true, () -> m_leftOutput, "TrajectoryDrive/leftOutput") - .register(true, () -> m_rightOutput, "TrajectoryDrive/rightOutput") - .register(true, () -> m_targetSpeed, "TrajectoryDrive/targetSpeed") - .register(true, () -> m_targetPose.getTranslation().getX(), "TrajectoryDrive/desired/x") - .register(true, () -> m_targetPose.getTranslation().getY(), "TrajectoryDrive/desired/y") - .register(true, () -> m_targetPose.getRotation().getDegrees(), "TrajectoryDrive/desired/a") - .register(true, () -> m_actualPose.getTranslation().getX(), "TrajectoryDrive/actual/x") - .register(true, () -> m_actualPose.getTranslation().getY(), "TrajectoryDrive/actual/y") - .register(true, () -> m_actualPose.getRotation().getDegrees(), "TrajectoryDrive/actual/a") - .register(true, () -> m_errorPose.getTranslation().getX(), "TrajectoryDrive/error/x") - .register(true, () -> m_errorPose.getTranslation().getY(), "TrajectoryDrive/error/y") - .register(true, () -> m_errorPose.getRotation().getDegrees(), "TrajectoryDrive/error/a"); + Chart.register(() -> clock.currentTime() - m_startTime, "TrajectoryDrive/elapsedTime"); + Chart.register(() -> m_trajectory == null ? 0 : m_trajectory.getTotalTimeSeconds(), "TrajectoryDrive/totalTime"); + Chart.register(() -> m_leftSpeedSetpoint, "TrajectoryDrive/trajectory/speed/leftSepoint"); + Chart.register(() -> m_rightSpeedSetpoint, "TrajectoryDrive/trajectory/speed/rightSepoint"); + Chart.register(() -> m_leftSpeedError, "TrajectoryDrive/trajectory/speed/leftError"); + Chart.register(() -> m_rightSpeedError, "TrajectoryDrive/trajectory/speed/rightError"); + Chart.register(() -> m_leftPIDResult, "TrajectoryDrive/trajectory/speed/leftPIDResult"); + Chart.register(() -> m_rightPIDResult, "TrajectoryDrive/trajectory/speed/rightPIDResult"); + Chart.register(() -> m_leftOutput, "TrajectoryDrive/leftOutput"); + Chart.register(() -> m_rightOutput, "TrajectoryDrive/rightOutput"); + Chart.register(() -> m_targetSpeed, "TrajectoryDrive/targetSpeed"); + Chart.register(() -> m_targetPose.getTranslation().getX(), "TrajectoryDrive/desired/x"); + Chart.register(() -> m_targetPose.getTranslation().getY(), "TrajectoryDrive/desired/y"); + Chart.register(() -> m_targetPose.getRotation().getDegrees(), "TrajectoryDrive/desired/a"); + Chart.register(() -> m_actualPose.getTranslation().getX(), "TrajectoryDrive/actual/x"); + Chart.register(() -> m_actualPose.getTranslation().getY(), "TrajectoryDrive/actual/y"); + Chart.register(() -> m_actualPose.getRotation().getDegrees(), "TrajectoryDrive/actual/a"); + Chart.register(() -> m_errorPose.getTranslation().getX(), "TrajectoryDrive/error/x"); + Chart.register(() -> m_errorPose.getTranslation().getY(), "TrajectoryDrive/error/y"); + Chart.register(() -> m_errorPose.getRotation().getDegrees(), "TrajectoryDrive/error/a"); } /** @@ -178,7 +177,7 @@ synchronized public DriveMotion getMotion(double leftSpeed, double rightSpeed) { @Override public boolean hasFinished() { if (clock.currentTime() - m_startTime > m_trajectory.getTotalTimeSeconds()){ - log.sub("%s: Finished spline with %s error", getName(), m_errorPose); + Log.debug("%s: Finished spline with %s error", getName(), m_errorPose); return true; } return false; diff --git a/src/main/java/frc/robot/drive/util/PositionCalc.java b/src/main/java/frc/robot/drive/util/PositionCalc.java index 8c0e974..859e52e 100644 --- a/src/main/java/frc/robot/drive/util/PositionCalc.java +++ b/src/main/java/frc/robot/drive/util/PositionCalc.java @@ -2,8 +2,6 @@ import org.strongback.components.Clock; -import frc.robot.interfaces.Log; - /** * Takes a target speed for a wheel and calculates the expected encoder * position had the wheel been doing the target speed. @@ -19,16 +17,14 @@ public class PositionCalc { private double maxJerk; private Clock clock; @SuppressWarnings("unused") - private Log log; private double lastTime; - public PositionCalc(double initialPosition, double initialSpeed, double maxJerk, Clock clock, Log log) { + public PositionCalc(double initialPosition, double initialSpeed, double maxJerk, Clock clock) { this.position = initialPosition; this.speed = this.targetSpeed = initialSpeed; this.accel = 0; this.maxJerk = maxJerk; this.clock = clock; - this.log = log; lastTime = clock.currentTime(); } @@ -54,8 +50,8 @@ public double update() { // Update the position based on the average speed over the last period. double averagespeed = (speed + lastSpeed) / 2; position += dtSec * averagespeed; - //log.sub("calc speed = %f", speed); - //log.sub("calc pos = %f", position); + //Logger.debug("calc speed = %f", speed); + //Logger.debug("calc pos = %f", position); return position; } @@ -65,7 +61,7 @@ public double update() { */ public void setTargetSpeed(double speed) { this.targetSpeed = speed; - //log.sub("target speed = %f", targetSpeed); + //Logger.debug("target speed = %f", targetSpeed); } public double getPosition() { diff --git a/src/main/java/frc/robot/drive/util/PositionPID.java b/src/main/java/frc/robot/drive/util/PositionPID.java index 7bc7b37..b27a6f0 100644 --- a/src/main/java/frc/robot/drive/util/PositionPID.java +++ b/src/main/java/frc/robot/drive/util/PositionPID.java @@ -3,7 +3,7 @@ import java.util.function.DoubleSupplier; import org.strongback.components.Clock; -import frc.robot.interfaces.Log; +import frc.robot.lib.chart.Chart; /** * Using a PositionCalc, takes a target speed and calculates what the encoder @@ -31,14 +31,14 @@ public class PositionPID { private boolean enabled = true; public PositionPID(String name, DoubleSupplier targetSpeed, double maxJerk, - DoubleSupplier encoderPos, DoubleSupplier encoderSpeed, Clock clock, Log log) { + DoubleSupplier encoderPos, DoubleSupplier encoderSpeed, Clock clock) { this.targetSpeed = targetSpeed; this.encoderPos = encoderPos; this.encoderSpeed = encoderSpeed; - calc = new PositionCalc(encoderPos.getAsDouble(), encoderSpeed.getAsDouble(), maxJerk, clock, log); - log.register(true, () -> calc.getSpeed(), "%s/targetSpeed", name) - .register(true, () -> calc.getPosition(), "%s/targetPos", name) - .register(true, () -> encoderPos.getAsDouble(), "%s/actualPos", name); + calc = new PositionCalc(encoderPos.getAsDouble(), encoderSpeed.getAsDouble(), maxJerk, clock); + Chart.register(() -> calc.getSpeed(), "%s/targetSpeed", name); + Chart.register(() -> calc.getPosition(), "%s/targetPos", name); + Chart.register(() -> encoderPos.getAsDouble(), "%s/actualPos", name); } public void setVAPID(double kV, double kA, double kP, double kI, double kD) { diff --git a/src/main/java/frc/robot/interfaces/Log.java b/src/main/java/frc/robot/interfaces/Log.java deleted file mode 100644 index ac4a7a4..0000000 --- a/src/main/java/frc/robot/interfaces/Log.java +++ /dev/null @@ -1,192 +0,0 @@ -package frc.robot.interfaces; - -import java.util.function.DoubleSupplier; -import java.util.function.IntSupplier; - -import org.strongback.Executable; -import org.strongback.components.Switch; -import org.strongback.components.ui.DirectionalAxis; - -/** - * This is the interface for the logging subsystem. - * - * The logging subsystem has to implement two main functions: - * 1) a text based log that allows us to log events, and - * 2) a graphing log system that allows us to build a graph of tracked elements. - * - * The text based log is a pure push log. Code calls its methods to write out events we wish to record. - * - * The graphing log system has a set of elements registered with it, and will regularly query those - * elements for their current values. These elements should be written out in such a way that they can be - * viewed as a graph by the user. - */ - -public interface Log extends Executable { - /** - * Pause the writing of records to the graphing log. If the robot is quiescent writing out log records just - * takes up space and slows down the viewing of logs. - */ - public Log pauseGraphLog(); - - /** - * resume logging after a pause. - */ - public Log resumeGraphLog(); - -//################ TXT file methods ################################################################################ - - /* - * Methods that handle writing to the .TXT log file. We allow freeform messages - * to be added to the log file. We precede each message with a time stamp and message type. - */ - - /** - * Logs to the external logger only. Generic log messages - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log logMessage(String message, Object... args); - - /** - * Logs to the external logger only. For high volume debug messages - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log debug(String message, Object... args); - - /** - * Logs to the external logger and console. For low volume informational messages (<1/sec). - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log info(String message, Object... args); - - /** - * Logs to the external logger and console. For warning Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log warning(String message, Object... args); - - /** - * Logs to the external logger and console. For important errors Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log error(String message, Object... args); - - /** - * Logs Exceptions to the external logger and console. - * @param message Message to Log - * @param e Exception to log after message - */ - public Log exception(String message, Exception e); - - /** - * Implementation of the familiar println interface. A single string is passed, and is added to the log - * with a terminating new line. - * @param message Message to add to the log - */ - public Log println(String message); - - /** - * Logs to the external logger and console. For important console Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log console(String message, Object... args); - - /** - * Logs to the external logger. For command execution Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log cmd(String message, Object... args); - - /** - * Logs to the external logger and console. For important subsystem Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log sub(String message, Object... args); - - /** - * Logs to the external logger and console. For important controller Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log ctrl(String message, Object... args); - - //################ Log data collection methods ################################################################################ - - /** - * Register a data stream for collection to the log. - * The sample needs to have a method that returns a double which is the data element. - * The format (and args) are used to construct the name of the data stream. - * @param sample An object that returns a double value when called - * @param format a VarArgs string which evaluates to the name of the data stream - * @param args Any arguments required for the format varargs - * @return the log itself to allow chaining of the register methods. - */ - public default Log register(boolean local, DoubleSupplier sample, String format, Object... args) { - return doRegister(local, sample, format, args); - } - - /** - * Register a data stream for collection to the log. - * The sample needs to have a method that returns an integer which is the data element. - * The format (and args) are used to construct the name of the data stream. - * @param sample An object that returns an integer value when called - * @param format a VarArgs string which evaluates to the name of the data stream - * @param args Any arguments required for the format varargs - * @return the log itself to allow chaining of the register methods. - */ - - public default Log register(boolean local, IntSupplier sample, String format, Object... args) { - return doRegister(local, () -> (double)sample.getAsInt(), format, args); - } - - /** - * Register a data stream for collection to the log. - * The sample needs to have a method that returns a DirectionalAxis which is the data element. - * The format (and args) are used to construct the name of the data stream. - * @param sample An object that returns a DirectionalAxis value when called - * @param format a VarArgs string which evaluates to the name of the data stream - * @param args Any arguments required for the format varargs - * @return the log itself to allow chaining of the register methods. - */ - public default Log register(boolean local, DirectionalAxis sample, String format, Object... args) { - return doRegister(local, () -> sample.getDirection(), format, args); - } - - /** - * Register a data stream for collection to the log. - * The sample needs to have a method that returns a Switch which is the data element. - * The format (and args) are used to construct the name of the data stream. - * @param sample An object that returns a Switch value when called - * @param format a VarArgs string which evaluates to the name of the data stream - * @param args Any arguments required for the format varargs - * @return the log itself to allow chaining of the register methods. - */ - public default Log register(boolean local, Switch sample, String format, Object... args) { - return doRegister(local, () -> (double)(sample.isTriggered()?1:0), format, args); - } - - /* - * Register the actual data stream. In preference one of the register() methods should be used - */ - public Log doRegister(boolean local, DoubleSupplier sample, String format, Object... args); - - /** - * Tells the logging subsystem that all registration events has occurred. - * Registration must be completed before we can start writing out logging records as the first record - * must be a heading record which requires all the elements to be queried - */ - public Log logCompletedElements(String matchDescription); - - /** - * Flush any outstanding data for the log files - * @return this for chaining, never null. - */ - public Log flush(); -} diff --git a/src/main/java/frc/robot/interfaces/LogWriter.java b/src/main/java/frc/robot/interfaces/LogWriter.java new file mode 100644 index 0000000..6514b94 --- /dev/null +++ b/src/main/java/frc/robot/interfaces/LogWriter.java @@ -0,0 +1,33 @@ +package frc.robot.interfaces; + +/** + * Simple log writer. + */ + +public interface LogWriter { + + /** + * Writes a single line to a log file. + * @param message Message to write to the log. + */ + public void write(String message); + + /** + * Flush any writes to disk. + */ + public void flush(); + + /** + * Close the file and clean up. + */ + public void close(); + + /** + * Create symbolic links to the file. Used to create Latest_x and dated + * symlinks. This version also takes a directory. + * + * @param dir sub directory relative to logging base dir to put link in. + * @param prefix as in _chart.html + */ + public void createSymbolicLink(String dir, String prefix); +} diff --git a/src/main/java/frc/robot/lib/ConfigReader.java b/src/main/java/frc/robot/lib/ConfigReader.java new file mode 100644 index 0000000..6bdb526 --- /dev/null +++ b/src/main/java/frc/robot/lib/ConfigReader.java @@ -0,0 +1,262 @@ +package frc.robot.lib; + +import static frc.robot.Constants.*; + +import java.io.BufferedReader; +import java.io.BufferedWriter; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.NoSuchFileException; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.nio.file.StandardOpenOption; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Collections; +import java.util.HashMap; +import java.util.Map; +import java.util.TreeMap; +import java.util.stream.Collectors; + +import frc.robot.Constants; +import frc.robot.lib.log.Log; + +/** + * Class responsible for reading parameters to control behaviour of robot hardware. + * (e.g. if subsystems are present or not) It reads from a text file + * Currently the supported types are String, int, double, boolean and int array. + * + * Example lines: + * drivebase/present = true + * drivebase/rampRate = 0.13125 + * drivebase/right/canIDs/withEncoders = 7,6 + * drivebase/right/canIDs/withoutEncoders = 5 + */ +public class ConfigReader { + + private String name = "RobotConfig"; + private String filePath; + private Map lines; + private Map ignoredEntries; // Lines/entries not used in the config file. + private Map nonDefaultParameters; // Non-default values used from the config file. + private ArrayList exampleText = new ArrayList(); + + public ConfigReader() { + this(Constants.CONFIG_FILE_PATH); + } + + public ConfigReader(String filePath) { + this.filePath = filePath; + + readLines(Paths.get(filePath)); + } + + /** + * Needs to be called after the config is loaded to write out an example config + * file and to print out details about the config file. + */ + public void finishLoadingConfig() { + Collections.sort(exampleText); + writeExampleFile(filePath, String.join("\n", exampleText)); + + if (!ignoredEntries.isEmpty()) { + Log.warning("WARNING: These config file lines weren't used:"); + for (String entry : ignoredEntries.values()) { + Log.warning(" %s", entry); + } + } + if (!nonDefaultParameters.isEmpty()) { + Log.warning("WARNING: These parameters have non-default values:"); + for (Map.Entry entry : nonDefaultParameters.entrySet()) { + Log.warning(" %s = %s", entry.getKey(), entry.getValue()); + } + } + Log.info("RobotConfig finished loading parameters\n"); + } + + private void readLines(Path path) { + Log.info("Reading config file " + path); + lines = new HashMap(); + ignoredEntries = new TreeMap(); + nonDefaultParameters = new TreeMap(); + try (BufferedReader reader = Files.newBufferedReader(path)) { + String line = null; + while ((line = reader.readLine()) != null) { + String[] parts = line.split("\\s*=\\s*", -1); // Keep empty values + if (parts.length < 2) { + Log.error("Bad config line " + line); + continue; + } + String tag = parts[0].trim(); + String value = parts[1].trim(); + if (lines.containsKey(tag)) { + Log.error("ERROR: Duplicate tag %s in configuration file, last value will be used.", tag); + } + lines.put(tag, value); + ignoredEntries.put(parts[0].trim(), line); + } + } catch (NoSuchFileException e) { + Log.error("Config file %s not found, attempting to create it", path); + // Touch the file so at least it's there next time. + try { + Files.createFile(path); + } catch (IOException e1) { + } + } catch (IOException e) { + Log.exception("Error loading configuration file " + path + ", using defaults", e); + } + } + + private void writeExampleFile(String filePath, String contents) { + Path exampleFile = Paths.get(filePath + ".example"); + try { + BufferedWriter writer; + writer = Files.newBufferedWriter(exampleFile, StandardOpenOption.CREATE); + writer.write(contents + "\n"); + writer.close(); + Log.info("Wrote example config file " + exampleFile.toString()); + } catch (IOException e) { + Log.exception("Unable to write example config file " + exampleFile.toString(), e); + } + } + + public String getMotorControllerType(String parameterName, String defaultValue) { + String type = getString(parameterName, defaultValue); + switch (type) { + default: + Log.error("Invalid value '%s' for parameter '%s'. Using %s.", type, parameterName, DEFAULT_MOTOR_CONTROLLER_TYPE); + return DEFAULT_MOTOR_CONTROLLER_TYPE; + + case MOTOR_CONTROLLER_TYPE_TALONSRX: + return MOTOR_CONTROLLER_TYPE_TALONSRX; + + case MOTOR_CONTROLLER_TYPE_SPARKMAX: + return MOTOR_CONTROLLER_TYPE_SPARKMAX; + } + } + + private void appendExample(String key, T defaultValue) { + exampleText.add(key + " = " + defaultValue); + } + + public int getInt(String key, int defaultValue) { + appendExample(key, defaultValue); + try { + if (lines.containsKey(key)) { + int value = Integer.valueOf(lines.get(key)); + ignoredEntries.remove(key); // Used this line. + Log.debug("%s: %s -> %d", name, key, value); + if (value != defaultValue) { + nonDefaultParameters.put(key, lines.get(key)); + } + return value; + } + } catch (Exception e) { + Log.exception("Error reading key: " + key + " using default", e); + } + return defaultValue; + } + + public double getDouble(String key, double defaultValue) { + appendExample(key, defaultValue); + try { + if (lines.containsKey(key)) { + double value = Double.valueOf(lines.get(key)); + ignoredEntries.remove(key); // Used this line. + Log.debug("%s: %s -> %f", name, key, value); + if (value != defaultValue) { + nonDefaultParameters.put(key, lines.get(key)); + } + return value; + } + } catch (Exception e) { + Log.exception("Error reading key: " + key + " using default", e); + } + return defaultValue; + } + + public PIDF getPIDF(String prefix, PIDF pidf) { + pidf.p = getDouble(prefix + "/p", pidf.p); + pidf.i = getDouble(prefix + "/i", pidf.i); + pidf.d = getDouble(prefix + "/d", pidf.d); + pidf.f = getDouble(prefix + "/f", pidf.f); + return pidf; + } + + public boolean getBoolean(String key, boolean defaultValue) { + appendExample(key, defaultValue); + try { + if (lines.containsKey(key)) { + boolean value = Boolean.valueOf(lines.get(key)); + ignoredEntries.remove(key); // Used this line. + Log.debug("%s: %s -> %s", name, key, value); + if (value != defaultValue) { + nonDefaultParameters.put(key, lines.get(key)); + } + return value; + } + } catch (Exception e) { + Log.exception("Error reading key: " + key + " using default", e); + } + return defaultValue; + } + + public String getString(String key, String defaultValue) { + appendExample(key, "\"" + defaultValue + "\""); + try { + if (lines.containsKey(key)) { + // Get the value between the quotes. + String[] parts = lines.get(key).split("\"", -1); + if (parts.length < 3) { + Log.error("Bad string value for %s, needs to be in double quotes, not: %s", key, lines.get(key)); + return defaultValue; + } + String value = parts[1]; + ignoredEntries.remove(key); // Used this line. + Log.debug("%s: %s -> %s", name, key, value); + if (!value.equals(defaultValue)) { + nonDefaultParameters.put(key, lines.get(key)); + } + return value; + } + } catch (Exception e) { + Log.exception("Error reading key: " + key + " using default", e); + } + return defaultValue; + } + + public int[] getIntArray(String key, int[] defaultValue) { + // Joining primitive arrays seems to be painful under Java. + appendExample(key, joinIntArray(defaultValue)); + try { + if (lines.containsKey(key)) { + String value = lines.get(key); + int[] values; + if (value.equals("")) { + // No values. + values = new int[0]; + } else { + // One or more values. + String[] parts = value.split("\\s*,\\s*"); + values = new int[parts.length]; + for (int i = 0; i < parts.length; i++) { + values[i] = Integer.valueOf(parts[i]); + } + } + ignoredEntries.remove(key); // Used this line. + Log.debug("%s: %s -> %s", name, key, joinIntArray(values)); + if (!java.util.Arrays.equals(values, defaultValue)) { + nonDefaultParameters.put(key, lines.get(key)); + } + return values; + } + } catch (Exception e) { + Log.exception("Error reading key: " + key + " using default", e); + } + return defaultValue; + } + + private static String joinIntArray(int[] values) { + return Arrays.stream(values).mapToObj(String::valueOf).collect(Collectors.joining(",")); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/Jevois.java b/src/main/java/frc/robot/lib/Jevois.java index 3e0bd68..f51b812 100644 --- a/src/main/java/frc/robot/lib/Jevois.java +++ b/src/main/java/frc/robot/lib/Jevois.java @@ -27,7 +27,7 @@ import com.fazecast.jSerialComm.*; import frc.robot.interfaces.JevoisInterface; -import frc.robot.interfaces.Log; +import frc.robot.lib.log.Log; public class Jevois implements JevoisInterface { @@ -159,7 +159,6 @@ public String toString() { } } - private Log log; private static final Mode webcamMode; private static final Mode visionMode; @@ -204,29 +203,28 @@ HSV ranges that seemed to work (Mark: October 2019) * * @throws IOException */ - public Jevois(Log log) throws IOException { - this.log = log; + public Jevois() throws IOException { SerialPort[] ports = SerialPort.getCommPorts(); for (int i = 0; i < ports.length; i++) { SerialPort port = ports[i]; - log.info("Found port %s\n", port.getDescriptivePortName()); + Log.info("Found port %s\n", port.getDescriptivePortName()); if (port.getDescriptivePortName().startsWith("JeVois")) { port.openPort(); - log.info("Found camera %s on %s\n", port.getDescriptivePortName(), port.getSystemPortName()); + Log.info("Found camera %s on %s\n", port.getDescriptivePortName(), port.getSystemPortName()); // Don't block too long in case a command needs to be sent. port.setComPortTimeouts(SerialPort.TIMEOUT_READ_SEMI_BLOCKING, 10, 0); istream = port.getInputStream(); ostream = port.getOutputStream(); connected = true; - log.info(issueCommand("listmappings")); + Log.info(issueCommand("listmappings")); setCameraMode(CameraMode.VISION); // Restore any mode if one has been set. - log.info(issueCommand("info")); + Log.info(issueCommand("info")); // Turn on the serial output over USB - log.info(issueCommand("setpar serout USB")); + Log.info(issueCommand("setpar serout USB")); return; } } - log.error("Failed to find JeVois camera, is it plugged in to a USB port and has an orange light?"); + Log.error("Failed to find JeVois camera, is it plugged in to a USB port and has an orange light?"); } public boolean isConnected() { @@ -245,19 +243,19 @@ public void setCameraMode(CameraMode mode) throws IOException { setParameters(visionMode); break; default: - log.error("Jevois: Unsupported vision mode %s\n", mode.toString()); + Log.error("Jevois: Unsupported vision mode %s\n", mode.toString()); } } private void setParameters(Mode mode) throws IOException { if (!isConnected()) return; // No connection, give up. - log.info("Setting %s camera mode.\n", mode.name); + Log.info("Setting %s camera mode.\n", mode.name); if (!mode.modeString.isEmpty()) { - log.info(issueCommand(mode.modeString)); + Log.info(issueCommand(mode.modeString)); } for (String command : mode.toString().split("\n")) { - log.info(issueCommand(command)); + Log.info(issueCommand(command)); } } @@ -278,14 +276,14 @@ private void setParameters(Mode mode) throws IOException { @Override public String readLine() throws IOException { if (!isConnected()){ - log.sub("CAMERA NOT CONNECTED"); + Log.debug("CAMERA NOT CONNECTED"); throw new IOException("No camera connected"); // No connection, give up. } StringBuffer line = new StringBuffer(200); while (true) { synchronized (this) { - //log.sub(line.toString()); + //Logger.debug(line.toString()); try { int b = istream.read(); if (b < 0) { @@ -306,7 +304,7 @@ public String readLine() throws IOException { } line.append((char) b); } catch (SerialPortTimeoutException e) { - log.sub("Jevois: There is serial port timeout!!!!!!!!!"); + Log.debug("Jevois: There is serial port timeout!!!!!!!!!"); // Give up reading in case a command needs to be sent. } } @@ -325,7 +323,7 @@ public synchronized String issueCommand(String command) throws IOException { if (!isConnected()) return "ERR: JeVois not connected"; - log.info(command); + Log.info(command); ostream.write(command.getBytes()); String newline = "\n"; ostream.write(newline.getBytes()); diff --git a/src/main/java/frc/robot/lib/LEDColour.java b/src/main/java/frc/robot/lib/LEDColour.java index e14a16c..f994ffd 100644 --- a/src/main/java/frc/robot/lib/LEDColour.java +++ b/src/main/java/frc/robot/lib/LEDColour.java @@ -19,8 +19,8 @@ public enum LEDColour { public final int b; LEDColour(int r, int g, int b) { - this.r = (int)(r * Constants.LED_PERCENTAGE); - this.g = (int)(g * Constants.LED_PERCENTAGE); - this.b = (int)(b * Constants.LED_PERCENTAGE); + this.r = (int)(r * Constants.LED_BRIGHTNESS_PERCENTAGE); + this.g = (int)(g * Constants.LED_BRIGHTNESS_PERCENTAGE); + this.b = (int)(b * Constants.LED_BRIGHTNESS_PERCENTAGE); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/LogGraph.java b/src/main/java/frc/robot/lib/LogGraph.java deleted file mode 100644 index 1476c16..0000000 --- a/src/main/java/frc/robot/lib/LogGraph.java +++ /dev/null @@ -1,607 +0,0 @@ -package frc.robot.lib; - -import java.io.BufferedReader; - -/* - * This class controls all logging for the system. - * - * It as two main functions: - * 1) a textual log which we can append to easily and quickly, and - * 2) a graphical log which subsystems have to register with and will be polled for current values for registered entries. - * - * A series of files are created: They are created in LOG_BASE_PATH and sub directories LOG_BASE_DATA_PATH and LOG_BASE_DATE_PATH - * The original files are created using an incrementing instance number (The current instance number is held in the base - * directory of the log area). They are linked to using a time stamp. - * - * We can pause, continue, or restart the logging. - * restarting the logging causes the current files to be closed and a new set of files to be created. - * - * For text logs we have a series of calls: - * log.{logMessage,debug,info,warning,error,ctrl,sub}(String message) - * Each message takes a varargs argument list, and prepends the message with a timestamp and type of message. - * - * For graphical logs the client class creates an arraylist of type LogGraphEntry and populates it with the entries that should be logged. - * At regular intervals the logging system will walk through this list and obtain the current value for each entry. - * These are added to the current graphical log file. - */ - -import java.io.BufferedWriter; -import java.io.IOException; -import java.nio.file.Files; -import java.nio.file.Path; -import java.nio.file.Paths; -import java.text.SimpleDateFormat; -import java.util.ArrayList; -import java.util.Calendar; -import java.util.function.DoubleSupplier; - -import org.strongback.components.Clock; -import frc.robot.interfaces.Log; - -/** - * This is the log file data logger. We split the log into two separate log systems. - * For the text file logging we use this method. - * - * We create a single text log file /.txt - * and provide a variety of methods to append to that log file. - */ - -public class LogGraph implements Log { - - private enum GraphLogState { - INVALID, // File has not yet been created - CREATED, // File has been created, but we are waiting for all logging classes to be created - CONFIGURED, // Logging classes are all created and have registered with the logging subsystem - ACTIVE, // .html files have been populated and we are ready to write records into the .csv file. - PAUSED, // Graphical Logging has been paused as the robot isn't doing anything - ERRORED // a problem has occurred. Abandon trying to write. - } - - private class LogGraphElement { - public String name; - public DoubleSupplier sample; - - public LogGraphElement(DoubleSupplier sample, String name, Object... args) { - this.name = String.format(name, args); - this.sample = sample; - } - } - - private final String robotName; - private final String basePath; // Where the log files live. - private final String logPath; - private long logFileNumber = 0; - private final String dateDir; - private final String dataDir; - private final String latestDir; - private final String eventDir; - // Log files. - private LogFileWriter csvWriter; - private LogFileWriter logWriter; - private LogFileWriter chartWriter; - private LogFileWriter locationWriter; - // Internal state. - private GraphLogState graphLogState = GraphLogState.INVALID; - private ArrayList logGraphElements; // list of registered graph elements - private String matchDescription; - private boolean createdDateFiles; - private Clock clock; - private boolean onlyLocal = false; // only log locally defined elements. - public Double enableOffset = 0.0; - - public LogGraph(String robotName, String basePath, String logPath, String dataDir, String dateDir, String latestDir, String eventDir, boolean onlyLocal, Clock clock) { - this.robotName = robotName; - this.basePath = basePath; - this.logPath = logPath; - this.dataDir = dataDir; - this.dateDir = dateDir; - this.latestDir = latestDir; - this.eventDir = eventDir; - this.clock = clock; - this.logGraphElements = new ArrayList(); - this.onlyLocal = onlyLocal; - createdDateFiles = false; - - initLogs(); - } - - /** - * Restarts logging, called each time robot is enabled or initialised. - */ - - public void restartLogs() { - initLogs(); - graphLogState = GraphLogState.CONFIGURED; - } - - /** - * Creates new log files on request. - */ - - public void initLogs() { - try { - // Set the graphLogState to INVALID as the new files have not yet been created. - graphLogState = GraphLogState.INVALID; - - // Get time since robot boot, so chart starts at time = 0. - enableOffset = getCurrentTime(); - - // Ensure the directories exist. - Files.createDirectories(getDataPath()); - Files.createDirectories(getDatePath()); - Files.createDirectories(getLatestPath()); - Files.createDirectories(getEventPath()); - - logFileNumber = getNextLogFileNumber(); // Different number each start. - - // Open all files. Also creates Latest symlink. - var path = Paths.get(basePath, robotName).toString(); - csvWriter = new LogFileWriter("data", logFileNumber, "csv", path, dataDir); - logWriter = new LogFileWriter("log", logFileNumber, "txt", path, dataDir); - chartWriter = new LogFileWriter("chart", logFileNumber, "html", path, dataDir); - locationWriter = new LogFileWriter("location", logFileNumber, "html", path, dataDir); - - // Everything was successfully created, we're good to go. - graphLogState = GraphLogState.CREATED; - } catch (IOException e) { - e.printStackTrace(); - System.out.printf("Failed to create log files in %s: %s\n", basePath, e.getMessage()); - graphLogState = GraphLogState.ERRORED; - } - } - - - /** - * Create the date based symbolic links. These create symbolic links from date stamped - * version of the file to the actual file. This is a separate method as we delay - * creating these until we are reasonably sure the date is correct. - * @param timestamp The date to use. - */ - public void createDateFiles(Calendar timestamp) { - String timestampStr = new SimpleDateFormat("yyyyMMdd_HH-mm-ss.SSS").format(timestamp.getTime()); - try { - // Create links based on the timestamp. - csvWriter.createSymbolicLink(dateDir, timestampStr); - logWriter.createSymbolicLink(dateDir, timestampStr); - chartWriter.createSymbolicLink(dateDir, timestampStr); - locationWriter.createSymbolicLink(dateDir, timestampStr); - // And on event name, match type, match number, replay number, alliance and position. - // These details should be available at the same time now that the drivers station is - // able to talk to the robot. - csvWriter.createSymbolicLink(eventDir, matchDescription); - logWriter.createSymbolicLink(eventDir, matchDescription); - chartWriter.createSymbolicLink(eventDir, matchDescription); - locationWriter.createSymbolicLink(eventDir, matchDescription); - } catch (Exception e) { - e.printStackTrace(); - System.out.printf("Error creating symlinks in %s\n", basePath); - } - } - - private void initChartFile(String csvColumns) { - /* - * Create the html file for the ploty chart. - */ - String title = "Run " + logFileNumber; - String file = String.format("data_%05d", logFileNumber); - writeMessageToFile(chartWriter, String.format( - "\n" + - "%1$s plot.ly chart\n" + - "\n" + - "\n" + - "\n" + - "\n" + - "
\n" + - "\n\n" + - "

\n" + - "Click on the series in the legend to swap y-axis and then to turn off.\n" + - "

\n" + - "Trying to run this locally? Run the following in the directory containing this file:\n" + - "

\n" + - "

\n" + 
-                " python -m SimpleHTTPServer\n" + 
-                "
\n" + - "

\n" + - "Then go to http://localhost:8000/Latest_chart.html\n" + - "\n", title, file, csvColumns, dataDir)); - try { - if (chartWriter != null) { - chartWriter.close(); - } - } catch (Exception e) { - e.printStackTrace(); - } - } - - private void initCSVFile(String csvColumns) { - /* - * Write the heading out to the CSV file. - */ - writeMessageToFile(csvWriter, csvColumns + "\n"); - } - - private void initLocationPlotFile() { - /* - * Create the html file for the location plot. - * We can only do this once the logging classes have all been instantiated. - */ - String title = "Instance " + logFileNumber; - String file = String.format("data_%05d", logFileNumber); - writeMessageToFile(locationWriter, String.format( - "%1$s\n" + - "\n" + - "

\n" + - "
\n" + - "" + - "

\n" + - "Trying to run this locally? Run the following in the directory containing this file:\n" + - "

\n" + - "

\n" + 
-                " python -m SimpleHTTPServer\n" + 
-                "
\n" + - "

\n" + - "Then go to http://localhost:8000/Latest_chart.html\n" + - "\n", title, file, dataDir)); - try { - - if (locationWriter != null) { - locationWriter.close(); - } - } catch (Exception e) { - e.printStackTrace(); - } - } - - public Log pauseGraphLog() { - if (graphLogState == GraphLogState.ACTIVE) { - graphLogState = GraphLogState.PAUSED; - } - return this; - } - - public Log resumeGraphLog() { - if (graphLogState == GraphLogState.PAUSED) { - graphLogState = GraphLogState.ACTIVE; - } - return this; - } - -//################ TXT file methods ################################################################################ - - /* - * Methods that handle writing to the .TXT log file. We allow freeform messages - * to be added to the log file. We precede each message with a time stamp and message type. - */ - - private void writeLogMessage(String message) { - writeMessageToFile(logWriter, message); - } - - /** - * Logs to the external logger only. Generic log messages - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log logMessage(String message, Object... args) { - String date = timeToLogString(getCurrentTime()); - message = String.format(date + ",L " + message + "\n", args); - writeLogMessage(message); - return this; - } - - /** - * Logs to the external logger only. For high volume debug messages - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log debug(String message, Object... args) { - String date = timeToLogString(getCurrentTime()); - message = String.format(date + ",D " + message + "\n", args); - // Don't print it to the console. - writeLogMessage(message); - return this; - } - - /** - * Logs to the external logger and console. For low volume informational messages (<1/sec). - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log info(String message, Object... args) { - String date = timeToLogString(getCurrentTime()); - message = String.format(date + ",I " + message + "\n", args); - // Print to the console. - System.out.print(message); - writeLogMessage(message); - return this; - } - - /** - * Logs to the external logger and console. For warning Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log warning(String message, Object... args) { - String date = timeToLogString(getCurrentTime()); - message = String.format(date + ",W " + message + "\n", args); - // Print to the console. - System.err.print(message); - writeLogMessage(message); - return this; - } - - /** - * Logs to the external logger and console. For important errors Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log error(String message, Object... args) { - String date = timeToLogString(getCurrentTime()); - message = String.format(date + ",E " + message + "\n", args); - // Print to the console. - System.err.print(message); - writeLogMessage(message); - return this; - } - - /** - * Logs Exceptions to the external logger and console. - * @param message Message to Log - * @param e Exception to log after message - */ - @Override - public Log exception(String message, Exception e) { - error(message + ": %s", e); - for (StackTraceElement frame : e.getStackTrace()) { - error(" at %s", frame.toString()); - } - return this; - } - - /** - * Implements the classic println interface. Single string to the console! - * @param message Message to Log - */ - public Log println(String message) { - console(message); - return this; - } - - /** - * Logs to the external logger and console. For important console Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log console(String message, Object... args) { - String date = timeToLogString(getCurrentTime()); - message = String.format(date + ",O " + message + "\n", args); - // Print to the console. - System.out.print(message); - writeLogMessage(message); - return this; - } - - /** - * Logs to the external logger. For command execution Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log cmd(String message, Object... args) { - String date = timeToLogString(getCurrentTime()); - message = String.format(date + ",C " + message + "\n", args); - writeLogMessage(message); - return this; - } - - /** - * Logs to the external logger and console. For important subsystem Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log sub(String message, Object... args) { - String date = timeToLogString(getCurrentTime()); - message = String.format(date + ",S " + message + "\n", args); - // Print to the console. -// System.out.print(message); - writeLogMessage(message); - return this; - } - - /** - * Logs to the external logger and console. For important subsystem Messages. - * @param message Message to Log - * @param args arguments to the message format string - */ - public Log ctrl(String message, Object... args) { - String date = timeToLogString(getCurrentTime()); - message = String.format(date + ",T " + message + "\n", args); - // Print to the console. -// System.out.print(message); - writeLogMessage(message); - return this; - } - - //################ Graphing file methods ################################################################################ - - @Override - public Log doRegister(boolean local, DoubleSupplier sample, String format, Object... args) { - if (onlyLocal && !local) { - return this; - } - if (graphLogState == GraphLogState.ERRORED) { - // the log system is very broken, possibly because of a faulty USB - return this; - } - if (graphLogState != GraphLogState.CREATED) { - error("trying to add Graph elements after graph configured" + graphLogState); - try { - throw new Exception(); - } - catch (Exception e) { - e.printStackTrace(); - } - } else { - logGraphElements.add(new LogGraphElement(sample, String.format(format, args))); - } - return this; - } - - @Override - public Log logCompletedElements(String matchDescription) { - this.matchDescription = matchDescription; - System.out.println("LogCompletedElements for match " + matchDescription); - graphLogState = GraphLogState.CONFIGURED; - this.debug("Log: Element additions completed"); - return this; - } - - public String getGraphHeaders() { - String headers = "date"; - for (LogGraphElement e: logGraphElements) { - if (e.name != null) { - headers = headers + "," + e.name; - } - } - return headers; - } - - public String getGraphValues() { - // Subtracts time offset from current time so graph starts at time = 0 - String value = timeToLogString(getCurrentTime() - enableOffset); - for (LogGraphElement e: logGraphElements) { - if (e.name != null) { - value = value + "," + e.sample.getAsDouble(); - } - } - value = value + "\n"; - return value; - } - - //################ Support file methods ################################################################################ - /* - * These are support methods for the class. They provide useful - * functionality but we separate them here for ease of reading above. - */ - private String timeToLogString(double time) { - /* - * Create the timestamp for this message. We use the robot time, so each - * log entry is time stamped for when it happened during the match. - */ - return String.format("%.3f", time); - } - - private Path getDataPath() { - return Paths.get(basePath, robotName, dataDir); - } - - private Path getDatePath() { - return Paths.get(basePath, robotName, dateDir); - } - - private Path getLatestPath() { - return Paths.get(logPath, robotName, latestDir); - } - - private Path getEventPath() { - return Paths.get(basePath, robotName, eventDir); - } - - - private Path getLogNumberPath() { - return Paths.get(logPath, robotName, "lognumber.txt"); - } - - private synchronized long getNextLogFileNumber() { - /* - * Get the next Log File number. We read the logInstancePath file. - * Increment the number contained within the file (or create the Log - * File as 1 if no such file exists) and write the log file number back - * out. - */ - long logFileNumber = 0; - var logInstancePath = getLogNumberPath(); - try { - BufferedReader br = Files.newBufferedReader(logInstancePath); - String s = br.readLine(); // read the line into the buffer. - logFileNumber = Integer.parseInt(s); - br.close(); - logFileNumber++; - } catch (IOException | NumberFormatException e) { - System.out.println("Cannot read Log Number file. Resetting number to 1"); - logFileNumber = 1; - } - try { - BufferedWriter bw = Files.newBufferedWriter(logInstancePath); - bw.write("" + logFileNumber + "\n"); - bw.flush(); - bw.close(); - System.out.printf("Wrote to %s %d\n", logInstancePath, logFileNumber); - } catch (IOException e) { - e.printStackTrace(); - System.out.println("Cannot write log number file. Possible old file overwrite."); - } - return logFileNumber; - } - - private double getCurrentTime() - { - return clock.currentTime(); - } - - private void writeMessageToFile(LogFileWriter file, String message) { - /* - * Writes the message to the file (if it is open). - */ - if (file == null) - return; // File logging not enabled. - try { - file.write(message); - } catch (Exception e) { - // nothing to do. If we can't write to the log file it's not a disaster. - } - } - - /** - * execute is called periodically - * This is the routine that performs the actual work for the graphing logs. - */ - @Override - public void execute(long timeInMillis) { - if (graphLogState == GraphLogState.CONFIGURED) { - String csvColumns = getGraphHeaders(); - initChartFile(csvColumns); - initCSVFile(csvColumns); - initLocationPlotFile(); - graphLogState = GraphLogState.ACTIVE; - } - if (graphLogState == GraphLogState.ACTIVE) { - writeMessageToFile(csvWriter, getGraphValues()); - if (!createdDateFiles) { - Calendar now = Calendar.getInstance(); - /* - * we may now have access to the correct date and time. If this is the first - * time through we should set the date version of the file links. - */ - if (now.get(Calendar.YEAR) >= 2017) { - createDateFiles(now); - createdDateFiles = true; - } - } - } - } - - - @Override - public Log flush() { - csvWriter.flush(); - logWriter.flush(); - chartWriter.flush(); - locationWriter.flush(); - return this; - } -} diff --git a/src/main/java/frc/robot/lib/LoggingInputDevice.java b/src/main/java/frc/robot/lib/LoggingInputDevice.java index 3d2e70a..750f5ce 100644 --- a/src/main/java/frc/robot/lib/LoggingInputDevice.java +++ b/src/main/java/frc/robot/lib/LoggingInputDevice.java @@ -1,8 +1,9 @@ package frc.robot.lib; -import frc.robot.interfaces.Log; import org.strongback.components.ui.InputDevice; +import frc.robot.lib.chart.Chart; + /** * Add generic logging to a Human Interface device (an InputDevice) * @@ -10,20 +11,20 @@ */ public class LoggingInputDevice { - public static void AddLog(InputDevice input, String name, Log log) { + public static void AddLog(InputDevice input, String name) { for (int i = 0; i < input.getAxisCount(); i++) { final int axis = i; - log.register(false, () -> input.getAxis(axis).read(), "%s/Axis/%d", name, i); + Chart.register(() -> input.getAxis(axis).read(), "%s/Axis/%d", name, i); } for (int i = 0; i < input.getButtonCount(); i++) { final int button = i+1; - log.register(false, input.getButton(button), "%s/Button/%d", name, i); + Chart.register(input.getButton(button), "%s/Button/%d", name, i); } for (int i = 0; i < input.getPOVCount(); i++) { final int axis = i; - log.register(false, input.getDPad(axis), "%s/DPad/%d", name, i); + Chart.register(input.getDPad(axis), "%s/DPad/%d", name, i); } } } diff --git a/src/main/java/frc/robot/lib/MotorFactory.java b/src/main/java/frc/robot/lib/MotorFactory.java index d977b8d..1887a5b 100644 --- a/src/main/java/frc/robot/lib/MotorFactory.java +++ b/src/main/java/frc/robot/lib/MotorFactory.java @@ -14,51 +14,54 @@ import org.strongback.hardware.HardwareSparkMAX; import org.strongback.hardware.HardwareTalonSRX; +import frc.robot.Config; import frc.robot.Constants; -import frc.robot.interfaces.Log; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; public class MotorFactory { - public static Motor getDriveMotor(boolean leftMotor, RobotConfiguration config, Clock clock, Log log) { - leftMotor = leftMotor ^ config.drivebaseSwapLeftRight; - int[] canIds = leftMotor ? config.drivebaseCanIdsLeftWithEncoders : config.drivebaseCanIdsRightWithEncoders; + public static Motor getDriveMotor(boolean leftMotor, Clock clock) { + leftMotor = leftMotor ^ Config.drivebase.swapLeftRight; + int[] canIds = leftMotor ? Config.drivebase.canIdsLeftWithEncoders : Config.drivebase.canIdsRightWithEncoders; - switch (config.drivebaseMotorControllerType) { + switch (Config.drivebase.motorControllerType) { case Constants.MOTOR_CONTROLLER_TYPE_SPARKMAX: { - HardwareSparkMAX spark = getSparkMAX("drive", canIds, leftMotor, NeutralMode.Brake, config.drivebasePIDF, log); + HardwareSparkMAX spark = getSparkMAX("drive", canIds, leftMotor, NeutralMode.Brake, Config.drivebase.pidf); spark.setScale(Constants.SPARKMAX_ENCODER_TICKS, Constants.DRIVE_GEABOX_RATIO, Constants.DRIVE_METRES_PER_REV); - spark.setSensorPhase(config.drivebaseSensorPhase); + spark.setSensorPhase(Config.drivebase.sensorPhase); /* * Setup Current Limiting */ - if (config.drivebaseCurrentLimiting) { - spark.setSmartCurrentLimit(config.drivebaseContCurrent, config.drivebaseContCurrent); // limit to 35 Amps when current exceeds 40 amps - // for 100ms - spark.setSecondaryCurrentLimit(config.drivebasePeakCurrent); + if (Config.drivebase.currentLimiting) { + // Limit to 35 Amps when current exceeds 40 amps for 100ms + spark.setSmartCurrentLimit(Config.drivebase.contCurrent, Config.drivebase.contCurrent); + spark.setSecondaryCurrentLimit(Config.drivebase.peakCurrent); } return spark; } default: - log.error("Invalid drive motor controller '%s'. Please use 'TalonSRX' or 'SparkMAX'. Using TalonSRX.", config.drivebaseMotorControllerType); + Log.error("Invalid drive motor controller '%s'. Please use 'TalonSRX' or 'SparkMAX'. Using TalonSRX.", + Config.drivebase.motorControllerType); // Falling through to TalonSRX. case Constants.MOTOR_CONTROLLER_TYPE_TALONSRX: - HardwareTalonSRX talon = getTalon("drive", canIds, leftMotor, NeutralMode.Brake, config.drivebasePIDF, log); // don't invert output + HardwareTalonSRX talon = getTalon("drive", canIds, leftMotor, NeutralMode.Brake, Config.drivebase.pidf); // don't invert output talon.setScale(Constants.FALCON_ENCODER_TICKS, Constants.DRIVE_GEABOX_RATIO, Constants.DRIVE_METRES_PER_REV); talon.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, 0, 10); - talon.setSensorPhase(config.drivebaseSensorPhase); - talon.configClosedloopRamp(config.drivebaseRampRate, 10); + talon.setSensorPhase(Config.drivebase.sensorPhase); + talon.configClosedloopRamp(Config.drivebase.rampRate, 10); talon.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 10, 10); /* * Setup Current Limiting */ - if (config.drivebaseCurrentLimiting) { - talon.configContinuousCurrentLimit(config.drivebaseContCurrent, 0); // limit to 35 Amps when current exceeds 40 amps for + if (Config.drivebase.currentLimiting) { + talon.configContinuousCurrentLimit(Config.drivebase.contCurrent, 0); // limit to 35 Amps when current exceeds 40 amps for // 100ms - talon.configPeakCurrentLimit(config.drivebasePeakCurrent, 0); + talon.configPeakCurrentLimit(Config.drivebase.peakCurrent, 0); talon.configPeakCurrentDuration(100, 0); talon.enableCurrentLimit(true); } @@ -66,22 +69,22 @@ public static Motor getDriveMotor(boolean leftMotor, RobotConfiguration config, } } - public static HardwareSparkMAX getIntakeMotor(RobotConfiguration config, Log log) { - HardwareSparkMAX motor = getSparkMAX("intake", config.intakeCanID, true, NeutralMode.Brake, config.intakePIDF, log); + public static HardwareSparkMAX getIntakeMotor() { + HardwareSparkMAX motor = getSparkMAX("intake", Config.intake.canID, true, NeutralMode.Brake, Config.intake.pidf); motor.setScale(Constants.SPARKMAX_ENCODER_TICKS, Constants.INTAKE_ENCODER_GEARBOX_RATIO); motor.setClosedLoopRampRate(0.5); return motor; } - public static HardwareTalonSRX getColourWheelMotor(RobotConfiguration config, Log log) { - HardwareTalonSRX motor = getTalon("colour", config.colourWheelCanID, true, NeutralMode.Brake, config.colourWheelPIDF, log); + public static HardwareTalonSRX getColourWheelMotor() { + HardwareTalonSRX motor = getTalon("colour", Config.colourWheel.canID, true, NeutralMode.Brake, Config.colourWheel.pidf); motor.configClosedloopRamp(.25, 10); return motor; } - public static HardwareTalonSRX getLoaderSpinnerMotor(RobotConfiguration config, Log log) { - HardwareTalonSRX motor = getTalon("loader spinner", config.loaderSpinnerCanID, false, NeutralMode.Brake, - config.loaderSpinnderPIDF, log); + public static HardwareTalonSRX getLoaderSpinnerMotor() { + HardwareTalonSRX motor = getTalon("loader spinner", Config.loader.spinnerCanID, false, NeutralMode.Brake, + Config.loader.spinnderPIDF); // In sensor (beambreak) for loader motor.configForwardLimitSwitchSource(LimitSwitchSource.Deactivated, LimitSwitchNormal.NormallyOpen, 10); // Out sensor (beambreak) for loader @@ -92,15 +95,15 @@ public static HardwareTalonSRX getLoaderSpinnerMotor(RobotConfiguration config, return motor; } - public static HardwareSparkMAX getLoaderPassthroughMotor(RobotConfiguration config, Log log) { - HardwareSparkMAX motor = getSparkMAX("loader passthrough", config.loaderPassthroughCanID, true, - NeutralMode.Brake, config.loaderPassthroughPIDF, log); + public static HardwareSparkMAX getLoaderPassthroughMotor() { + HardwareSparkMAX motor = getSparkMAX("loader passthrough", Config.loader.passthroughCanID, true, + NeutralMode.Brake, Config.loader.passthroughPIDF); return motor; } - public static HardwareTalonSRX getShooterMotor(RobotConfiguration config, Clock clock, Log log) { - HardwareTalonSRX motor = getTalon("shooter", config.shooterCanIds, false, NeutralMode.Coast, - config.shooterPIDF, log); + public static HardwareTalonSRX getShooterMotor(Clock clock) { + HardwareTalonSRX motor = getTalon("shooter", Config.shooter.canIds, false, NeutralMode.Coast, + Config.shooter.pidf); motor.setSensorPhase(true); motor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10); motor.setScale(Constants.S4T_ENCODER_TICKS, Constants.SHOOTER_GEARBOX_RATIO); @@ -125,9 +128,9 @@ public static HardwareTalonSRX getShooterMotor(RobotConfiguration config, Clock * @return the leader HardwareTalonSRX */ private static HardwareTalonSRX getTalon(String name, int[] canIDs, boolean invert, NeutralMode mode, - PIDF pidf, Log log) { + PIDF pidf) { HardwareTalonSRX leader = Hardware.Motors.talonSRX(abs(canIDs[0]), invert, mode); - log.register(false, () -> leader.getOutputCurrent(), "Talons/%d/Current", canIDs[0]); + Chart.register(() -> leader.getOutputCurrent(), "Talons/%d/Current", canIDs[0]); leader.configContinuousCurrentLimit(Constants.DEFAULT_CONTINUOUS_CURRENT_LIMIT, 10); leader.configPeakCurrentLimit(Constants.DEFAULT_PEAK_CURRENT_LIMIT, 10); TunableMotor.tuneMotor(leader, pidf, new NetworkTablesHelper(name)); @@ -137,7 +140,7 @@ private static HardwareTalonSRX getTalon(String name, int[] canIDs, boolean inve if (canIDs[n] < 0) shouldInvert = !shouldInvert; HardwareTalonSRX follower = Hardware.Motors.talonSRX(abs(canIDs[n]), shouldInvert, mode); follower.getHWTalon().follow(leader.getHWTalon()); - log.register(false, () -> follower.getOutputCurrent(), "Talons/%d/Current", canIDs[n]); + Chart.register(() -> follower.getOutputCurrent(), "Talons/%d/Current", canIDs[n]); } return leader; } @@ -154,11 +157,11 @@ private static HardwareTalonSRX getTalon(String name, int[] canIDs, boolean inve * @return the HardwareTalonSRX motor controller. */ private static HardwareTalonSRX getTalon(String name, int canID, boolean invert, - NeutralMode mode, PIDF pidf, Log log) { - log.sub("%s: " + canID, "talon"); + NeutralMode mode, PIDF pidf) { + Log.debug("%s: " + canID, "talon"); int[] canIDs = new int[1]; canIDs[0] = canID; - return getTalon(name, canIDs, invert, mode, pidf, log); + return getTalon(name, canIDs, invert, mode, pidf); } /** @@ -175,10 +178,10 @@ private static HardwareTalonSRX getTalon(String name, int canID, boolean invert, * @return the leader SparkMAX */ - private static HardwareSparkMAX getSparkMAX(String name, int[] canIDs, boolean invert, NeutralMode mode, PIDF pidf, Log log) { + private static HardwareSparkMAX getSparkMAX(String name, int[] canIDs, boolean invert, NeutralMode mode, PIDF pidf) { HardwareSparkMAX leader = Hardware.Motors.sparkMAX(abs(canIDs[0]), MotorType.kBrushless, invert); leader.setIdleMode(mode == NeutralMode.Brake ? IdleMode.kBrake : IdleMode.kCoast); - log.register(false, () -> leader.getOutputCurrent(), "SparkMAX/%d/Current", canIDs[0]); + Chart.register(() -> leader.getOutputCurrent(), "SparkMAX/%d/Current", canIDs[0]); leader.setSmartCurrentLimit(Constants.DEFAULT_CONTINUOUS_CURRENT_LIMIT, 10); leader.setSecondaryCurrentLimit(Constants.DEFAULT_PEAK_CURRENT_LIMIT, 10); TunableMotor.tuneMotor(leader, pidf, new NetworkTablesHelper(name)); @@ -189,7 +192,7 @@ private static HardwareSparkMAX getSparkMAX(String name, int[] canIDs, boolean i shouldInvert = !shouldInvert; HardwareSparkMAX follower = Hardware.Motors.sparkMAX(abs(canIDs[n]), MotorType.kBrushless, shouldInvert); follower.getHWSpark().follow(leader.getHWSpark()); - log.register(false, () -> follower.getOutputCurrent(), "SparkMAX/%d/Current", canIDs[n]); + Chart.register(() -> follower.getOutputCurrent(), "SparkMAX/%d/Current", canIDs[n]); } return leader; } @@ -205,11 +208,11 @@ private static HardwareSparkMAX getSparkMAX(String name, int[] canIDs, boolean i * @param log for registration of the current reporting. * @return the HardwareSparkMAX motor controller. */ - private static HardwareSparkMAX getSparkMAX(String name, int canID, boolean invert, NeutralMode mode, PIDF pidf, Log log) { - log.sub("%s: " + canID, " spark max"); + private static HardwareSparkMAX getSparkMAX(String name, int canID, boolean invert, NeutralMode mode, PIDF pidf) { + Log.debug("%s: " + canID, " spark max"); int[] canIDs = new int[1]; canIDs[0] = canID; - return getSparkMAX(name, canIDs, invert, mode, pidf, log); + return getSparkMAX(name, canIDs, invert, mode, pidf); } private static int abs(int value) { diff --git a/src/main/java/frc/robot/lib/NavXGyroscope.java b/src/main/java/frc/robot/lib/NavXGyroscope.java index 6e09109..0c2e615 100644 --- a/src/main/java/frc/robot/lib/NavXGyroscope.java +++ b/src/main/java/frc/robot/lib/NavXGyroscope.java @@ -2,7 +2,8 @@ import org.strongback.components.Gyroscope; import frc.robot.interfaces.DashboardUpdater; -import frc.robot.interfaces.Log; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; import com.kauailabs.navx.frc.AHRS; @@ -41,7 +42,6 @@ public enum Drift { FINISHED } private AHRS ahrs = null; - private Log log; private double baseAngle = 0; private double basePitch = 0; private double driftStartTime; // In drift calculations we see how much the gyro drifts during a known stop period. @@ -50,9 +50,8 @@ public enum Drift { private Drift driftState = Drift.NOT_STARTED; private String name; - public NavXGyroscope(String name, boolean present, Log log) { + public NavXGyroscope(String name, boolean present) { this.name = name; - this.log = log; ahrs = null; if (present) { @@ -67,21 +66,21 @@ public NavXGyroscope(String name, boolean present, Log log) { } } if (ahrs != null) { - log.register(true, (() -> { return getAngle(); } ), "%s/angle", name) - .register(true, (() -> { return getYaw(); } ), "%s/yaw", name) - .register(true, (() -> { return getRoll(); } ), "%s/roll", name) - .register(true, (() -> { return getPitch(); } ), "%s/pitch", name) - .register(true, (() -> { return driftRate; } ), "%s/drift", name) - .register(true, (() -> { return isCalibrating()?1.0:0.0; } ), "%s/Misc/Calibrating", name) - .register(false, (() -> { return getDisplacementX(); } ), "%s/disp/X", name) - .register(false, (() -> { return getDisplacementY(); } ), "%s/disp/Y", name) - .register(false, (() -> { return getDisplacementZ(); } ), "%s/disp/Z", name) - .register(false, (() -> { return getWorldLinearAccelX(); } ), "%s/WorldAccel/X", name) - .register(false, (() -> { return getWorldLinearAccelY(); } ), "%s/WorldAccel/Y", name) - .register(false, (() -> { return getWorldLinearAccelZ(); } ), "%s/WorldAccel/Z", name) - .register(false, (() -> { return getRawAccelX(); } ), "%s/RawAccel/X", name) - .register(false, (() -> { return getRawAccelY(); } ), "%s/RawAccel/Y", name) - .register(false, (() -> { return getRawAccelZ(); } ), "%s/RawAccel/Z", name); + Chart.register((() -> { return getAngle(); } ), "%s/angle", name); + Chart.register((() -> { return getYaw(); } ), "%s/yaw", name); + Chart.register((() -> { return getRoll(); } ), "%s/roll", name); + Chart.register((() -> { return getPitch(); } ), "%s/pitch", name); + Chart.register((() -> { return driftRate; } ), "%s/drift", name); + Chart.register((() -> { return isCalibrating()?1.0:0.0; } ), "%s/Misc/Calibrating", name); + Chart.register((() -> { return getDisplacementX(); } ), "%s/disp/X", name); + Chart.register((() -> { return getDisplacementY(); } ), "%s/disp/Y", name); + Chart.register((() -> { return getDisplacementZ(); } ), "%s/disp/Z", name); + Chart.register((() -> { return getWorldLinearAccelX(); } ), "%s/WorldAccel/X", name); + Chart.register((() -> { return getWorldLinearAccelY(); } ), "%s/WorldAccel/Y", name); + Chart.register((() -> { return getWorldLinearAccelZ(); } ), "%s/WorldAccel/Z", name); + Chart.register((() -> { return getRawAccelX(); } ), "%s/RawAccel/X", name); + Chart.register((() -> { return getRawAccelY(); } ), "%s/RawAccel/Y", name); + Chart.register((() -> { return getRawAccelZ(); } ), "%s/RawAccel/Z", name); } zero(); } @@ -105,7 +104,7 @@ public boolean startDriftCalculation() { public void endDriftCalculation() { driftRate = (getRawAngle() - driftStartHeading) / (Timer.getFPGATimestamp() - driftStartTime); driftState = Drift.FINISHED; - log.debug("Finished Drift Calculation: %f degrees per second", driftRate); + Log.debug("Finished Drift Calculation: %f degrees per second", driftRate); } public Drift currentDriftCalculation() { @@ -144,14 +143,14 @@ public Gyroscope zero() { driftStartTime = Timer.getFPGATimestamp(); } System.out.println("reset baseAngle = " + baseAngle); - log.debug("reset baseAngle = %f", baseAngle); + Log.debug("reset baseAngle = %f", baseAngle); return this; } public void setAngle(double angle) { zero(); baseAngle -= angle; - log.debug("set baseAngle = %f", baseAngle); + Log.debug("set baseAngle = %f", baseAngle); } private double getRawAngle() { diff --git a/src/main/java/frc/robot/lib/PowerMonitor.java b/src/main/java/frc/robot/lib/PowerMonitor.java index 5449778..8794621 100644 --- a/src/main/java/frc/robot/lib/PowerMonitor.java +++ b/src/main/java/frc/robot/lib/PowerMonitor.java @@ -1,7 +1,8 @@ package frc.robot.lib; import frc.robot.interfaces.DashboardUpdater; -import frc.robot.interfaces.Log; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -20,23 +21,23 @@ public class PowerMonitor implements DashboardUpdater { // no interface, as thi PowerDistributionPanel pdp; - public PowerMonitor (PowerDistributionPanel pdp, int[] channelsToMonitor, boolean enabled, Log log) { + public PowerMonitor (PowerDistributionPanel pdp, int[] channelsToMonitor, boolean enabled) { final String name = "Power"; this.pdp = pdp; this.enabled = enabled; if (!enabled) { - log.sub("PDP not enabled"); + Log.debug("PDP not enabled"); return; } - log.sub("PDP enabled"); - log.register(false, (() -> { return pdp.getTotalEnergy(); } ), "%s/totalEnergy", name) - .register(false, (() -> { return pdp.getTotalPower(); } ), "%s/totalPower", name) - .register(false, (() -> { return pdp.getTotalCurrent(); } ), "%s/totalCurrent", name) - .register(false, (() -> { return pdp.getTemperature(); } ), "%s/temperature", name) - .register(false, (() -> { return pdp.getVoltage(); } ), "%s/inputVoltage", name); + Log.debug("PDP enabled"); + Chart.register((() -> { return pdp.getTotalEnergy(); } ), "%s/totalEnergy", name); + Chart.register((() -> { return pdp.getTotalPower(); } ), "%s/totalPower", name); + Chart.register((() -> { return pdp.getTotalCurrent(); } ), "%s/totalCurrent", name); + Chart.register((() -> { return pdp.getTemperature(); } ), "%s/temperature", name); + Chart.register((() -> { return pdp.getVoltage(); } ), "%s/inputVoltage", name); for (int i = 0; i < channelsToMonitor.length; i++) { final int channel = channelsToMonitor[i]; - log.register(false, (() -> { return pdp.getCurrent(channel); } ), "%s/channelCurrent/%d", name, channel); + Chart.register((() -> { return pdp.getCurrent(channel); } ), "%s/channelCurrent/%d", name, channel); } } diff --git a/src/main/java/frc/robot/lib/RedundantTalonSRX.java b/src/main/java/frc/robot/lib/RedundantTalonSRX.java index fa2cdf0..e51ce22 100644 --- a/src/main/java/frc/robot/lib/RedundantTalonSRX.java +++ b/src/main/java/frc/robot/lib/RedundantTalonSRX.java @@ -11,7 +11,8 @@ import org.strongback.components.TalonSensorCollection; import org.strongback.hardware.HardwareTalonSRX; -import frc.robot.interfaces.Log; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; /** * Creates what appears to be a single TalonSRX, but is really a list of @@ -36,7 +37,6 @@ public class RedundantTalonSRX extends HardwareTalonSRX implements TalonSensorCo private static ArrayList badTalons = new ArrayList<>(); // Talons that have drawn so much current. private static ArrayList badEncoders = new ArrayList<>(); // Talons that have bad encoders. private Clock clock; - private static Log log; // Current draw. // Record how long each talon has been an outlier for current draw. If they are long enough, @@ -58,22 +58,21 @@ public class RedundantTalonSRX extends HardwareTalonSRX implements TalonSensorCo ControlMode lastMode = ControlMode.Speed; double lastDemand0 = 0; - public RedundantTalonSRX(ArrayList potentialLeaders, ArrayList followers, Clock clock, Log log) { + public RedundantTalonSRX(ArrayList potentialLeaders, ArrayList followers, Clock clock) { super(potentialLeaders.get(0).getTalonSRX()); this.potentialLeaders = potentialLeaders; this.followers = followers; this.clock = clock; - RedundantTalonSRX.log = log; activeTalons = new ArrayList(); activeTalons.addAll(potentialLeaders); activeTalons.addAll(followers); otherLeaders = new ArrayList(); changeLeader(0); for(HardwareTalonSRX talon : activeTalons) { - log.register(false, () -> talon.getOutputCurrent(), "Talons/%d/Current", talon.getDeviceID()); + Chart.register(() -> talon.getOutputCurrent(), "Talons/%d/Current", talon.getDeviceID()); } - log.register(false, () -> (double)badEncoders.size(), "RedundantTalons/numBadEncoders"); - log.register(false, () -> (double)badTalons.size(), "RedundantTalons/numBadTalons"); + Chart.register(() -> (double)badEncoders.size(), "RedundantTalons/numBadEncoders"); + Chart.register(() -> (double)badTalons.size(), "RedundantTalons/numBadTalons"); // Ensure execute gets called to check the talons/encoders. // Disable for performance. //Strongback.executor().register(this, Priority.LOW); @@ -221,7 +220,7 @@ private void limitedError(String key, String msg, Object... args) { // Has it been long enough since the last log message? if (now - lastErrorTime.getOrDefault(key, 0.) < kMinLoggingIntervalSec) return; // It has. Log now. - log.error(msg, args); + Log.error(msg, args); // Update the last log time. lastErrorTime.put(key, now); } @@ -239,7 +238,7 @@ public void disableTalon(HardwareTalonSRX talon) { } // Disable it now that it's not the leader. talon.set(ControlMode.Disabled, 0); - log.error("Disabled TalonSRX with CAN ID %d", talon.getDeviceID()); + Log.error("Disabled TalonSRX with CAN ID %d", talon.getDeviceID()); } /** @@ -255,9 +254,9 @@ public void disableEncoder(HardwareTalonSRX talon) { // If it was the leader, then find a new leader. if (leader == talon) { changeLeader(0); - log.error("Bad encoder on leader TalonSRX %d, changed leadership", talon.getDeviceID()); + Log.error("Bad encoder on leader TalonSRX %d, changed leadership", talon.getDeviceID()); } else { - log.error("Converted potential leader TalonSRX %d with bad encoder to a follower only", talon.getDeviceID()); + Log.error("Converted potential leader TalonSRX %d with bad encoder to a follower only", talon.getDeviceID()); } // It will have been setup as a follower now. @@ -286,21 +285,20 @@ public void changeLeader(int index) { * Print out any issues seen across all RedundantTalonSRX's. */ public static void printStatus() { - if (log == null) return; if (!badEncoders.isEmpty()) { - log.error("The following talons have a bad encoder:"); + Log.error("The following talons have a bad encoder:"); for (HardwareTalonSRX talon : badEncoders) { - log.error(" Talon SRX %d", talon.getDeviceID()); + Log.error(" Talon SRX %d", talon.getDeviceID()); } } if (!badTalons.isEmpty()) { - log.error("The following talons or motors have drawn a large amount of current:"); + Log.error("The following talons or motors have drawn a large amount of current:"); for (HardwareTalonSRX talon : badTalons) { - log.error(" Talon SRX %d", talon.getDeviceID()); + Log.error(" Talon SRX %d", talon.getDeviceID()); } } if (badTalons.isEmpty() && badEncoders.isEmpty()) { - log.info("No bad talons or encoders found"); + Log.info("No bad talons or encoders found"); } } diff --git a/src/main/java/frc/robot/lib/RobotConfiguration.java b/src/main/java/frc/robot/lib/RobotConfiguration.java deleted file mode 100644 index b1f2704..0000000 --- a/src/main/java/frc/robot/lib/RobotConfiguration.java +++ /dev/null @@ -1,400 +0,0 @@ -package frc.robot.lib; - -import java.io.BufferedReader; -import java.io.BufferedWriter; -import java.io.IOException; -import java.nio.file.Files; -import java.nio.file.NoSuchFileException; -import java.nio.file.Path; -import java.nio.file.Paths; -import java.nio.file.StandardOpenOption; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Collections; -import java.util.HashMap; -import java.util.Map; -import java.util.TreeMap; -import java.util.stream.Collectors; - -import frc.robot.Constants; -import frc.robot.interfaces.Log; - -/** - * Class responsible for updating values which are dependent on robot hardware. - * (e.g. if subsystems are present or not) It reads from a text file - * Currently the supported types are String, int, double, boolean and int array. - * - * Example lines: - * drivebase/present = true - * drivebase/rampRate = 0.13125 - * drivebase/right/canIDs/withEncoders = 7,6 - * drivebase/right/canIDs/withoutEncoders = 5 - */ -public class RobotConfiguration { - private String name = "RobotConfig"; - - private Log log; - private Map lines; - private Map ignoredEntries; // Lines/entries not used in the config file. - private Map nonDefaultParameters; // Non-default values used from the config file. - private ArrayList exampleText = new ArrayList(); - - // These are variables which will be updated - - public String robotName = "3132"; - public double robotLengthWithBumpers = 0; - public double robotWidthWithBumpers = 0; - public double cameraFromFrontWithBumpers = 0; - public double cameraFromLeftWithBumpers = 0; - - public int teamNumber = 3132; - public boolean drivebaseIsPresent = Constants.DRIVE_PRESENT_DEFAULT; - public String drivebaseMotorControllerType = Constants.DRIVE_DEFAULT_CONTROLLER_TYPE; - public int[] drivebaseCanIdsLeftWithEncoders = Constants.DRIVE_LEFT_TALON_WITH_ENCODERS_CAN_ID_LIST; - public int[] drivebaseCanIdsLeftWithoutEncoders = Constants.DRIVE_LEFT_TALON_WITHOUT_ENCODERS_CAN_ID_LIST; - public int[] drivebaseCanIdsRightWithEncoders = Constants.DRIVE_RIGHT_TALON_WITH_ENCODERS_CAN_ID_LIST; - public int[] drivebaseCanIdsRightWithoutEncoders = Constants.DRIVE_RIGHT_TALON_WITHOUT_ENCODERS_CAN_ID_LIST; - public boolean drivebaseCurrentLimiting = true; - public int drivebaseContCurrent = Constants.DRIVE_CONT_CURRENT; - public int drivebasePeakCurrent = Constants.DRIVE_PEAK_CURRENT; - public double drivebaseRampRate = Constants.DRIVE_RAMP_RATE; - public PIDF drivebasePIDF = new PIDF(Constants.DRIVE_P, Constants.DRIVE_I, Constants.DRIVE_D, Constants.DRIVE_F); - public String drivebaseMode = Constants.DRIVE_DEFAULT_MODE; - public double drivebaseMaxSpeed = Constants.DRIVE_MAX_SPEED; - - public int climberPtoPort = Constants.CLIMBER_PTO_SOLENOID_PORT; - public int climberBrakePort = Constants.CLIMBER_BRAKE_SOLENOID_PORT; - - - public boolean drivebaseSwapLeftRight = false; - public boolean drivebaseSensorPhase = false; - public double drivebaseCount = Constants.DRIVE_COUNT_100ms; - - public boolean intakeIsPresent = Constants.INTAKE_PRESENT_DEFAULT; - public int intakeCanID = Constants.INTAKE_CAN_ID; - public PIDF intakePIDF = new PIDF(Constants.INTAKE_P, Constants.INTAKE_I, Constants.INTAKE_D, Constants.INTAKE_F); - - public boolean colourWheelIsPresent = Constants.COLOUR_WHEEL_PRESENT_DEFAULT; - public int colourWheelCanID = Constants.COLOUR_WHEEL_CAN_ID; - public PIDF colourWheelPIDF = new PIDF(0, 0, 0, 0); - - public boolean loaderIsPresent = Constants.LOADER_PRESENT_DEFAULT; - public int loaderSpinnerCanID = Constants.LOADER_SPINNER_MOTOR_CAN_ID; - public PIDF loaderSpinnderPIDF = new PIDF(Constants.LOADER_SPINNER_P, Constants.LOADER_SPINNER_I, Constants.LOADER_SPINNER_D, Constants.LOADER_SPINNER_F); - public int loaderPassthroughCanID = Constants.LOADER_PASSTHROUGH_MOTOR_CAN_ID; - public PIDF loaderPassthroughPIDF = new PIDF(0, 0, 0, 0); - - public boolean shooterIsPresent = Constants.SHOOTER_PRESENT_DEFAULT; - public int[] shooterCanIds = Constants.SHOOTER_TALON_CAN_ID_LIST; - public PIDF shooterPIDF = new PIDF(Constants.SHOOTER_P, Constants.SHOOTER_I, Constants.SHOOTER_D, Constants.SHOOTER_F); - - public boolean pdpIsPresent = Constants.PDP_PRESENT_DEFAULT; - public int pdpCanId = Constants.PDP_CAN_ID; - public boolean pdpMonitor = false; // by default we do NOT monitor the PDP - public int[] pdpChannelsToMonitor = new int[0]; // by default we do NOT monitor any channels - - public boolean pcmIsPresent = Constants.PCM_PRESENT_DEFAULT; - public int pcmCanId = Constants.PCM_CAN_ID; - - public boolean navxIsPresent = Constants.NAVX_PRESENT_DEFAULT; - - public boolean visionIsPresent = Constants.VISION_PRESENT_DEFAULT; - public double visionHMin = Constants.VISION_H_MIN; - public double visionHMax = Constants.VISION_H_MAX; - public double visionSMin = Constants.VISION_S_MIN; - public double visionSMax = Constants.VISION_S_MAX; - public double visionVMin = Constants.VISION_V_MIN; - public double visionVMax = Constants.VISION_V_MAX; - - // Not currently used or read from the config file. - public boolean buddyClimbIsPresent = Constants.BUDDYCLIMB_PRESENT_DEFAULT; - public int buddyClimbSolenoidPort = Constants.BUDDYCLIMB_SOLENOID_PORT; - - // Logging default is to not log anything to the graph, and to only log local information when we turn it on. - // These are the safest defaults. - public boolean doLogging = false; - public boolean onlyLocal = true; - - public boolean ledStripIsPresent = Constants.LED_STRIP_PRESENT_DEFAULT; - - public RobotConfiguration(String filePath, Log log) { - this(filePath, TeamNumber.get(), log); - } - - public RobotConfiguration(String filePath, int teamNumber, Log log) { - this.teamNumber = teamNumber; - this.log = log; - - readLines(Paths.get(filePath)); - readParameters(); // Creates example contents. - Collections.sort(exampleText); - writeExampleFile(filePath, String.join("\n", exampleText)); - } - - private void writeExampleFile(String filePath, String contents) { - Path exampleFile = Paths.get(filePath + ".example"); - try { - BufferedWriter writer; - writer = Files.newBufferedWriter(exampleFile, StandardOpenOption.CREATE); - writer.write(contents + "\n"); - writer.close(); - log.info("Wrote example config file " + exampleFile.toString()); - } catch (IOException e) { - log.exception("Unable to write example config file " + exampleFile.toString(), e); - } - } - - private void readLines(Path path) { - log.info("Reading config file " + path); - lines = new HashMap(); - ignoredEntries = new TreeMap(); - nonDefaultParameters = new TreeMap(); - try (BufferedReader reader = Files.newBufferedReader(path)) { - String line = null; - while ((line = reader.readLine()) != null) { - String[] parts = line.split("\\s*=\\s*", -1); // Keep empty values - if (parts.length < 2) { - log.error("Bad config line " + line); - continue; - } - String tag = parts[0].trim(); - String value = parts[1].trim(); - if (lines.containsKey(tag)) { - log.error("ERROR: Duplicate tag %s in configuration file, last value will be used.", tag); - } - lines.put(tag, value); - ignoredEntries.put(parts[0].trim(), line); - } - } catch (NoSuchFileException e) { - log.error("Config file %s not found, attempting to create it", path); - // Touch the file so at least it's there next time. - try { - Files.createFile(path); - } catch (IOException e1) {} - } catch (IOException e) { - log.exception("Error loading configuration file " + path + ", using defaults", e); - } - } - - private void readParameters() { - drivebaseIsPresent = getAsBoolean("drivebase/present", drivebaseIsPresent); - drivebaseMotorControllerType = getMotorControllerType("drivebase/motorControllerType", drivebaseMotorControllerType); - drivebaseCanIdsLeftWithEncoders = getAsIntArray("drivebase/left/canIDs/withEncoders", drivebaseCanIdsLeftWithEncoders); - drivebaseCanIdsLeftWithoutEncoders = getAsIntArray("drivebase/left/canIDs/withoutEncoders", drivebaseCanIdsLeftWithoutEncoders); - drivebaseCanIdsRightWithEncoders = getAsIntArray("drivebase/right/canIDs/withEncoders", drivebaseCanIdsRightWithEncoders); - drivebaseCanIdsRightWithoutEncoders = getAsIntArray("drivebase/right/canIDs/withoutEncoders", drivebaseCanIdsRightWithoutEncoders); - drivebaseCurrentLimiting = getAsBoolean("drivebase/currentLimiting", drivebaseCurrentLimiting); - drivebaseContCurrent = getAsInt("drivebase/maxCurrent", drivebaseContCurrent); - drivebasePeakCurrent = getAsInt("drivebase/peakCurrent", drivebasePeakCurrent); - drivebaseRampRate = getAsDouble("drivebase/rampRate", drivebaseRampRate); - drivebasePIDF = getAsPIDF("drivebase", drivebasePIDF); - drivebaseMode = getAsString("drivebase/mode", drivebaseMode); - drivebaseMaxSpeed = getAsDouble("drivebase/maxSpeed", drivebaseMaxSpeed); - drivebaseSwapLeftRight = getAsBoolean("drivebase/swapLeftRight", drivebaseSwapLeftRight); - drivebaseSensorPhase = getAsBoolean("drivebase/sensor/phase", drivebaseSensorPhase); - drivebaseCount = getAsDouble("drivebase/count100ms", drivebaseCount); - - - intakeIsPresent = getAsBoolean("intake/present", intakeIsPresent); - intakeCanID = getAsInt("intake/canID", Constants.INTAKE_CAN_ID); - intakePIDF = getAsPIDF("intake", intakePIDF); - - colourWheelIsPresent = getAsBoolean("colourWheel/present", colourWheelIsPresent); - colourWheelCanID = getAsInt("colourWheel/canID", Constants.COLOUR_WHEEL_CAN_ID); - colourWheelPIDF = getAsPIDF("colourWheel", colourWheelPIDF); - - loaderIsPresent = getAsBoolean("loader/present", loaderIsPresent); - loaderSpinnerCanID = getAsInt("loader/spinner/canID", Constants.LOADER_SPINNER_MOTOR_CAN_ID); - loaderSpinnderPIDF = getAsPIDF("loader/spinner/", loaderSpinnderPIDF); - loaderPassthroughCanID = getAsInt("loader/passthrough/canID", Constants.LOADER_PASSTHROUGH_MOTOR_CAN_ID); - loaderPassthroughPIDF = getAsPIDF("loader/passthrough/", loaderPassthroughPIDF); - - shooterIsPresent = getAsBoolean("shooter/present", shooterIsPresent); - shooterCanIds = getAsIntArray("shooter/shooterCanIDs", Constants.SHOOTER_TALON_CAN_ID_LIST); - shooterPIDF = getAsPIDF("shooter/", shooterPIDF); - - pdpIsPresent = getAsBoolean("pdp/present", pdpIsPresent); - pdpCanId = getAsInt("pdp/canID", Constants.PDP_CAN_ID); - pdpMonitor = getAsBoolean("pdp/monitor", false); // by default we do NOT monitor the PDP - pdpChannelsToMonitor = getAsIntArray("pdp/channels", new int[0]); // by default we do NOT monitor and channels - - pcmIsPresent = getAsBoolean("pcm/present", pcmIsPresent); - pcmCanId = getAsInt("pcm/canID", Constants.PCM_CAN_ID); - - navxIsPresent = getAsBoolean("navx/present", navxIsPresent); - - robotLengthWithBumpers = getAsDouble("dimensions/robot/lengthWithBumpers", 0.0); - robotWidthWithBumpers = getAsDouble("dimensions/robot/widthWithBumpers", 0.0); - cameraFromFrontWithBumpers = getAsDouble("dimensions/cameraFromFrontWithBumpers", 0.0); - cameraFromLeftWithBumpers = getAsDouble("dimensions/cameraFromLeftWithBumpers", 0.0); - - visionIsPresent = getAsBoolean("vision/present", visionIsPresent); - visionHMin = getAsDouble("vision/hsvFilter/h/min", Constants.VISION_H_MIN); - visionHMax = getAsDouble("vision/hsvFilter/h/max", Constants.VISION_H_MAX); - visionSMin = getAsDouble("vision/hsvFilter/s/min", Constants.VISION_S_MIN); - visionSMax = getAsDouble("vision/hsvFilter/s/max", Constants.VISION_S_MAX); - visionVMin = getAsDouble("vision/hsvFilter/v/min", Constants.VISION_V_MIN); - visionVMax = getAsDouble("vision/hsvFilter/v/max", Constants.VISION_V_MAX); - - // logging default is to not log anything to the graph, and to only log local information when we turn it on. - // These are the safest defaults. - doLogging = getAsBoolean("logging/enabled", true); - onlyLocal = getAsBoolean("logging/onlyLocal", true); - - ledStripIsPresent = getAsBoolean("ledStrip/present", ledStripIsPresent); - - if (!ignoredEntries.isEmpty()) { - log.warning("WARNING: These config file lines weren't used:"); - for (String entry : ignoredEntries.values()) { - log.warning(" %s", entry); - } - } - if (!nonDefaultParameters.isEmpty()) { - log.warning("WARNING: These parameters have non-default values:"); - for (Map.Entry entry : nonDefaultParameters.entrySet()) { - log.warning(" %s = %s", entry.getKey(), entry.getValue()); - } - } - log.info("RobotConfig finished loading parameters\n"); - } - - private String getMotorControllerType(String parameterName, String defaultValue) { - String type = getAsString(parameterName, defaultValue); - switch(type) { - default: - log.error("Invalid value '%s' for parameter '%s'. Using TalonSRX.", type, parameterName); - // Falling through to TalonSRX. - - case Constants.MOTOR_CONTROLLER_TYPE_TALONSRX: - return Constants.MOTOR_CONTROLLER_TYPE_TALONSRX; - - case Constants.MOTOR_CONTROLLER_TYPE_SPARKMAX: - return Constants.MOTOR_CONTROLLER_TYPE_SPARKMAX; - } - } - - private void appendExample(String key, T defaultValue) { - exampleText.add(key + " = " + defaultValue); - } - - private int getAsInt(String key, int defaultValue) { - appendExample(key, defaultValue); - try { - if (lines.containsKey(key)) { - int value = Integer.valueOf(lines.get(key)); - ignoredEntries.remove(key); // Used this line. - log.debug("%s: %s -> %d", name, key, value); - if (value != defaultValue) { - nonDefaultParameters.put(key, lines.get(key)); - } - return value; - } - } catch (Exception e) { - log.exception("Error reading key: " + key + " using default", e); - } - return defaultValue; - } - - private double getAsDouble(String key, double defaultValue) { - appendExample(key, defaultValue); - try { - if (lines.containsKey(key)) { - double value = Double.valueOf(lines.get(key)); - ignoredEntries.remove(key); // Used this line. - log.debug("%s: %s -> %f", name, key, value); - if (value != defaultValue) { - nonDefaultParameters.put(key, lines.get(key)); - } - return value; - } - } catch (Exception e) { - log.exception("Error reading key: " + key + " using default", e); - } - return defaultValue; - } - - private PIDF getAsPIDF(String prefix, PIDF pidf) { - pidf.p = getAsDouble(prefix+"/p", pidf.p); - pidf.i = getAsDouble(prefix+"/i", pidf.i); - pidf.d = getAsDouble(prefix+"/d", pidf.d); - pidf.f = getAsDouble(prefix+"/f", pidf.f); - return pidf; - } - - private boolean getAsBoolean(String key, boolean defaultValue) { - appendExample(key, defaultValue); - try { - if (lines.containsKey(key)) { - boolean value = Boolean.valueOf(lines.get(key)); - ignoredEntries.remove(key); // Used this line. - log.debug("%s: %s -> %s", name, key, value); - if (value != defaultValue) { - nonDefaultParameters.put(key, lines.get(key)); - } - return value; - } - } catch (Exception e) { - log.exception("Error reading key: " + key + " using default", e); - } - return defaultValue; - } - - private String getAsString(String key, String defaultValue) { - appendExample(key, "\"" + defaultValue + "\""); - try { - if (lines.containsKey(key)) { - // Get the value between the quotes. - String[] parts = lines.get(key).split("\"", -1); - if (parts.length < 3) { - log.error("Bad string value for %s, needs to be in double quotes, not: %s", key, lines.get(key)); - return defaultValue; - } - String value = parts[1]; - ignoredEntries.remove(key); // Used this line. - log.debug("%s: %s -> %s", name, key, value); - if (!value.equals(defaultValue)) { - nonDefaultParameters.put(key, lines.get(key)); - } - return value; - } - } catch (Exception e) { - log.exception("Error reading key: " + key + " using default", e); - } - return defaultValue; - } - - private int[] getAsIntArray(String key, int[] defaultValue) { - // Joining primitive arrays seems to be painful under Java. - appendExample(key, joinIntArray(defaultValue)); - try { - if (lines.containsKey(key)) { - String value = lines.get(key); - int[] values; - if (value.equals("")) { - // No values. - values = new int[0]; - } else { - // One or more values. - String[] parts = value.split("\\s*,\\s*"); - values = new int[parts.length]; - for (int i = 0; i < parts.length; i++) { - values[i] = Integer.valueOf(parts[i]); - } - } - ignoredEntries.remove(key); // Used this line. - log.debug("%s: %s -> %s", name, key, joinIntArray(values)); - if (!java.util.Arrays.equals(values, defaultValue)) { - nonDefaultParameters.put(key, lines.get(key)); - } - return values; - } - } catch (Exception e) { - log.exception("Error reading key: " + key + " using default", e); - } - return defaultValue; - } - - private String joinIntArray(int[] values) { - return Arrays.stream(values).mapToObj(String::valueOf).collect(Collectors.joining(",")); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/RobotName.java b/src/main/java/frc/robot/lib/RobotName.java index 422dcb7..a0e010f 100644 --- a/src/main/java/frc/robot/lib/RobotName.java +++ b/src/main/java/frc/robot/lib/RobotName.java @@ -6,7 +6,14 @@ import java.nio.file.Paths; import java.util.Random; +import frc.robot.Constants; + /** + * Read a speciall file on the filesystem to get the robot name. + * + * The logging and charting files will put their files in a subdirectory of + * this name so that multiple robots worth of log files can be overlaid with + * rsync and not overwrite each other. */ public class RobotName { @@ -43,4 +50,14 @@ public static String get(String path) { } return robotName; } + + /** + * Attempts to read the first line of a file to get the robot name. If the file + * doesn't exist it will create a new random name, write the file and return + * that. + * @return name of the robot as a String. + */ + public static String get() { + return get(Constants.ROBOT_NAME_FILE_PATH); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/Subsystem.java b/src/main/java/frc/robot/lib/Subsystem.java index 2d0ff39..d5ff4d2 100644 --- a/src/main/java/frc/robot/lib/Subsystem.java +++ b/src/main/java/frc/robot/lib/Subsystem.java @@ -4,7 +4,6 @@ import org.strongback.command.Requirable; import frc.robot.interfaces.DashboardInterface; import frc.robot.interfaces.DashboardUpdater; -import frc.robot.interfaces.Log; import frc.robot.interfaces.NetworkTableHelperInterface; import edu.wpi.first.wpilibj.Notifier; @@ -24,17 +23,15 @@ public abstract class Subsystem implements Requirable, Executable, DashboardUpda protected Notifier notifier; protected boolean enabled; protected DashboardInterface dashboard; - protected Log log; protected NetworkTableHelperInterface networkTable; - public Subsystem(String name, DashboardInterface dashboard, Log log) { - this(name, null, dashboard, log); + public Subsystem(String name, DashboardInterface dashboard) { + this(name, null, dashboard); } - public Subsystem(String name, NetworkTableHelperInterface networkTable, DashboardInterface dashboard, Log log) { + public Subsystem(String name, NetworkTableHelperInterface networkTable, DashboardInterface dashboard) { this.name = name; this.dashboard = dashboard; - this.log = log; this.networkTable = networkTable; enabled = false; // we start out disabled } diff --git a/src/main/java/frc/robot/lib/chart/Chart.java b/src/main/java/frc/robot/lib/chart/Chart.java new file mode 100644 index 0000000..bc00412 --- /dev/null +++ b/src/main/java/frc/robot/lib/chart/Chart.java @@ -0,0 +1,309 @@ +package frc.robot.lib.chart; + +import java.io.IOException; +import java.nio.file.Paths; +import java.text.SimpleDateFormat; +import java.util.ArrayList; +import java.util.Calendar; +import java.util.function.DoubleSupplier; +import java.util.function.IntSupplier; + +import org.strongback.Executable; +import org.strongback.Strongback; +import org.strongback.components.Switch; +import org.strongback.components.ui.DirectionalAxis; + +import frc.robot.Constants; +import frc.robot.interfaces.LogWriter; +import frc.robot.lib.RobotName; +import frc.robot.lib.log.LogFileNumber; +import frc.robot.lib.log.Log; +import frc.robot.lib.log.NullLogWriter; +import frc.robot.lib.log.TimestampedLogWriter; + +/** + * Writes data out in a CSV file for charting. Creates html files for + * viewing the data via the web server. + */ +public class Chart implements Executable { + + /** + * Register a data stream for collection to the log. + * The sample needs to have a method that returns a double which is the data element. + * The format (and args) are used to construct the name of the data stream. + * @param sample An object that returns a double value when called + * @param format a VarArgs string which evaluates to the name of the data stream + * @param args Any arguments required for the format varargs + */ + public static synchronized void register(DoubleSupplier sample, String format, Object... args) { + if (state == State.ERRORED) { + // The log system is very broken, possibly because of a missing or faulty USB + // flash drive. + return; + } + if (state == State.CREATED) { + columns.add(new Column(sample, String.format(format, args))); + return; + } + // Invalid state. Create a stack trace to help with debugging. + // It's likely that registrationComplete() has already been called. + try { + throw new Exception("Tried to add sample in an invalid state: " + state); + } catch (Exception e) { + Log.exception("Failure to register: ", e); + } + } + + /** + * Register a data stream for collection to the log. + * The sample needs to have a method that returns an integer which is the data element. + * The format (and args) are used to construct the name of the data stream. + * @param sample An object that returns an integer value when called + * @param format a VarArgs string which evaluates to the name of the data stream + * @param args Any arguments required for the format varargs + */ + public static void register(IntSupplier sample, String format, Object... args) { + register(() -> (double)sample.getAsInt(), format, args); + } + + /** + * Register a data stream for collection to the log. + * The sample needs to have a method that returns a DirectionalAxis which is the data element. + * The format (and args) are used to construct the name of the data stream. + * @param sample An object that returns a DirectionalAxis value when called + * @param format a VarArgs string which evaluates to the name of the data stream + * @param args Any arguments required for the format varargs + */ + public static void register(DirectionalAxis sample, String format, Object... args) { + register(() -> (double)sample.getDirection(), format, args); + } + + /** + * Register a data stream for collection to the log. + * The sample needs to have a method that returns a Switch which is the data element. + * The format (and args) are used to construct the name of the data stream. + * @param sample An object that returns a Switch value when called + * @param format a VarArgs string which evaluates to the name of the data stream + * @param args Any arguments required for the format varargs + */ + public static void register(Switch sample, String format, Object... args) { + register(() -> (double)(sample.isTriggered()?1:0), format, args); + } + + /** + * Call when all registrations have been done and recording should start. + */ + public static synchronized void registrationComplete(String matchDescription) { + Chart.matchDescription = matchDescription; + state = State.CONFIGURED; + Log.debug("Chart: Column additions completed"); + } + + /** + * Restarts logging, called each time robot is enabled or initialised. + */ + public synchronized static void restartCharts() { + csvWriter.flush(); + init(); + // Jump straight to configured as all the columns have already been registered. + state = State.CONFIGURED; + } + + // Implementation + + private enum State { + INVALID, // File has not yet been created + CREATED, // File has been created, but we are waiting for all logging classes to be created + CONFIGURED, // Logging classes are all created and have registered with the logging subsystem + ACTIVE, // .html files have been populated and we are ready to write records into the .csv file. + ERRORED // a problem has occurred. Abandon trying to write. + } + + /** + * Holds a name and a way to get a value for it. + * For example "battery voltage" and a callback to query it. + */ + private static class Column { + public String name; + public DoubleSupplier sample; + + public Column(DoubleSupplier sample, String name, Object... args) { + this.name = String.format(name, args); + this.sample = sample; + } + } + + // Log file. + private static LogWriter csvWriter = new NullLogWriter(); + private static LogWriter chartHTMLWriter = new NullLogWriter(); + private static LogWriter locationHTMLWriter = new NullLogWriter(); + // Internal state. + private static State state = State.INVALID; + private static String matchDescription = "Invalid"; + // Registered samples/columns. + private static ArrayList columns = new ArrayList(); + private static Double timeOffset = 0.0; // Make the graphs always start at zero. + private static boolean createdDateFiles = false; + + static { + // Setup the files. + init(); + } + + /** + * Creates new log files on request. + */ + private static synchronized void init() { + // Set the graphLogState to INVALID as the new files have not yet been created. + state = State.INVALID; + createdDateFiles = false; + + // Get time since robot boot, so chart starts at time = 0. + timeOffset = Strongback.timeSystem().currentTime(); + try { + if (System.getProperty("user.name").equals("lvuser")) { + // Running on the robot. Write for real. + String baseDir = Paths.get(Constants.LOG_BASE_PATH, RobotName.get()).toString(); + long logNum = LogFileNumber.get(); + // Open all files. Also creates Latest symlink. + csvWriter = new TimestampedLogWriter(baseDir, "data", logNum, "csv"); + chartHTMLWriter = new TimestampedLogWriter(baseDir, "chart", logNum, "html"); + locationHTMLWriter = new TimestampedLogWriter(baseDir, "location", logNum, "html"); + } + + // Everything was successfully created, we're good to go. + state = State.CREATED; + } catch (IOException e) { + e.printStackTrace(); + System.err.printf("Failed to create log files in %s: %s\n", Constants.LOG_BASE_PATH, e.getMessage()); + state = State.ERRORED; + } + } + + /** + * execute is called periodically. + */ + @Override + public void execute(long timeInMillis) { + update(); + } + + /** + * Called periodically to record the latest values and write them to file. + */ + private static synchronized void update() { + if (state == State.CONFIGURED) { + String csvColumns = getGraphHeaders(); + initCSVFile(csvColumns); + initChartFile(csvColumns); + initLocationFile(); + state = State.ACTIVE; + } + if (state == State.ACTIVE) { + csvWriter.write(getGraphValues()); + if (createdDateFiles) return; + // If there is a valid date and time from the drivers station, + // create the dated links to the files. + Calendar now = Calendar.getInstance(); + if (now.get(Calendar.YEAR) >= 2017) { + createDateFiles(now); + createdDateFiles = true; + } + } + } + + /** + * Insert the header row into the CSV file now that all the columns are known. + */ + private static void initCSVFile(String csvColumns) { + csvWriter.write(csvColumns + "\n"); + } + + /** + * Create the html file for viewing the data as a plot.ly chart. + */ + private static void initChartFile(String csvColumns) { + String title = "Run " + LogFileNumber.get(); + String file = String.format("data_%05d", LogFileNumber.get()); + chartHTMLWriter.write(String.format("\n" + "%1$s plot.ly chart\n" + "\n" + + "\n" + "\n" + + "\n" + + "

\n" + + "\n\n" + "

\n" + + "Click on the series in the legend to swap y-axis and then to turn off.\n" + "

\n" + + "Trying to run this locally? Run the following in the directory containing this file:\n" + "

\n" + + "

\n" + " python -m SimpleHTTPServer\n" + "
\n" + "

\n" + + "Then go to http://localhost:8000/Latest_chart.html\n" + + "\n", title, file, csvColumns)); + chartHTMLWriter.close(); + } + + /** + * Create the html file for the location plot. + */ + private static void initLocationFile() { + String title = "Instance " + LogFileNumber.get(); + String file = String.format("data_%05d", LogFileNumber.get()); + locationHTMLWriter.write(String.format("%1$s\n" + + "\n" + + "

\n" + + "
\n" + + "" + "

\n" + + "Trying to run this locally? Run the following in the directory containing this file:\n" + "

\n" + + "

\n" + " python -m SimpleHTTPServer\n" + "
\n" + "

\n" + + "Then go to http://localhost:8000/Latest_chart.html\n" + + "\n", title, file)); + locationHTMLWriter.close(); + } + + private static String getGraphHeaders() { + String headers = "date"; + for (Column e : columns) { + if (e.name != null) { + headers = headers + "," + e.name; + } + } + return headers; + } + + public static String getGraphValues() { + StringBuffer s = new StringBuffer(); + // Subtracts time offset from current time so graph starts at time = 0 + s.append(String.format("%.3f", Strongback.timeSystem().currentTime() - timeOffset)); + for (Column e : columns) { + if (e.name != null) { + s.append(","); + s.append(e.sample.getAsDouble()); + } + } + s.append("\n"); + return s.toString(); + } + + /** + * Create the date based symbolic links. These create symbolic links from date + * stamped version of the file to the actual file. This is a separate method as + * we delay creating these until we are reasonably sure the date is correct. + * + * @param timestamp The date to use. + */ + public static void createDateFiles(Calendar timestamp) { + String timestampStr = new SimpleDateFormat("yyyyMMdd_HH-mm-ss.SSS").format(timestamp.getTime()); + try { + // Create links based on the timestamp. + csvWriter.createSymbolicLink(Constants.LOG_DATE_EXTENSION, timestampStr); + chartHTMLWriter.createSymbolicLink(Constants.LOG_DATE_EXTENSION, timestampStr); + locationHTMLWriter.createSymbolicLink(Constants.LOG_DATE_EXTENSION, timestampStr); + // And on event name, match type, match number, replay number, alliance and + // position. These details should be available at the same time now that the + // drivers station is able to talk to the robot. + csvWriter.createSymbolicLink(Constants.LOG_EVENT_EXTENSION, matchDescription); + chartHTMLWriter.createSymbolicLink(Constants.LOG_EVENT_EXTENSION, matchDescription); + locationHTMLWriter.createSymbolicLink(Constants.LOG_EVENT_EXTENSION, matchDescription); + } catch (Exception e) { + e.printStackTrace(); + System.out.printf("Error creating symlinks in %s: %s\n", Constants.LOG_BASE_PATH, e.getMessage()); + } + } +} diff --git a/src/main/java/frc/robot/lib/log/Log.java b/src/main/java/frc/robot/lib/log/Log.java new file mode 100644 index 0000000..9838e4f --- /dev/null +++ b/src/main/java/frc/robot/lib/log/Log.java @@ -0,0 +1,170 @@ +package frc.robot.lib.log; + +/* + * This class provides easy access to logging functions, allowing debugging + * after the program has run and persisting through robot code restarts. + * + * What it does depends on what user starts it. + * - If running as lvuser, it assumes it's running on a RoboRio and will log to + * the flash drive for easy post match analysis. + * - If started by any other user it will just print logging and not write + * anything to disk (eg in unit tests) + * + * For text logs we have a series of calls: + * log.{debug,info,warning,error}(String message) + * + * Each message takes a varargs argument list, and prepends the message with a + * timestamp and type of message. + * + * Static implementation so that it doesn't need to be passed around everywhere. + */ + +import java.io.IOException; +import java.nio.file.Paths; +import java.text.SimpleDateFormat; +import java.util.Calendar; + +import org.strongback.Strongback; + +import frc.robot.Constants; +import frc.robot.interfaces.LogWriter; +import frc.robot.lib.RobotName; + +/** + * This is the log file data logger. We split the log into two separate log systems. + * For the text file logging we use this method. + * + * We create a single text log file /.txt + * and provide a variety of methods to append to that log file. + */ + +public class Log { + /** + * Logs to disk only. For high volume debug messages + * @param message Message to log + * @param args arguments to the message format string + */ + public static void debug(String message, Object... args) { + message = String.format(timeString() + ",D " + message + "\n", args); + // Don't print it to the console. + writer.write(message); + } + + /** + * Logs to disk and console. For low volume informational messages (<1/sec). + * @param message Message to Log + * @param args arguments to the message format string + */ + public static void info(String message, Object... args) { + message = String.format(timeString() +",I " + message + "\n", args); + // Print to the console. + System.out.print(message); + writer.write(message); + } + + /** + * Logs to disk and console. For warning Messages. + * @param message Message to Log + * @param args arguments to the message format string + */ + public static void warning(String message, Object... args) { + message = String.format(timeString() +",W " + message + "\n", args); + // Print to the console. + System.err.print(message); + writer.write(message); + } + + /** + * Logs to disk and console. For important errors Messages. + * @param message Message to Log + * @param args arguments to the message format string + */ + public static void error(String message, Object... args) { + message = String.format(timeString() +",E " + message + "\n", args); + // Print to the console. + System.err.print(message); + writer.write(message); + } + + /** + * Logs Exceptions to disk and console. + * @param message Message to Log + * @param e Exception to log after message + */ + public static void exception(String message, Exception e) { + error(message + ": %s", e); + for (StackTraceElement frame : e.getStackTrace()) { + error(" at %s", frame.toString()); + } + } + + /** + * Implements the classic println interface. Single string to the console! + * @param message Message to Log + */ + public static void println(String message, Object... args) { + message = String.format(timeString() + ",O " + message + "\n", args); + // Print to the console. + System.out.print(message); + writer.write(message); + } + + /** + * Restart logging. Called each time robot is enabled or initialised. + */ + public static void restartLogs() { + writer.flush(); + // Make the time start at zero within the log file. + timeOffset = Strongback.timeSystem().currentTime(); + // Create a new logger to get new files. + writer = createWriter(); + } + + /** + * Create the date based symbolic links. These create symbolic links from date stamped + * version of the file to the actual file. This is a separate method as we delay + * creating these until we are reasonably sure the date is correct. + * @param timestamp The date to use. + */ + public static void createDateFiles(Calendar timestamp, String matchDescription) { + String timestampStr = new SimpleDateFormat("yyyyMMdd_HH-mm-ss.SSS").format(timestamp.getTime()); + try { + // Create links based on the timestamp. + writer.createSymbolicLink(Constants.LOG_DATE_EXTENSION, timestampStr); + writer.createSymbolicLink(Constants.LOG_EVENT_EXTENSION, matchDescription); + } catch (Exception e) { + System.out.printf("Error creating logging symlinks\n"); + e.printStackTrace(); + } + } + + // Implementation only from here. + + private static double timeOffset = 0; + private static LogWriter writer = createWriter(); + + private static LogWriter createWriter() { + if (System.getProperty("user.name").equals("lvuser")) { + // Running on the robot. Log for real. + String baseDir = Paths.get(Constants.LOG_BASE_PATH, RobotName.get()).toString(); + long logNum = LogFileNumber.get(); + try { + return new TimestampedLogWriter(baseDir, "log", logNum, "txt"); + } catch (IOException e) { + System.err.println("Failed to create logger, maybe usb flash drive isn't plugged in?"); + e.printStackTrace(); + // Fall through to create a NullLogWriter(); + } + } + // Likely a unit test, only write to the console. + return new NullLogWriter(); + } + + /* + * Create the timestamp for this message. We use the robot time, so each log + * entry is time stamped for when it happened during the match. + */ + private static String timeString() { + return String.format("%.3f", Strongback.timeSystem().currentTime() - timeOffset); + } +} diff --git a/src/main/java/frc/robot/lib/log/LogFileNumber.java b/src/main/java/frc/robot/lib/log/LogFileNumber.java new file mode 100644 index 0000000..4e222bc --- /dev/null +++ b/src/main/java/frc/robot/lib/log/LogFileNumber.java @@ -0,0 +1,81 @@ +package frc.robot.lib.log; + +import java.io.BufferedReader; +import java.io.BufferedWriter; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Paths; + +import frc.robot.Constants; +import frc.robot.lib.RobotName; + +/** + * Returns the next unique log file number to write to. + * + * To make the log files easier to find they should to be timestamped so that the + * run at a specific time can be found. + * + * The roborio doesn't have a battery powered clock so when it first starts it + * doesn't have the time and it will start from the same timestamp every time. + * The time is supplied by the drivers station when it connects. + * + * Unfortunately the code (and the logging of its output) will start well + * before this. + * + * To work around this, the logs are witten to unique incrementing file names + * and when the real time is set on the roborio when the drivers station + * connects a symbolic link (think shortcut) is created named with the current + * time. We also know the match number at that time, so another link is created + * based on the match number. + * + * This class supplies the number that makes the incrementing number. + * + * The different log files (chart, etc) all share the same number and they + * are all recreated with a new number when the robot is enabled so that output + * of every enable is in a new file. + */ +public class LogFileNumber { + static long value = 0; + + static { + // Increment and get the number from disk. + increment(); + } + + /** + * Returns the current unique log file number. + * + * @return the log file number to use. + */ + public static long get() { + return value; + } + + /** + * Reads the file and increments (or uses 1 if no such file exists) and writes + * the log file number back out. + */ + public static void increment() { + var path = Paths.get(Constants.LOG_BASE_PATH, RobotName.get(), "lognumber.txt"); + try { + BufferedReader br = Files.newBufferedReader(path); + String s = br.readLine(); // read the line into the buffer. + value = Integer.parseInt(s); + br.close(); + value++; + } catch (IOException | NumberFormatException e) { + System.err.printf("Cannot read %s. Resetting log number to 1.\n", path.toString()); + value = 1; + } + try { + BufferedWriter bw = Files.newBufferedWriter(path); + bw.write(value + "\n"); + bw.flush(); + bw.close(); + System.out.printf("Wrote %d to %s\n", value, path); + } catch (IOException e) { + e.printStackTrace(); + System.out.println("Cannot write log number file. Possible old file overwrite."); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/log/NullLogWriter.java b/src/main/java/frc/robot/lib/log/NullLogWriter.java new file mode 100644 index 0000000..c0555a5 --- /dev/null +++ b/src/main/java/frc/robot/lib/log/NullLogWriter.java @@ -0,0 +1,26 @@ +package frc.robot.lib.log; + +import frc.robot.interfaces.LogWriter; + +/** + * Throws away log messages. Useful for unit tests that logging + * doesn't need to be persisted. + */ +public class NullLogWriter implements LogWriter { + + public NullLogWriter() {} + + @Override + public void write(String contents) { + // Don't do anything with it. + } + + @Override + public void flush() {} + + @Override + public void close() {} + + @Override + public void createSymbolicLink(String dir, String prefix) {} +} diff --git a/src/main/java/frc/robot/lib/log/TimestampedLogWriter.java b/src/main/java/frc/robot/lib/log/TimestampedLogWriter.java new file mode 100644 index 0000000..52bdbd5 --- /dev/null +++ b/src/main/java/frc/robot/lib/log/TimestampedLogWriter.java @@ -0,0 +1,147 @@ +package frc.robot.lib.log; + +import java.io.BufferedWriter; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; + +import frc.robot.interfaces.LogWriter; + +/** + * Creates the files and the symbolic links for a single stream of data + * normally on the USB flash drive. + * + * 'Latest' symlink created to point to this file. + */ +public class TimestampedLogWriter implements LogWriter { + + private final String name; // eg chart + private final String extn; // eg "html" or "csv" + private final String baseDir; // All logs are below this directory + private Path filePath = null; + private BufferedWriter writer = null; + + /** + * Write free form data to a file and create multiple symbolic links to it. + * Used for csv and graphing files. + * + * Creates + * 

+	 *    baseDir/
+   	 *            data/name_000filenum.extn
+   	 *            latest/Latest_name.extn -> data/name_filenum.extn
+	 *            date/timestamp_name.extn -> ../data/name_filenum.extn 
+	 *			  event/event_match_name.extn -> ../data/name_filenum.extn
+	 *
+ * + * @param baseDir Where on the file system to put the logging directories. + * @param name the type of data, eg "data", "chart" + * @param filenum the number of the file. Incremented every start of the code. + * @param extn the file extension + * @throws IOException + */ + public TimestampedLogWriter(String baseDir, String name, long filenum, String extn) throws IOException { + this.baseDir = baseDir; + this.name = name; + this.extn = extn; + // The absolute path to the data file so we can write to the file. + filePath = Paths.get(baseDir, "data", String.format("%s_%05d.%s", name, filenum, extn)); + // Ensure the parent directory exists. + Files.createDirectories(filePath.getParent()); + // Create the file writer. + writer = Files.newBufferedWriter(filePath); + createSymbolicLink("latest/Latest"); + } + + /** + * Create symbolic links to the file. Used to create Latest_x and dated + * symlinks. This version also takes a directory. + * + * @param dir sub directory relative to logging base dir to put link in. + * @param prefix as in _chart.html + */ + @Override + public void createSymbolicLink(String dir, String prefix) { + Path path = Paths.get(dir, prefix); + createSymbolicLink(path.toString()); + } + + /** + * Create symbolic links to the file. Used to create Latest_x and dated + * symlinks. + * + * @param prefix as in _chart.html + */ + private void createSymbolicLink(String prefix) { + Path symlinkPath = Paths.get(baseDir, String.format("%s_%s.%s", prefix, name, extn)); + createSymbolicLink(symlinkPath, filePath); + } + + /** + * Create symbolic links to the file. Used to create Latest_x and dated + * symlinks. + * + * @param prefix as in _chart.html + */ + private static void createSymbolicLink(Path from, Path to) { + Path relPath = createRelativePath(from, to); + try { + // Ensure the parent directory exists. + Files.createDirectories(from.getParent()); + Files.deleteIfExists(from); + Files.createSymbolicLink(from, relPath); + } catch (Exception e) { + //e.printStackTrace(); + System.err.printf("Failed to create symbolic link: Are we on windows? %s", e); + } + } + + /** + * Does some relative path magic to ensure that the target is + * relative to the file so that it will work no matter where the file system + * is mounted. + * + * Examples: + * prefix="Latest" creates a symlink "Latest_chart.html" -> "data/chart_00007.html" + * prefix="date/20180303" creates a symlink "date/20180303_chart.html" -> "../data/chart_00007.html" + * + * @param from the full path to the symlink. + * @param to the full path to the file to link to. + * @return the relative path to the file to link to from from's perspective + */ + private static Path createRelativePath(Path from, Path to) { + // Make a relative path out of this path so the symbolic link will continue to + // work no matter where the USB flash drive is mounted. + return from.getParent().relativize(to); + } + + @Override + public void write(String contents) { + if (writer == null) return; // File logging not enabled. + try { + writer.write(contents); + writer.flush(); + } catch (Exception e) { + // nothing to do. If we can't write to the log file it's not a disaster. + } + } + + @Override + public void flush() { + try { + writer.flush(); + } catch (IOException e) { + // nothing to do. If we can't write to the log file it's not a disaster. + } + } + + @Override + public void close() { + try { + writer.close(); + } catch (IOException e) { + e.printStackTrace(); + } + } +} diff --git a/src/main/java/frc/robot/mock/MockBuddyClimb.java b/src/main/java/frc/robot/mock/MockBuddyClimb.java index 43deecc..fa00caf 100644 --- a/src/main/java/frc/robot/mock/MockBuddyClimb.java +++ b/src/main/java/frc/robot/mock/MockBuddyClimb.java @@ -1,12 +1,11 @@ package frc.robot.mock; import frc.robot.interfaces.BuddyClimbInterface; -import frc.robot.interfaces.Log; public class MockBuddyClimb implements BuddyClimbInterface { private boolean isExtended = false; - public MockBuddyClimb(Log log) { + public MockBuddyClimb() { } @Override diff --git a/src/main/java/frc/robot/mock/MockColourWheel.java b/src/main/java/frc/robot/mock/MockColourWheel.java index 735751b..7811acf 100644 --- a/src/main/java/frc/robot/mock/MockColourWheel.java +++ b/src/main/java/frc/robot/mock/MockColourWheel.java @@ -1,7 +1,6 @@ package frc.robot.mock; import frc.robot.interfaces.ColourWheelInterface; -import frc.robot.interfaces.Log; import frc.robot.lib.WheelColour; import frc.robot.interfaces.ColourWheelInterface.ColourAction.ColourWheelType; @@ -9,7 +8,7 @@ public class MockColourWheel implements ColourWheelInterface { private ColourAction action = new ColourAction(ColourWheelType.NONE, WheelColour.UNKNOWN); private boolean extended = false; - public MockColourWheel(Log log) { + public MockColourWheel() { } @Override diff --git a/src/main/java/frc/robot/mock/MockDrivebase.java b/src/main/java/frc/robot/mock/MockDrivebase.java index a660050..a417a7e 100644 --- a/src/main/java/frc/robot/mock/MockDrivebase.java +++ b/src/main/java/frc/robot/mock/MockDrivebase.java @@ -2,16 +2,14 @@ import frc.robot.drive.routines.DriveRoutine; import frc.robot.interfaces.DrivebaseInterface; -import frc.robot.interfaces.Log; public class MockDrivebase implements DrivebaseInterface { private DriveRoutineParameters parameters = new DriveRoutineParameters(DriveRoutineType.ARCADE_DUTY_CYCLE); private boolean ClimbModeEnabled = false; private boolean BrakeApplied = false; String name = "MockDrivebase"; - Log log; - public MockDrivebase(Log log) { - this.log = log; + ; + public MockDrivebase() { } @Override diff --git a/src/main/java/frc/robot/mock/MockIntake.java b/src/main/java/frc/robot/mock/MockIntake.java index 52945c4..ad99797 100644 --- a/src/main/java/frc/robot/mock/MockIntake.java +++ b/src/main/java/frc/robot/mock/MockIntake.java @@ -1,7 +1,6 @@ package frc.robot.mock; import frc.robot.interfaces.IntakeInterface; -import frc.robot.interfaces.Log; /** @@ -11,7 +10,7 @@ public class MockIntake implements IntakeInterface { private double output = 0; private boolean isExtended = false; - public MockIntake(Log log) { + public MockIntake() { } @Override diff --git a/src/main/java/frc/robot/mock/MockLoader.java b/src/main/java/frc/robot/mock/MockLoader.java index a7e72eb..1d23cd3 100644 --- a/src/main/java/frc/robot/mock/MockLoader.java +++ b/src/main/java/frc/robot/mock/MockLoader.java @@ -1,13 +1,12 @@ package frc.robot.mock; import frc.robot.interfaces.LoaderInterface; -import frc.robot.interfaces.Log; public class MockLoader implements LoaderInterface { private double spinnerRPS = 0; private double passthroughPower = 0; - public MockLoader(Log log) { + public MockLoader() { } // Paddle private boolean isPaddleBlocking = false; diff --git a/src/main/java/frc/robot/mock/MockLog.java b/src/main/java/frc/robot/mock/MockLog.java deleted file mode 100644 index 4f04d3c..0000000 --- a/src/main/java/frc/robot/mock/MockLog.java +++ /dev/null @@ -1,121 +0,0 @@ -package frc.robot.mock; - -import java.util.function.DoubleSupplier; - -import frc.robot.interfaces.Log; - -public class MockLog implements Log { - - protected boolean logToStdOut = false; - - public MockLog() { - this(false); - } - - public MockLog(boolean logToStdOut) { - this.logToStdOut = logToStdOut; - } - @Override - public Log pauseGraphLog() { - return this; - } - - @Override - public Log resumeGraphLog() { - return this; - } - - @Override - public synchronized Log logMessage(String message, Object... args) { - if (!logToStdOut) return this; - System.out.printf(message, args); - System.out.println(); - return this; - } - - @Override - public Log debug(String message, Object... args) { - logMessage(message, args); - return this; - } - - @Override - public Log info(String message, Object... args) { - logMessage(message, args); - return this; - } - - @Override - public Log warning(String message, Object... args) { - logMessage(message, args); - return this; - } - - @Override - public Log error(String message, Object... args) { - logMessage(message, args); - return this; - } - - @Override - public Log exception(String message, Exception e) { - error(message + ": %s", e); - for (StackTraceElement frame : e.getStackTrace()) { - error(" at %s", frame.toString()); - } - return this; - } - - @Override - public Log println(String message) { - logMessage(message); - return this; - } - - @Override - public Log console(String message, Object... args) { - logMessage(message, args); - return this; - } - - @Override - public Log cmd(String message, Object... args) { - logMessage(message, args); - return this; - } - - @Override - public Log sub(String message, Object... args) { - logMessage(message, args); - return this; - } - - @Override - public Log ctrl(String message, Object... args) { - logMessage(message, args); - return this; - } - - @Override - public Log logCompletedElements(String matchDescription) { - return this; - } - - @Override - public Log doRegister(boolean local, DoubleSupplier sample, String format, Object... args) { - return this; - } - - @Override - public void execute(long timeInMillis) { - } - - public Log regiser(boolean local, DoubleSupplier sample, String format, Object... args) { - return doRegister(local, sample, format, args); - } - - @Override - public Log flush() { - return this; - } -} diff --git a/src/main/java/frc/robot/mock/MockShooter.java b/src/main/java/frc/robot/mock/MockShooter.java index 603fad0..0ae84fa 100644 --- a/src/main/java/frc/robot/mock/MockShooter.java +++ b/src/main/java/frc/robot/mock/MockShooter.java @@ -1,14 +1,13 @@ package frc.robot.mock; import frc.robot.interfaces.ShooterInterface; -import frc.robot.interfaces.Log; public class MockShooter implements ShooterInterface { private double targetRPS = 0; private boolean isExtended = false; - public MockShooter(Log log) { + public MockShooter() { } @Override diff --git a/src/main/java/frc/robot/subsystems/BuddyClimb.java b/src/main/java/frc/robot/subsystems/BuddyClimb.java index 3fe2049..325528d 100644 --- a/src/main/java/frc/robot/subsystems/BuddyClimb.java +++ b/src/main/java/frc/robot/subsystems/BuddyClimb.java @@ -4,20 +4,18 @@ import frc.robot.interfaces.BuddyClimbInterface; import frc.robot.interfaces.DashboardInterface; -import frc.robot.interfaces.Log; import frc.robot.lib.Subsystem; +import frc.robot.lib.chart.Chart; public class BuddyClimb extends Subsystem implements BuddyClimbInterface { private Solenoid solenoid; - public BuddyClimb(Solenoid solenoid, DashboardInterface dashboard, Log log) { - super("BuddyClimb", dashboard, log); + public BuddyClimb(Solenoid solenoid, DashboardInterface dashboard) { + super("BuddyClimb", dashboard); this.solenoid = solenoid; - // Motors, sensors here etc…. - log.register(true, () -> isExtended(), "%s/extended", name) - .register(true, () -> isRetracted(), "%s/retracted", name); - // More logging here... + Chart.register(() -> isExtended(), "%s/extended", name); + Chart.register(() -> isRetracted(), "%s/retracted", name); } @Override @@ -49,6 +47,5 @@ public boolean isRetracted() { public void updateDashboard() { dashboard.putString("Buddy climb position", isExtended() ? "extended" : isRetracted() ? "retracted" : "moving"); - // Motors, sensors here etc…. } } diff --git a/src/main/java/frc/robot/subsystems/ColourWheel.java b/src/main/java/frc/robot/subsystems/ColourWheel.java index 9ee4601..b49b11c 100644 --- a/src/main/java/frc/robot/subsystems/ColourWheel.java +++ b/src/main/java/frc/robot/subsystems/ColourWheel.java @@ -12,10 +12,11 @@ import frc.robot.interfaces.ColourWheelInterface.ColourAction.ColourWheelType; import frc.robot.interfaces.DashboardInterface; import frc.robot.interfaces.LEDStripInterface; -import frc.robot.interfaces.Log; import frc.robot.lib.LEDColour; import frc.robot.lib.Subsystem; import frc.robot.lib.WheelColour; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; /** * This subsystem is made to spin the Colour Wheel on the control panel in the @@ -58,19 +59,19 @@ public class ColourWheel extends Subsystem implements ColourWheelInterface { private final Supplier colourSensor; public ColourWheel(Motor motor, Solenoid solenoid, Supplier colourSensor, LEDStripInterface ledStrip, - Clock clock, DashboardInterface dash, Log log) { - super("ColourWheel", dash, log); - log.info("Creating Colour Wheel Subsystem"); + Clock clock, DashboardInterface dash) { + super("ColourWheel", dash); + Log.info("Creating Colour Wheel Subsystem"); this.motor = motor; this.clock = clock; this.colourSensor = colourSensor; this.ledStrip = ledStrip; this.solenoid = solenoid; - log.register(false, () -> (double) colour.id, "%s/colour", name) - .register(false, () -> (double) rotCount, "%s/rotCount", name) - .register(false, () -> (double) motor.getOutputPercent(), "%s/motorspeed", name) - .register(false, () -> (double) nextColour.id, "%s/nextColour", name) - .register(false, () -> (double) spinTime, "%s/spinTime", name); + Chart.register(() -> (double) colour.id, "%s/colour", name); + Chart.register(() -> (double) rotCount, "%s/rotCount", name); + Chart.register(() -> (double) motor.getOutputPercent(), "%s/motorspeed", name); + Chart.register(() -> (double) nextColour.id, "%s/nextColour", name); + Chart.register(() -> (double) spinTime, "%s/spinTime", name); } public void update() { @@ -97,7 +98,7 @@ public void update() { newSpeed = Constants.COLOUR_WHEEL_MOTOR_OFF; break; default: - log.error("%s: Unknown Type %s", name, action.type); + Log.error("%s: Unknown Type %s", name, action.type); break; } if (newSpeed != speed) { @@ -132,14 +133,14 @@ public void update() { */ public double rotationalControl() { if (colour.equals(nextColour)) { - log.info("%s: Found %s.", name, colour); - log.info("%s: Added one to rotations. %d", name, rotCount); + Log.info("%s: Found %s.", name, colour); + Log.info("%s: Added one to rotations. %d", name, rotCount); rotCount += 1; firstLoop = true; } if (firstLoop) { nextColour = colour.next(speed); - log.info("%s: Next Colour is %s.", name, nextColour); + Log.info("%s: Next Colour is %s.", name, nextColour); firstLoop = false; } if (rotCount < Constants.COLOUR_WHEEL_ROTATION_TARGET) { @@ -201,7 +202,7 @@ public double positionalControl(WheelColour desired) { return motor.get(); } else { action = new ColourAction(ColourWheelType.NONE, WheelColour.UNKNOWN); - log.info("ColourWheel: Desired colour found."); + Log.info("ColourWheel: Desired colour found."); return Constants.COLOUR_WHEEL_MOTOR_OFF; } } @@ -235,7 +236,7 @@ private WheelColour doubleCheck(WheelColour sensedColour) { public ColourWheelInterface setDesiredAction(ColourAction action) { this.action = action; if (action == new ColourAction(ColourWheelType.POSITION, WheelColour.UNKNOWN)) { - log.error("%s: No colour found in FMS!", name); + Log.error("%s: No colour found in FMS!", name); setDesiredAction(new ColourAction(ColourWheelType.NONE, WheelColour.UNKNOWN)); } firstLoop = true; diff --git a/src/main/java/frc/robot/subsystems/Drivebase.java b/src/main/java/frc/robot/subsystems/Drivebase.java index 3b3a34f..a48645a 100644 --- a/src/main/java/frc/robot/subsystems/Drivebase.java +++ b/src/main/java/frc/robot/subsystems/Drivebase.java @@ -11,8 +11,9 @@ import frc.robot.drive.routines.DriveRoutine; import frc.robot.interfaces.DashboardInterface; import frc.robot.interfaces.DrivebaseInterface; -import frc.robot.interfaces.Log; import frc.robot.lib.Subsystem; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; /** * Subsystem responsible for the drivetrain @@ -31,37 +32,35 @@ public class Drivebase extends Subsystem implements DrivebaseInterface { private final Motor right; private Solenoid ptoSolenoid; private Solenoid brakeSolenoid; - private final Log log; private DriveMotion currentMotion; public Drivebase(Motor left, Motor right, Solenoid ptoSolenoid, Solenoid brakeSolenoid, - DashboardInterface dashboard, Log log) { - super("Drive", dashboard, log); + DashboardInterface dashboard) { + super("Drive", dashboard); this.left = left; this.right = right; this.ptoSolenoid = ptoSolenoid; this.brakeSolenoid = brakeSolenoid; - this.log = log; currentMotion = new DriveMotion(0, 0); - routine = new ConstantDrive("Constant Drive", ControlMode.DutyCycle, log); + routine = new ConstantDrive("Constant Drive", ControlMode.DutyCycle); disable(); // disable until we are ready to use it. - log.register(true, () -> currentMotion.left, "%s/setpoint/Left", name) - .register(true, () -> currentMotion.right, "%s/setpoint/Right", name) - .register(false, () -> left.getPosition(), "%s/position/Left", name) - .register(false, () -> right.getPosition(), "%s/position/Right", name) - .register(false, () -> left.getSpeed(), "%s/speed/Left", name) - .register(false, () -> right.getSpeed(), "%s/speed/Right", name) - .register(false, () -> left.getOutputVoltage(), "%s/outputVoltage/Left", name) - .register(false, () -> right.getOutputVoltage(), "%s/outputVoltage/Right", name) - .register(false, () -> left.getOutputPercent(), "%s/outputPercentage/Left", name) - .register(false, () -> right.getOutputPercent(), "%s/outputPercentage/Right", name) - .register(false, () -> left.getOutputCurrent(), "%s/outputCurrent/Left", name) - .register(false, () -> right.getOutputCurrent(), "%s/outputCurrent/Right", name) - .register(true, () -> isClimbModeEnabled(), "%s/extended", name) - .register(true, () -> isDriveModeEnabled(), "%s/retracted", name) - .register(true, () -> isBrakeApplied(), "%s/extended", name) - .register(true, () -> isBrakeReleased(), "%s/retracted", name); + Chart.register(() -> currentMotion.left, "%s/setpoint/Left", name); + Chart.register(() -> currentMotion.right, "%s/setpoint/Right", name); + Chart.register(() -> left.getPosition(), "%s/position/Left", name); + Chart.register(() -> right.getPosition(), "%s/position/Right", name); + Chart.register(() -> left.getSpeed(), "%s/speed/Left", name); + Chart.register(() -> right.getSpeed(), "%s/speed/Right", name); + Chart.register(() -> left.getOutputVoltage(), "%s/outputVoltage/Left", name); + Chart.register(() -> right.getOutputVoltage(), "%s/outputVoltage/Right", name); + Chart.register(() -> left.getOutputPercent(), "%s/outputPercentage/Left", name); + Chart.register(() -> right.getOutputPercent(), "%s/outputPercentage/Right", name); + Chart.register(() -> left.getOutputCurrent(), "%s/outputCurrent/Left", name); + Chart.register(() -> right.getOutputCurrent(), "%s/outputCurrent/Right", name); + Chart.register(() -> isClimbModeEnabled(), "%s/extended", name); + Chart.register(() -> isDriveModeEnabled(), "%s/retracted", name); + Chart.register(() -> isBrakeApplied(), "%s/extended", name); + Chart.register(() -> isBrakeReleased(), "%s/retracted", name); } @Override @@ -89,7 +88,7 @@ public DrivebaseInterface applyBrake(boolean extend) { @Override public void setDriveRoutine(DriveRoutineParameters parameters) { if (this.parameters != null && parameters.equals(this.parameters)) { - log.sub("%s: Parameters are identical not setting these", name); + Log.debug("%s: Parameters are identical not setting these", name); return; } // Drive routine has changed. @@ -97,12 +96,12 @@ public void setDriveRoutine(DriveRoutineParameters parameters) { // Find a routine to handle it DriveRoutine newRoutine = driveModes.getOrDefault(parameters.type, null); if (newRoutine == null) { - log.error("%s: Bad drive mode %s", name, parameters.type); + Log.error("%s: Bad drive mode %s", name, parameters.type); return; } // Tell the drive routine to change what it is doing. newRoutine.reset(parameters); - log.sub("%s: Switching to %s drive routine using ControlMode %s", name, newRoutine.getName(), newRoutine.getControlMode()); + Log.debug("%s: Switching to %s drive routine using ControlMode %s", name, newRoutine.getName(), newRoutine.getControlMode()); if (newRoutine != null) newRoutine.disable(); newRoutine.enable(); routine = newRoutine; @@ -120,7 +119,7 @@ synchronized public void update() { if (routine == null) return; // No drive routine set yet. // Ask for the power to supply to each side. Pass in the current wheel speeds. DriveMotion motion = routine.getMotion(left.getSpeed(), right.getSpeed()); - //log.debug("drive subsystem motion = %.1f, %.1f", motion.left, motion.right); + //Logger.debug("drive subsystem motion = %.1f, %.1f", motion.left, motion.right); if (motion.equals(currentMotion)) { return; // No change. } @@ -170,31 +169,31 @@ public boolean hasFinished() { @Override public void registerDriveRoutine(DriveRoutineType mode, DriveRoutine routine) { - log.sub("%s: Registered %s drive routine", name, routine.getName()); + Log.debug("%s: Registered %s drive routine", name, routine.getName()); driveModes.put(mode, routine); } @Override public boolean isClimbModeEnabled() { - // log.sub("Is intake extended: " + solenoid.isExtended()); + // Logger.debug("Is intake extended: " + solenoid.isExtended()); return ptoSolenoid.isExtended(); } @Override public boolean isDriveModeEnabled() { - // log.sub("Is intake extended: " + solenoid.isExtended()); + // Logger.debug("Is intake extended: " + solenoid.isExtended()); return ptoSolenoid.isRetracted(); } @Override public boolean isBrakeApplied() { - // log.sub("Is intake extended: " + solenoid.isExtended()); + // Logger.debug("Is intake extended: " + solenoid.isExtended()); return brakeSolenoid.isExtended(); } @Override public boolean isBrakeReleased() { - // log.sub("Is intake extended: " + solenoid.isExtended()); + // Logger.debug("Is intake extended: " + solenoid.isExtended()); return brakeSolenoid.isRetracted(); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0e711e5..accd387 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -6,8 +6,9 @@ import frc.robot.interfaces.DashboardInterface; import frc.robot.interfaces.IntakeInterface; -import frc.robot.interfaces.Log; import frc.robot.lib.Subsystem; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; /** * Intake Subsystem 2019: @@ -19,18 +20,18 @@ public class Intake extends Subsystem implements IntakeInterface private Solenoid solenoid; private IntakeWheel intakeWheel; - public Intake(Motor motor, Solenoid solenoid, DashboardInterface dashboard, Log log) { - super("Intake", dashboard, log); + public Intake(Motor motor, Solenoid solenoid, DashboardInterface dashboard) { + super("Intake", dashboard); this.motor = motor; this.solenoid = solenoid; intakeWheel = new IntakeWheel(motor); - log.register(true, () -> isExtended(), "%s/extended", name) - .register(true, () -> isRetracted(), "%s/retracted", name) - .register(false, motor::getOutputVoltage, "%s/outputVoltage", name) - .register(false, motor::getOutputPercent, "%s/outputPercent", name) - .register(false, motor::getOutputCurrent, "%s/outputCurrent", name) - .register(false, () -> intakeWheel.getTargetRPS(), "%s/targetRPS", name) - .register(false, () -> intakeWheel.getRPS(), "%s/rps", name); + Chart.register(() -> isExtended(), "%s/extended", name); + Chart.register(() -> isRetracted(), "%s/retracted", name); + Chart.register(motor::getOutputVoltage, "%s/outputVoltage", name); + Chart.register(motor::getOutputPercent, "%s/outputPercent", name); + Chart.register(motor::getOutputCurrent, "%s/outputCurrent", name); + Chart.register(() -> intakeWheel.getTargetRPS(), "%s/targetRPS", name); + Chart.register(() -> intakeWheel.getRPS(), "%s/rps", name); } @@ -56,7 +57,7 @@ public IntakeInterface setExtended(boolean extend) { @Override public boolean isExtended() { - //log.sub("Is intake extended: " + solenoid.isExtended()); + //Logger.debug("Is intake extended: " + solenoid.isExtended()); return solenoid.isExtended(); } @@ -97,12 +98,12 @@ public void setTargetRPS(double rps) { // change the control mode from percent output, to avoid putting // unnecessary load on the battery and motor. if (rps == 0) { - log.sub("Turning intake wheel off."); + Log.debug("Turning intake wheel off."); motor.set(ControlMode.DutyCycle, 0); } else { motor.set(ControlMode.Speed, rps); } - log.sub("Setting intake target speed to %f", targetRPS); + Log.debug("Setting intake target speed to %f", targetRPS); } public double getTargetRPS() { diff --git a/src/main/java/frc/robot/subsystems/LEDStrip.java b/src/main/java/frc/robot/subsystems/LEDStrip.java index 25bb9cb..03ba595 100644 --- a/src/main/java/frc/robot/subsystems/LEDStrip.java +++ b/src/main/java/frc/robot/subsystems/LEDStrip.java @@ -3,7 +3,6 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; import frc.robot.interfaces.LEDStripInterface; -import frc.robot.interfaces.Log; import frc.robot.lib.LEDColour; import frc.robot.lib.MathUtil; @@ -13,12 +12,9 @@ public class LEDStrip implements LEDStripInterface { public AddressableLED ledStrip; public AddressableLEDBuffer ledStripBuffer; private final int numberOfLEDs; - @SuppressWarnings("unused") - private final Log log; - public LEDStrip(int PWM_Port, int numberOfLEDs, Log log) { + public LEDStrip(int PWM_Port, int numberOfLEDs) { this.numberOfLEDs = numberOfLEDs; - this.log = log; ledStrip = new AddressableLED(PWM_Port); ledStripBuffer = new AddressableLEDBuffer(numberOfLEDs); diff --git a/src/main/java/frc/robot/subsystems/Loader.java b/src/main/java/frc/robot/subsystems/Loader.java index 9d9c6f3..d62eee3 100644 --- a/src/main/java/frc/robot/subsystems/Loader.java +++ b/src/main/java/frc/robot/subsystems/Loader.java @@ -11,9 +11,10 @@ import frc.robot.interfaces.DashboardInterface; import frc.robot.interfaces.DashboardUpdater; import frc.robot.interfaces.LEDStripInterface; -import frc.robot.interfaces.Log; import frc.robot.lib.LEDColour; import frc.robot.lib.Subsystem; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; public class Loader extends Subsystem implements LoaderInterface { final private Motor spinner, passthrough; @@ -26,9 +27,8 @@ public class Loader extends Subsystem implements LoaderInterface { private double targetPassthroughDutyCycle = 0; public Loader(final Motor loaderSpinnerMotor, final Motor loaderPassthroughMotor, final Solenoid paddleSolenoid, - final BooleanSupplier inSensor, final BooleanSupplier outSensor, LEDStripInterface led, final DashboardInterface dashboard, - final Log log) { - super("Loader", dashboard, log); + final BooleanSupplier inSensor, final BooleanSupplier outSensor, LEDStripInterface led, final DashboardInterface dashboard) { + super("Loader", dashboard); this.spinner = loaderSpinnerMotor; this.passthrough = loaderPassthroughMotor; this.paddleSolenoid = paddleSolenoid; @@ -36,26 +36,26 @@ public Loader(final Motor loaderSpinnerMotor, final Motor loaderPassthroughMotor inSensorCount = new Counter("loader:inSensor", inSensor); outSensorCount = new Counter("loader:outSensor", outSensor); - log.register(true, () -> passthrough.getOutputCurrent(), "%s/passthrough/Current", name) - .register(true, () -> passthrough.getOutputPercent(), "%s/passthrough/PercentOut", name) - .register(true, () -> getSpinnerMotorRPS(), "%s/spinner/rps", name) - .register(true, () -> getTargetSpinnerRPS(), "%s/spinner/targetRPS", name) - .register(true, () -> spinner.getOutputCurrent(), "%s/spinner/Current", name) - .register(true, () -> spinner.getOutputPercent(), "%s/spinner/PercentOut", name) - .register(true, () -> (double) getCurrentBallCount(), "%s/spinner/CurrentBallCount", name) - .register(true, () -> (double) inSensorCount.count, "%s/spinner/totalBallsIn", name) - .register(true, () -> (double) outSensorCount.count, "%s/spinner/totalBallsOut", name) - .register(true, () -> (double) initBallCount, "%s/spinner/initialBallCount", name) - .register(true, () -> isPaddleBlocking(), "%s/paddleRetracted", name) - .register(true, () -> inSensor.getAsBoolean(), "%s/spinner/inSensorState", name) - .register(true, () -> outSensor.getAsBoolean(), "%s/spinner/outSensorState", name); + Chart.register(() -> passthrough.getOutputCurrent(), "%s/passthrough/Current", name); + Chart.register(() -> passthrough.getOutputPercent(), "%s/passthrough/PercentOut", name); + Chart.register(() -> getSpinnerMotorRPS(), "%s/spinner/rps", name); + Chart.register(() -> getTargetSpinnerRPS(), "%s/spinner/targetRPS", name); + Chart.register(() -> spinner.getOutputCurrent(), "%s/spinner/Current", name); + Chart.register(() -> spinner.getOutputPercent(), "%s/spinner/PercentOut", name); + Chart.register(() -> (double) getCurrentBallCount(), "%s/spinner/CurrentBallCount", name); + Chart.register(() -> (double) inSensorCount.count, "%s/spinner/totalBallsIn", name); + Chart.register(() -> (double) outSensorCount.count, "%s/spinner/totalBallsOut", name); + Chart.register(() -> (double) initBallCount, "%s/spinner/initialBallCount", name); + Chart.register(() -> isPaddleBlocking(), "%s/paddleRetracted", name); + Chart.register(() -> inSensor.getAsBoolean(), "%s/spinner/inSensorState", name); + Chart.register(() -> outSensor.getAsBoolean(), "%s/spinner/outSensorState", name); } @Override public void setTargetSpinnerRPS(final double rps) { spinnerRPS = rps; - log.sub("%s: Setting loader motor rps to: %f", name, rps); + Log.debug("%s: Setting loader motor rps to: %f", name, rps); // If motor is zero in velocity the PID will try and reverse the motor in order // to slow down if (rps == 0) { @@ -76,7 +76,7 @@ public double getSpinnerMotorRPS() { @Override public void setTargetPassthroughDutyCycle(double percent) { - log.sub("%s: Setting loader in motor percent output to: %f", name, percent); + Log.debug("%s: Setting loader in motor percent output to: %f", name, percent); // If motor is zero in velocity the PID will try and reverse the motor in order // to slow down targetPassthroughDutyCycle = percent; @@ -104,7 +104,7 @@ public LoaderInterface setPaddleBlocking(final boolean block) { @Override public boolean isPaddleNotBlocking() { - // log.sub("Is intake extended: " + solenoid.isExtended()); + // Logger.debug("Is intake extended: " + solenoid.isExtended()); return paddleSolenoid.isRetracted(); } @@ -166,7 +166,7 @@ private class Counter implements DashboardUpdater, Executable { public Counter(final String name, final BooleanSupplier sensor) { this.name = name; this.sensor = sensor; - log.register(false, () -> (double) getCount(), "%s/count", name); + Chart.register(() -> (double) getCount(), "%s/count", name); } public int getCount() { diff --git a/src/main/java/frc/robot/subsystems/Location.java b/src/main/java/frc/robot/subsystems/Location.java index 7a0d00f..f98d460 100644 --- a/src/main/java/frc/robot/subsystems/Location.java +++ b/src/main/java/frc/robot/subsystems/Location.java @@ -11,12 +11,13 @@ import frc.robot.Constants; import frc.robot.interfaces.DashboardInterface; import frc.robot.interfaces.LocationInterface; -import frc.robot.interfaces.Log; import frc.robot.lib.LocationHistory; import frc.robot.lib.MathUtil; import frc.robot.lib.NavXGyroscope; import frc.robot.lib.Position; import frc.robot.lib.Subsystem; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; /** * Location Subsystem. @@ -145,8 +146,8 @@ public double getAsDouble() { * @param gyro The gyro to get angles * @param log The log to store debug and other logging messages */ - public Location(Runnable resetEncoders, DoubleSupplier leftDistance, DoubleSupplier rightDistance, Gyroscope gyro, Clock clock, DashboardInterface dashboard, Log log) { - super("Location", dashboard, log); // always present! + public Location(Runnable resetEncoders, DoubleSupplier leftDistance, DoubleSupplier rightDistance, Gyroscope gyro, Clock clock, DashboardInterface dashboard) { + super("Location", dashboard); // always present! this.resetEncoders = resetEncoders; this.leftDistance = leftDistance; this.rightDistance = rightDistance; @@ -160,16 +161,16 @@ public Location(Runnable resetEncoders, DoubleSupplier leftDistance, DoubleSuppl current = new Position(0, 0, 0, 0, clock.currentTime()); desired = new Position(0, 0, 0, 0, clock.currentTime()); - log.register(true, () -> current.x, "%s/actual/x", name) - .register(true, () -> current.y, "%s/actual/y", name) - .register(true, () -> current.heading, "%s/actual/a", name) - .register(true, () -> current.speed, "%s/s/acutal/speed", name) - .register(true, () -> current.timeSec, "%s/actual/time", name) - .register(true, () -> desired.x, "%s/desired/x", name) - .register(true, () -> desired.y, "%s/desired/y", name) - .register(true, () -> desired.heading, "%s/desired/a", name) - .register(true, () -> desired.speed, "%s/desired/speed", name) - .register(true, () -> desired.timeSec, "%s/desired/time", name); + Chart.register(() -> current.x, "%s/actual/x", name); + Chart.register(() -> current.y, "%s/actual/y", name); + Chart.register(() -> current.heading, "%s/actual/a", name); + Chart.register(() -> current.speed, "%s/s/acutal/speed", name); + Chart.register(() -> current.timeSec, "%s/actual/time", name); + Chart.register(() -> desired.x, "%s/desired/x", name); + Chart.register(() -> desired.y, "%s/desired/y", name); + Chart.register(() -> desired.heading, "%s/desired/a", name); + Chart.register(() -> desired.speed, "%s/desired/speed", name); + Chart.register(() -> desired.timeSec, "%s/desired/time", name); // Enable this subsystem by default. enable(); @@ -192,7 +193,7 @@ public void setCurrentLocation(Position location) { */ @Override public void setCurrentLocation(Pose2d pose) { - log.sub("%s: resetting to: %s", name, pose.toString()); + Log.debug("%s: resetting to: %s", name, pose.toString()); ((NavXGyroscope) gyro).setAngle(pose.getRotation().getDegrees()); current.speed = 0; current.timeSec = clock.currentTime(); // time of last update @@ -294,7 +295,7 @@ public void update() { // Average of the distance - inches. double averageDistance = (newLeft + newRight) / 2.0; - /*log.sub("%s: leftSupplier: %f, leftDelta: %f, rightSupplier: %f, rightDelta: %f", name, + /*Logger.debug("%s: leftSupplier: %f, leftDelta: %f, rightSupplier: %f, rightDelta: %f", name, leftDistanceSupplier.getAsDouble(), newLeft, rightDistanceSupplier.getAsDouble(), newRight); */ @@ -314,7 +315,7 @@ public void update() { history.addLocation(current); if (debug) { - log.debug("%s: %s", name, current.toString()); + Log.debug("%s: %s", name, current.toString()); } } diff --git a/src/main/java/frc/robot/subsystems/OverridableSubsystem.java b/src/main/java/frc/robot/subsystems/OverridableSubsystem.java index 786a264..a788ee1 100644 --- a/src/main/java/frc/robot/subsystems/OverridableSubsystem.java +++ b/src/main/java/frc/robot/subsystems/OverridableSubsystem.java @@ -4,7 +4,8 @@ import java.lang.reflect.Method; import java.lang.reflect.Proxy; -import frc.robot.interfaces.Log; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; /** * A shim layer between a real subsystem and the robot controller so that the @@ -24,7 +25,7 @@ * * USE AT YOUR OWN RISK!! * - * Example: OverrideableSubsystem overridable = new OverridableSubsystem("Lift", LiftInterface.class, lift, liftSimulator, mockLift, log); + * Example: OverrideableSubsystem overridable = new OverridableSubsystem("Lift", LiftInterface.class, lift, liftSimulator, mockLift); * // Get the endpoint that the controller would use. * IntakeInterface normalIntake = intakeOverride.getNormalInterface(); * // Get the endpoint that the diag box uses. @@ -48,7 +49,6 @@ public class OverridableSubsystem { // The real and simulator lifts that commands are sent to depending on the mode. private String name; private SubIF normalInterface, overrideInterface; - private Log log; private enum OverrideMode { AUTOMATIC(1), MANUAL(-1), OFF(0); @@ -62,10 +62,9 @@ private OverrideMode(int value) { private OverrideMode mode = OverrideMode.AUTOMATIC; @SuppressWarnings("unchecked") - public OverridableSubsystem(String name, Class clazz, SubIF real, SubIF simulator, SubIF mock, Log log) { + public OverridableSubsystem(String name, Class clazz, SubIF real, SubIF simulator, SubIF mock) { this.name = name; - this.log = log; - log.register(true, () -> (double)mode.value, "%s/overrideMode", name); + Chart.register(() -> (double)mode.value, "%s/overrideMode", name); // Create the magic that allows this class to pick between which backend is used by what. InvocationHandler normalHandler = new InvocationHandler() { @@ -120,19 +119,19 @@ public SubIF getOverrideInterface() { // Mode change methods. public void setAutomaticMode() { // This may need to be more clever to carry over state. - log.sub("%s switched to normal/automatic mode", name); + Log.debug("%s switched to normal/automatic mode", name); mode = OverrideMode.AUTOMATIC; } public void setManualMode() { // This may need to be more clever to carry over state. - log.sub("%s switched manual mode", name); + Log.debug("%s switched manual mode", name); mode = OverrideMode.MANUAL; } public void turnOff() { // This may need to be more clever to carry over state. - log.sub("%s switched off", name); + Log.debug("%s switched off", name); mode = OverrideMode.OFF; } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 7aa780c..6369746 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -6,9 +6,10 @@ import frc.robot.Constants; import frc.robot.interfaces.DashboardInterface; -import frc.robot.interfaces.Log; import frc.robot.interfaces.ShooterInterface; import frc.robot.lib.Subsystem; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; /** * On the 2020 robot, there are three shooter motors. @@ -19,12 +20,12 @@ public class Shooter extends Subsystem implements ShooterInterface { private final ShooterWheel flyWheel; private final Solenoid hood; - public Shooter(Motor shooterMotor, Solenoid solenoid, DashboardInterface dashboard, Log log) { - super("Shooter", dashboard, log); + public Shooter(Motor shooterMotor, Solenoid solenoid, DashboardInterface dashboard) { + super("Shooter", dashboard); this.hood = solenoid; flyWheel = new ShooterWheel(shooterMotor); - log.register(true, () -> isHoodExtended(), "%s/extended", name) - .register(true, () -> isHoodRetracted(), "%s/retracted", name); + Chart.register(() -> isHoodExtended(), "%s/extended", name); + Chart.register(() -> isHoodRetracted(), "%s/retracted", name); } @Override @@ -80,11 +81,11 @@ protected class ShooterWheel { public ShooterWheel(Motor motor) { this.motor = motor; - log.register(false, () -> getTargetRPS(), "shooter/flyWheel/targetSpeed", name) - .register(false, () -> getRPS(), "shooter/flyWheel/rps", name) - .register(false, motor::getOutputVoltage, "shooter/flyWheel/outputVoltage", name) - .register(false, motor::getOutputPercent, "shooter/flyWheel/outputPercent", name) - .register(false, motor::getOutputCurrent, "shooter/flyWheel/outputCurrent", name); + Chart.register(() -> getTargetRPS(), "shooter/flyWheel/targetSpeed", name); + Chart.register(() -> getRPS(), "shooter/flyWheel/rps", name); + Chart.register(motor::getOutputVoltage, "shooter/flyWheel/outputVoltage", name); + Chart.register(motor::getOutputPercent, "shooter/flyWheel/outputPercent", name); + Chart.register(motor::getOutputCurrent, "shooter/flyWheel/outputCurrent", name); } public void setTargetRPS(double rps) { @@ -96,12 +97,12 @@ public void setTargetRPS(double rps) { // change the control mode from percent output, to avoid putting // unnecessary load on the battery and motor. if (rps == 0) { - log.sub("Turning shooter wheel off."); + Log.debug("Turning shooter wheel off."); motor.set(ControlMode.DutyCycle, 0); } else { motor.set(ControlMode.Speed, rps); } - log.sub("Setting shooter target speed to %f", targetRPS); + Log.debug("Setting shooter target speed to %f", targetRPS); } public double getTargetRPS() { diff --git a/src/main/java/frc/robot/subsystems/Subsystems.java b/src/main/java/frc/robot/subsystems/Subsystems.java index 7900ce5..5aaa481 100644 --- a/src/main/java/frc/robot/subsystems/Subsystems.java +++ b/src/main/java/frc/robot/subsystems/Subsystems.java @@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.I2C; - +import frc.robot.Config; import frc.robot.Constants; import frc.robot.drive.routines.ArcadeDrive; import frc.robot.drive.routines.CheesyDpadDrive; @@ -41,7 +41,6 @@ import frc.robot.interfaces.LEDStripInterface; import frc.robot.interfaces.LoaderInterface; import frc.robot.interfaces.LocationInterface; -import frc.robot.interfaces.Log; import frc.robot.interfaces.ShooterInterface; import frc.robot.interfaces.VisionInterface; import frc.robot.interfaces.VisionInterface.TargetDetails; @@ -52,8 +51,9 @@ import frc.robot.lib.MotorFactory; import frc.robot.lib.NavXGyroscope; import frc.robot.lib.Position; -import frc.robot.lib.RobotConfiguration; import frc.robot.lib.WheelColour; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; import frc.robot.mock.MockBuddyClimb; import frc.robot.mock.MockColourWheel; import frc.robot.mock.MockDrivebase; @@ -73,9 +73,7 @@ public class Subsystems implements DashboardUpdater { // Not really a subsystem, but used by all subsystems. public DashboardInterface dashboard; - public RobotConfiguration config; public Clock clock; - public Log log; public LEDStripInterface ledStrip; public LocationInterface location; public DrivebaseInterface drivebase; @@ -101,11 +99,9 @@ public class Subsystems implements DashboardUpdater { private final I2C.Port i2cPort = I2C.Port.kOnboard; - public Subsystems(DashboardInterface dashboard, RobotConfiguration config, Clock clock, Log log) { + public Subsystems(DashboardInterface dashboard, Clock clock) { this.dashboard = dashboard; - this.config = config; this.clock = clock; - this.log = log; } public void createOverrides() { @@ -115,7 +111,7 @@ public void createOverrides() { } public void enable() { - log.info("Enabling subsystems"); + Log.info("Enabling subsystems"); // location is always enabled. drivebase.enable(); intake.enable(); @@ -125,7 +121,7 @@ public void enable() { } public void disable() { - log.info("Disabling Subsystems"); + Log.info("Disabling Subsystems"); drivebase.disable(); intake.disable(); shooter.disable(); @@ -150,21 +146,21 @@ public void updateDashboard() { * requested by the controller. */ public void createDrivebaseLocation(InputDevice leftStick, InputDevice rightStick) { - if (!config.drivebaseIsPresent) { - log.sub("Using mock drivebase"); - drivebase = new MockDrivebase(log); + if (! Config.drivebase.present) { + Log.debug("Using mock drivebase"); + drivebase = new MockDrivebase(); location = new MockLocation(); - log.sub("Created a mock drivebase and location"); + Log.debug("Created a mock drivebase and location"); return; } // Redundant drive motors - automatic failover if the talon or the encoders // fail. - Motor leftMotor = MotorFactory.getDriveMotor(true, config, clock, log); - Motor rightMotor = MotorFactory.getDriveMotor(false, config, clock, log); - Solenoid ptoSolenoid = Hardware.Solenoids.singleSolenoid(config.pcmCanId, Constants.CLIMBER_PTO_SOLENOID_PORT, + Motor leftMotor = MotorFactory.getDriveMotor(true, clock); + Motor rightMotor = MotorFactory.getDriveMotor(false, clock); + Solenoid ptoSolenoid = Hardware.Solenoids.singleSolenoid(Config.pcm.canId, Config.climber.ptoPort, 0.1, 0.1); // TODO: Test and work out correct timings. - Solenoid brakeSolenoid = Hardware.Solenoids.singleSolenoid(config.pcmCanId, - Constants.CLIMBER_BRAKE_SOLENOID_PORT, 0.1, 0.1); // TODO: Test and work out correct timings. + Solenoid brakeSolenoid = Hardware.Solenoids.singleSolenoid(Config.pcm.canId, + Config.climber.brakePort, 0.1, 0.1); // TODO: Test and work out correct timings. leftDriveDistance = () -> leftMotor.getPosition(); rightDriveDistance = () -> rightMotor.getPosition(); @@ -178,24 +174,24 @@ public void createDrivebaseLocation(InputDevice leftStick, InputDevice rightStic Thread.sleep(100); } catch (InterruptedException e) { } - log.error("Reset drive encoders to zero, currently are: %f, %f", leftMotor.getPosition(), + Log.error("Reset drive encoders to zero, currently are: %f, %f", leftMotor.getPosition(), rightMotor.getPosition()); - Gyroscope gyro = new NavXGyroscope("NavX", config.navxIsPresent, log); + Gyroscope gyro = new NavXGyroscope("NavX", Config.navx.present); gyro.zero(); location = new Location(() -> { leftMotor.setPosition(0); rightMotor.setPosition(0); - }, leftDriveDistance, rightDriveDistance, gyro, clock, dashboard, log); // Encoders must return metres. - drivebase = new Drivebase(leftMotor, rightMotor, ptoSolenoid, brakeSolenoid, dashboard, log); + }, leftDriveDistance, rightDriveDistance, gyro, clock, dashboard); // Encoders must return metres. + drivebase = new Drivebase(leftMotor, rightMotor, ptoSolenoid, brakeSolenoid, dashboard); Strongback.executor().register(drivebase, Priority.HIGH); Strongback.executor().register(location, Priority.HIGH); // Add the supported drive routines drivebase.registerDriveRoutine(DriveRoutineType.CONSTANT_POWER, - new ConstantDrive("Constant Power", ControlMode.DutyCycle, log)); + new ConstantDrive("Constant Power", ControlMode.DutyCycle)); drivebase.registerDriveRoutine(DriveRoutineType.CONSTANT_SPEED, - new ConstantDrive("Constant Speed", ControlMode.Speed, log)); + new ConstantDrive("Constant Speed", ControlMode.Speed)); // The old favourite arcade drive with throttling if a button is pressed. drivebase.registerDriveRoutine(DriveRoutineType.ARCADE_DUTY_CYCLE, @@ -204,28 +200,27 @@ public void createDrivebaseLocation(InputDevice leftStick, InputDevice rightStic .scale(leftStick.getButton(1).isTriggered() ? 1 : 0.6), // Throttle. rightStick.getAxis(0).invert().deadband(Constants.JOYSTICK_DEADBAND_MINIMUM_VALUE) .scale(leftStick.getButton(1).isTriggered() ? 1 : 0.6), // Turn power. - true, log)); + true)); // The old favourite arcade drive with throttling if a button is pressed but // using velocity mode. drivebase.registerDriveRoutine(DriveRoutineType.ARCADE_VELOCITY, - new ArcadeDrive("ArcadeVelocity", ControlMode.Speed, config.drivebaseMaxSpeed, + new ArcadeDrive("ArcadeVelocity", ControlMode.Speed, Config.drivebase.maxSpeed, leftStick.getAxis(1).invert().deadband(Constants.JOYSTICK_DEADBAND_MINIMUM_VALUE) .scale(leftStick.getButton(1).isTriggered() ? 1 : 0.6), // Throttle rightStick.getAxis(0).invert().deadband(Constants.JOYSTICK_DEADBAND_MINIMUM_VALUE) .scale(leftStick.getButton(1).isTriggered() ? 1 : 0.6), // Turn power. - true, log)); + true)); // Cheesy drive. drivebase.registerDriveRoutine(DriveRoutineType.CHEESY, new CheesyDpadDrive("CheesyDPad", leftStick.getDPad(0), // DPad leftStick.getAxis(GamepadButtonsX.LEFT_Y_AXIS), // Throttle leftStick.getAxis(GamepadButtonsX.RIGHT_X_AXIS), // Wheel (turn?) - leftStick.getButton(GamepadButtonsX.RIGHT_TRIGGER_AXIS), // Is quick turn - log)); + leftStick.getButton(GamepadButtonsX.RIGHT_TRIGGER_AXIS))); // Is quick turn // Drive through supplied waypoints using splines. - drivebase.registerDriveRoutine(DriveRoutineType.TRAJECTORY, new TrajectoryDrive(location, clock, log)); + drivebase.registerDriveRoutine(DriveRoutineType.TRAJECTORY, new TrajectoryDrive(location, clock)); // Driving using the vision targets to help with alignment. Overrides the // steering but not the speed. @@ -234,7 +229,7 @@ public void createDrivebaseLocation(InputDevice leftStick, InputDevice rightStic () -> getVisionDriveSpeed(10 /* maxSpeed */, 40 /* (stopAtDistance */), () -> getVisionTurnWaypointAdjustment(), Constants.VISION_SPEED_SCALE, Constants.VISION_ASSIST_ANGLE_SCALE, Constants.VISION_MAX_VELOCITY_JERK, leftDriveDistance, - leftDriveSpeed, rightDriveDistance, rightDriveSpeed, clock, log)); + leftDriveSpeed, rightDriveDistance, rightDriveSpeed, clock)); // Vision aiming for shooter drivebase.registerDriveRoutine(DriveRoutineType.VISION_AIM, @@ -245,29 +240,29 @@ public void createDrivebaseLocation(InputDevice leftStick, InputDevice rightStic -Constants.VISION_MAX_DRIVE_SPEED, Constants.VISION_MAX_DRIVE_SPEED), () -> getVisionTurnAdjustment(), Constants.VISION_SPEED_SCALE, Constants.VISION_AIM_ANGLE_SCALE, Constants.VISION_MAX_VELOCITY_JERK, leftDriveDistance, leftDriveSpeed, rightDriveDistance, - rightDriveSpeed, clock, log)); + rightDriveSpeed, clock)); // Turns on the spot to a specified angle. drivebase.registerDriveRoutine(DriveRoutineType.TURN_TO_ANGLE, new PositionalPIDDrive("angle", () -> 0, () -> getTurnToAngleTurnAdjustment(), 0, Constants.TURN_TO_ANGLE_ANGLE_SCALE, Constants.TURN_TO_ANGLE_MAX_VELOCITY_JERK, - leftDriveDistance, leftDriveSpeed, rightDriveDistance, rightDriveSpeed, clock, log)); + leftDriveDistance, leftDriveSpeed, rightDriveDistance, rightDriveSpeed, clock)); // Map joysticks in arcade mode for testing/tuning drivebase.registerDriveRoutine(DriveRoutineType.POSITION_PID_ARCADE, new PositionalPIDDrive("posArcade", () -> -leftStick.getAxis(1).read(), () -> rightStick.getAxis(0).read(), 50 /* joystick scale */, 50 /* turn scale */, 50 /* jerk */, - leftDriveDistance, leftDriveSpeed, rightDriveDistance, rightDriveSpeed, clock, log)); + leftDriveDistance, leftDriveSpeed, rightDriveDistance, rightDriveSpeed, clock)); // Log some useful values for debugging. - log.register(true, () -> getVisionTurnWaypointAdjustment(), "Drive/vision/turnAdj") - .register(true, () -> getVisionDriveSpeed(10 /* maxSpeed */, 40 /* (stopAtDistance */), - "Drive/vision/distance") - .register(true, () -> getTurnToAngleTurnAdjustment(), "Drive/angle/turnAdj") - .register(true, () -> getVisionWaypoint().x, "Drive/vision/waypointX") - .register(true, () -> getVisionWaypoint().y, "Drive/vision/waypointY") - .register(true, () -> getVisionTurnAdjustment(), "Drive/vision/visionAim") - .register(true, () -> getVisionDistance(), "Drive/vision/visionAimDistance"); + Chart.register(() -> getVisionTurnWaypointAdjustment(), "Drive/vision/turnAdj"); + Chart.register(() -> getVisionDriveSpeed(10 /* maxSpeed */, 40 /* (stopAtDistance */), + "Drive/vision/distance"); + Chart.register(() -> getTurnToAngleTurnAdjustment(), "Drive/angle/turnAdj"); + Chart.register(() -> getVisionWaypoint().x, "Drive/vision/waypointX"); + Chart.register(() -> getVisionWaypoint().y, "Drive/vision/waypointY"); + Chart.register(() -> getVisionTurnAdjustment(), "Drive/vision/visionAim"); + Chart.register(() -> getVisionDistance(), "Drive/vision/visionAimDistance"); } @@ -287,15 +282,15 @@ public void createDrivebaseLocation(InputDevice leftStick, InputDevice rightStic public double getVisionTurnAdjustment() { if (vision == null || !vision.isConnected()) return 0; - // log.sub("Vision is connected"); + // Logger.debug("Vision is connected"); TargetDetails details = vision.getTargetDetails(); if (!details.isValid(clock.currentTime())) return 0; - // log.sub("Target is valid"); + // Logger.debug("Target is valid"); // We have a recent target position relative to the robot starting position. Position current = location.getCurrentLocation(); - // log.sub("curr pos=%s target = %s", current, details.location); - // log.sub("VISION: bearingToVision = %.1f", + // Logger.debug("curr pos=%s target = %s", current, details.location); + // Logger.debug("VISION: bearingToVision = %.1f", // current.bearingTo(details.location)); // Scale turnadjustment depending on distance from goal @@ -343,9 +338,9 @@ public double getVisionTurnWaypointAdjustment() { // We have a recent target position relative to the robot starting position. Position current = location.getCurrentLocation(); Position waypoint = getVisionWaypoint(); - // log.sub("curr pos=%s waypoint = %s target = %s", current, waypoint, + // Logger.debug("curr pos=%s waypoint = %s target = %s", current, waypoint, // details.location); - // log.sub("bearingToWaypoint = %.1f bearingToVision = %.1f", + // Logger.debug("bearingToWaypoint = %.1f bearingToVision = %.1f", // current.bearingTo(waypoint), current.bearingTo(details.location)); return -current.bearingTo(waypoint); } @@ -368,55 +363,55 @@ public double getVisionDriveSpeed(double maxSpeed, double stopAtDistance) { public double getTurnToAngleTurnAdjustment() { double target = drivebase.getDriveRoutineParameters().value; double actual = location.getBearing(); - // log.sub("angle diff = %f\n", MathUtil.getAngleDiff(actual, target)); + // Logger.debug("angle diff = %f\n", MathUtil.getAngleDiff(actual, target)); return MathUtil.clamp(MathUtil.getAngleDiff(actual, target), -100, 100); } public void createIntake() { - if (!config.intakeIsPresent) { - intake = new MockIntake(log); - log.sub("Intake not present, using a mock intake instead"); + if (!Config.intake.present) { + intake = new MockIntake(); + Log.debug("Intake not present, using a mock intake instead"); return; } - Solenoid intakeSolenoid = Hardware.Solenoids.singleSolenoid(config.pcmCanId, Constants.INTAKE_SOLENOID_PORT, + Solenoid intakeSolenoid = Hardware.Solenoids.singleSolenoid(Config.pcm.canId, Config.intake.solenoidPort, 0.2, 0.2); // TODO: Test and work out correct timings. - Motor intakeMotor = MotorFactory.getIntakeMotor(config, log); - intake = hwIntake = new Intake(intakeMotor, intakeSolenoid, dashboard, log); + Motor intakeMotor = MotorFactory.getIntakeMotor(); + intake = hwIntake = new Intake(intakeMotor, intakeSolenoid, dashboard); } public void createIntakeOverride() { // Setup the diagBox so that it can take control. IntakeSimulator simulator = new IntakeSimulator(); - MockIntake mock = new MockIntake(log); + MockIntake mock = new MockIntake(); intakeOverride = new OverridableSubsystem("intake", IntakeInterface.class, intake, simulator, - mock, log); + mock); // Plumb accessing the intake through the override. intake = intakeOverride.getNormalInterface(); Strongback.executor().register(simulator, Priority.HIGH); } public void createBuddyClimb() { - if (!config.buddyClimbIsPresent) { - buddyClimb = new MockBuddyClimb(log); - log.sub("Buddy climb not present, using a mock buddy climb instead"); + if (!Config.buddyClimb.present) { + buddyClimb = new MockBuddyClimb(); + Log.debug("Buddy climb not present, using a mock buddy climb instead"); return; } - Solenoid buddyClimbSolenoid = Hardware.Solenoids.singleSolenoid(config.pcmCanId, - Constants.BUDDYCLIMB_SOLENOID_PORT, 0.1, 0.1); // TODO: Test and work out correct timings. - buddyClimb = new BuddyClimb(buddyClimbSolenoid, dashboard, log); + Solenoid buddyClimbSolenoid = Hardware.Solenoids.singleSolenoid(Config.pcm.canId, + Config.buddyClimb.solenoidPort, 0.1, 0.1); // TODO: Test and work out correct timings. + buddyClimb = new BuddyClimb(buddyClimbSolenoid, dashboard); } public void createColourWheel() { - if (!config.colourWheelIsPresent) { - colourWheel = new MockColourWheel(log); - log.sub("Colour Sensor not present, using a mock colour sensor instead"); + if (!Config.colourWheel.present) { + colourWheel = new MockColourWheel(); + Log.debug("Colour Sensor not present, using a mock colour sensor instead"); return; } - Motor motor = MotorFactory.getColourWheelMotor(config, log); - Solenoid colourWheelSolenoid = Hardware.Solenoids.singleSolenoid(config.pcmCanId, - Constants.COLOUR_WHEEL_SOLENOID_PORT, 0.1, 0.1); // TODO: Test and work out correct timings. + Motor motor = MotorFactory.getColourWheelMotor(); + Solenoid colourWheelSolenoid = Hardware.Solenoids.singleSolenoid(Config.pcm.canId, + Config.colourWheel.solenoidPort, 0.1, 0.1); // TODO: Test and work out correct timings. ColorSensorV3 colourSensor = new ColorSensorV3(i2cPort); ColorMatch colourMatcher = new ColorMatch(); @@ -442,17 +437,17 @@ public WheelColour get() { } return sensedColour; } - }, ledStrip, clock, dashboard, log); + }, ledStrip, clock, dashboard); Strongback.executor().register(colourWheel, Priority.HIGH); } public void createLEDStrip() { - if (!config.ledStripIsPresent) { + if (!Config.ledStrip.present) { ledStrip = new MockLEDStrip(); - log.sub("LED Strip not present, using a mock LED Strip instead."); + Log.debug("LED Strip not present, using a mock LED Strip instead."); return; } - ledStrip = new LEDStrip(Constants.LED_STRIP_PWM_PORT, Constants.LED_STRIP_NUMBER_OF_LEDS, log); + ledStrip = new LEDStrip(Config.ledStrip.pwmPort, Config.ledStrip.numLEDs); } public void updateIdleLED() { @@ -469,57 +464,57 @@ public void setLEDFinalCountdown(double time) { @SuppressWarnings("resource") public void createLoader() { - if (!config.loaderIsPresent) { - loader = new MockLoader(log); - log.sub("Created a mock loader!"); + if (!Config.loader.present) { + loader = new MockLoader(); + Log.debug("Created a mock loader!"); return; } - Motor spinnerMotor = MotorFactory.getLoaderSpinnerMotor(config, log); - Motor loaderPassthroughMotor = MotorFactory.getLoaderPassthroughMotor(config, log); - Solenoid paddleSolenoid = Hardware.Solenoids.singleSolenoid(config.pcmCanId, Constants.PADDLE_SOLENOID_PORT, + Motor spinnerMotor = MotorFactory.getLoaderSpinnerMotor(); + Motor loaderPassthroughMotor = MotorFactory.getLoaderPassthroughMotor(); + Solenoid paddleSolenoid = Hardware.Solenoids.singleSolenoid(Config.pcm.canId, Config.loader.solenoidPort, 0.1, 0.1); // TODO: Test and work out correct timings. // The ball sensors are connected to the DIO ports on the rio. - DigitalInput inBallSensor = new DigitalInput(Constants.IN_BALL_DETECTOR_DIO_PORT); - DigitalInput outBallSensor = new DigitalInput(Constants.OUT_BALL_DETECTOR_DIO_PORT); + DigitalInput inBallSensor = new DigitalInput(Config.loader.ballInDetectorPort); + DigitalInput outBallSensor = new DigitalInput(Config.loader.ballOutDetectorPort); BooleanSupplier loaderInSensor = () -> !inBallSensor.get(); BooleanSupplier loaderOutSensor = () -> !outBallSensor.get(); loader = hwLoader = new Loader(spinnerMotor, loaderPassthroughMotor, paddleSolenoid, loaderInSensor, - loaderOutSensor, ledStrip, dashboard, log); + loaderOutSensor, ledStrip, dashboard); Strongback.executor().register(loader, Priority.LOW); } public void createLoaderOverride() { // Setup the diagBox so that it can take control. - MockLoader simulator = new MockLoader(log); // Nothing to simulate, use the mock - MockLoader mock = new MockLoader(log); + MockLoader simulator = new MockLoader(); // Nothing to simulate, use the mock + MockLoader mock = new MockLoader(); loaderOverride = new OverridableSubsystem("loader", LoaderInterface.class, loader, simulator, - mock, log); + mock); // Plumb accessing the lift through the override. loader = loaderOverride.getNormalInterface(); } public void createShooter() { - if (!config.shooterIsPresent) { - shooter = new MockShooter(log); - log.sub("Created a mock shooter!"); + if (!Config.shooter.present) { + shooter = new MockShooter(); + Log.debug("Created a mock shooter!"); return; } - Solenoid hoodSolenoid = Hardware.Solenoids.singleSolenoid(config.pcmCanId, Constants.SHOOTER_HOOD_SOLENOID_PORT, + Solenoid hoodSolenoid = Hardware.Solenoids.singleSolenoid(Config.pcm.canId, Config.shooter.solenoidPort, 0.1, 0.1); // TODO: Test and work out correct timings. - Motor shooterMotor = MotorFactory.getShooterMotor(config, clock, log); + Motor motor = MotorFactory.getShooterMotor(clock); - shooter = hwShooter = new Shooter(shooterMotor, hoodSolenoid, dashboard, log); + shooter = hwShooter = new Shooter(motor, hoodSolenoid, dashboard); } public void createShooterOverride() { // Setup the diagBox so that it can take control. - MockShooter simulator = new MockShooter(log); // Nothing to simulate, use a mock instead. - MockShooter mock = new MockShooter(log); + MockShooter simulator = new MockShooter(); // Nothing to simulate, use a mock instead. + MockShooter mock = new MockShooter(); shooterOverride = new OverridableSubsystem("shooter", ShooterInterface.class, shooter, - simulator, mock, log); + simulator, mock); // Plumb accessing the shooter through the override. shooter = shooterOverride.getNormalInterface(); } @@ -528,26 +523,26 @@ public void createShooterOverride() { * Create the Pneumatics Control Module (PCM) subsystem. */ public void createPneumatics() { - if (!config.pcmIsPresent) { - compressor = Mock.pneumaticsModule(config.pcmCanId); - log.sub("Created a mock compressor"); + if (!Config.pcm.present) { + compressor = Mock.pneumaticsModule(Config.pcm.canId); + Log.debug("Created a mock compressor"); return; } - compressor = Hardware.pneumaticsModule(config.pcmCanId); + compressor = Hardware.pneumaticsModule(Config.pcm.canId); } public void createVision() { - if (!config.visionIsPresent) { + if (!Config.vision.present) { vision = new MockVision(); - log.sub("Created a mock vision subsystem"); + Log.debug("Created a mock vision subsystem"); return; } try { - jevois = new Jevois(log); - vision = new Vision(jevois, location, dashboard, clock, config.visionHMin, config.visionSMin, - config.visionVMin, config.visionHMax, config.visionSMax, config.visionVMax, log); + jevois = new Jevois(); + vision = new Vision(jevois, location, dashboard, clock, Config.vision.hMin, Config.vision.sMin, + Config.vision.vMin, Config.vision.hMax, Config.vision.sMax, Config.vision.vMax); } catch (IOException e) { - log.exception("Unable to create an instance of the jevois camera", e); + Log.exception("Unable to create an instance of the jevois camera", e); e.printStackTrace(); vision = new MockVision(); } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 589d8d8..f759e94 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -8,10 +8,11 @@ import frc.robot.interfaces.DashboardInterface; import frc.robot.interfaces.JevoisInterface; import frc.robot.interfaces.LocationInterface; -import frc.robot.interfaces.Log; import frc.robot.interfaces.VisionInterface; import frc.robot.lib.Position; import frc.robot.lib.Subsystem; +import frc.robot.lib.chart.Chart; +import frc.robot.lib.log.Log; public class Vision extends Subsystem implements VisionInterface, Runnable { private JevoisInterface jevois; @@ -24,8 +25,8 @@ public class Vision extends Subsystem implements VisionInterface, Runnable { public Vision(JevoisInterface jevois, LocationInterface location, DashboardInterface dashboard, Clock clock, double visionHMin, double visionSMin, double visionVMin, double visionHMax, double visionSMax, - double visionVMax, Log log) { - super("Vision", dashboard, log); + double visionVMax) { + super("Vision", dashboard); this.jevois = jevois; this.location = location; this.clock = clock; @@ -36,16 +37,15 @@ public Vision(JevoisInterface jevois, LocationInterface location, DashboardInter this.visionSMax = visionSMax; this.visionVMax = visionVMax; - log.register(true, () -> isConnected(), "%s/connected", name) - .register(true, () -> lastSeenTarget.location.x, "%s/curX", name) - .register(true, () -> lastSeenTarget.location.y, "%s/curY", name) - .register(true, () -> lastSeenTarget.location.heading, "%s/heading", name) - // .register(true, () -> clock.currentTime() - lastSeenTarget.seenAtSec, - // "%s/seenAt", name) - .register(true, () -> lastSeenTarget.imageTimestamp, "%s/seenAtSec", name) - .register(true, () -> lastSeenTarget.targetFound, "%s/targetFound", name) - .register(true, () -> lastSeenTarget.distance, "%s/distance", name) - .register(true, () -> lastSeenTarget.angle, "%s/angle", name); + Chart.register(() -> isConnected(), "%s/connected", name); + Chart.register(() -> lastSeenTarget.location.x, "%s/curX", name); + Chart.register(() -> lastSeenTarget.location.y, "%s/curY", name); + Chart.register(() -> lastSeenTarget.location.heading, "%s/heading", name); + // Chart.register(() -> clock.currentTime() - lastSeenTarget.seenAtSec, "%s/seenAt", name) + Chart.register(() -> lastSeenTarget.imageTimestamp, "%s/seenAtSec", name); + Chart.register(() -> lastSeenTarget.targetFound, "%s/targetFound", name); + Chart.register(() -> lastSeenTarget.distance, "%s/distance", name); + Chart.register(() -> lastSeenTarget.angle, "%s/angle", name); // Start reading from the Jevois camera. (new Thread(this)).start(); } @@ -65,16 +65,16 @@ public synchronized TargetDetails getTargetDetails() { @Override public void run() { while (true) { - log.sub("Vision waiting for the camera server to start up"); + Log.debug("Vision waiting for the camera server to start up"); try { Thread.sleep(5000); } catch (InterruptedException e1) { } - log.sub("Starting to read from Jevois camera\n"); + Log.debug("Starting to read from Jevois camera\n"); try { // Attempt to detect if there is a camera plugged in. It will throw an exception // if not. - log.sub(jevois.issueCommand("info")); + Log.debug(jevois.issueCommand("info")); connected = true; // Update the HSV filter ranges from the config values. @@ -88,7 +88,7 @@ public void run() { processLine(jevois.readLine()); } } catch (IOException e) { - log.error("Failed to read from jevois, aborting vision processing\n"); + Log.error("Failed to read from jevois, aborting vision processing\n"); connected = false; e.printStackTrace(); } @@ -120,10 +120,10 @@ private void processLine(String line) { String[] parts = line.split("\\s+"); if (!parts[0].equals("D3")) { - log.info("Ignoring non-vision target line: %s", line); + Log.info("Ignoring non-vision target line: %s", line); return; } - //log.sub("Vision::processLine(%s)\n", line); + //Logger.debug("Vision::processLine(%s)\n", line); TargetDetails newTarget = new TargetDetails(); newTarget.targetFound = Boolean.parseBoolean(parts[2]); @@ -142,7 +142,7 @@ private void processLine(String line) { newTarget.location.heading += newTarget.angle - newTarget.skew; - // log.sub("Location set."); + // Logger.debug("Location set."); // newTarget.location.heading += newTarget.angle @@ -158,7 +158,7 @@ private void processLine(String line) { synchronized (this) { lastSeenTarget = newTarget; } - // log.sub("Vision: Updated target %s", lastSeenTarget); + // Logger.debug("Vision: Updated target %s", lastSeenTarget); } diff --git a/src/test/java/frc/robot/TestSuite.java b/src/test/java/frc/robot/TestSuite.java index 8cb5db4..7c42d7b 100644 --- a/src/test/java/frc/robot/TestSuite.java +++ b/src/test/java/frc/robot/TestSuite.java @@ -4,7 +4,7 @@ import org.junit.runners.Suite; import org.strongback.command.TestCommandGroup; import frc.robot.controller.TestController; -import frc.robot.lib.RobotConfigurationTest; +import frc.robot.lib.TestConfigReader; import frc.robot.lib.TestLogFileWriter; import frc.robot.lib.TestMovementSimulator; import frc.robot.lib.TestRedundantTalonSRX; @@ -18,7 +18,7 @@ @Suite.SuiteClasses({ TestCommandGroup.class, TestController.class, - RobotConfigurationTest.class, + TestConfigReader.class, TestLogFileWriter.class, TestMovementSimulator.class, TestRedundantTalonSRX.class, diff --git a/src/test/java/frc/robot/controller/TestController.java b/src/test/java/frc/robot/controller/TestController.java index fba6dc2..909c23f 100644 --- a/src/test/java/frc/robot/controller/TestController.java +++ b/src/test/java/frc/robot/controller/TestController.java @@ -12,7 +12,6 @@ import frc.robot.Constants; import frc.robot.interfaces.DashboardInterface; -import frc.robot.interfaces.Log; import frc.robot.lib.WheelColour; import frc.robot.mock.MockBuddyClimb; import frc.robot.mock.MockColourWheel; @@ -20,7 +19,6 @@ import frc.robot.mock.MockDrivebase; import frc.robot.mock.MockLEDStrip; import frc.robot.mock.MockLocation; -import frc.robot.mock.MockLog; import frc.robot.mock.MockLoader; import frc.robot.mock.MockShooter; import frc.robot.simulator.IntakeSimulator; @@ -37,7 +35,6 @@ public class TestController { private final long kRandomSeed = 123456; private final double kMaxWaitTimeSeconds = 4; protected DashboardInterface dashboard = new MockDashboard(); - protected Log log = new MockLog(true); private MockClock clock; private Subsystems subsystems; // Store direct access to the simulators so the simulator-only @@ -55,18 +52,18 @@ public class TestController { public void setUp() { System.out.println("\n******************************"); clock = new MockClock(); - subsystems = new Subsystems(new MockDashboard(), null, clock, log); + subsystems = new Subsystems(new MockDashboard(), clock); subsystems.intake = intake = new IntakeSimulator(); subsystems.compressor = new MockPneumaticsModule(); - subsystems.drivebase = new MockDrivebase(log); - subsystems.loader = new MockLoader(log); - subsystems.shooter = new MockShooter(log); + subsystems.drivebase = new MockDrivebase(); + subsystems.loader = new MockLoader(); + subsystems.shooter = new MockShooter(); subsystems.location = new MockLocation(); - subsystems.colourWheel = new MockColourWheel(log); + subsystems.colourWheel = new MockColourWheel(); subsystems.leftDriveDistance = () -> 0; subsystems.rightDriveDistance = () -> 0; - subsystems.buddyClimb = new MockBuddyClimb(log); + subsystems.buddyClimb = new MockBuddyClimb(); subsystems.ledStrip = new MockLEDStrip(); exec = new Controller(subsystems, getFMSColour()); diff --git a/src/test/java/frc/robot/drive/util/TestPositionCalc.java b/src/test/java/frc/robot/drive/util/TestPositionCalc.java index 2b359e6..605287a 100644 --- a/src/test/java/frc/robot/drive/util/TestPositionCalc.java +++ b/src/test/java/frc/robot/drive/util/TestPositionCalc.java @@ -6,8 +6,6 @@ import org.junit.Test; import org.strongback.mock.MockClock; -import frc.robot.mock.MockLog; - public class TestPositionCalc { /** @@ -20,9 +18,8 @@ public void testBasic() { double speed = 1; double maxJerk = 1; MockClock clock = new MockClock(); - MockLog log = new MockLog(); - PositionCalc calc = new PositionCalc(position, speed, maxJerk, clock, log); + PositionCalc calc = new PositionCalc(position, speed, maxJerk, clock); long dtMSec = 1000; // A whole second. clock.incrementByMilliseconds(dtMSec); double newPos = calc.update(); diff --git a/src/test/java/frc/robot/lib/RobotConfigurationTest.java b/src/test/java/frc/robot/lib/RobotConfigurationTest.java deleted file mode 100644 index 67596dc..0000000 --- a/src/test/java/frc/robot/lib/RobotConfigurationTest.java +++ /dev/null @@ -1,78 +0,0 @@ -package frc.robot.lib; - -import static org.hamcrest.MatcherAssert.assertThat; -import static org.hamcrest.Matchers.equalTo; -import static org.hamcrest.Matchers.greaterThan; -import static org.hamcrest.Matchers.is; - -import java.io.File; -import java.io.IOException; -import java.nio.charset.StandardCharsets; -import java.nio.file.Files; -import java.nio.file.Path; -import java.nio.file.Paths; - -import org.junit.Test; -import frc.robot.Constants; -import frc.robot.mock.MockLog; - -public class RobotConfigurationTest { - MockLog log = new MockLog(true); - - // Ensure all subsystems are enabled to their default values if the file is empty. - @Test - public void testMissingFile() throws IOException { - Path tempDir = Files.createTempDirectory("robot"); - Path path = Paths.get(tempDir.toString(), "robot.config"); - RobotConfiguration config = new RobotConfiguration(path.toString(), 3132, log); - - assertThat(config.drivebaseIsPresent, is(equalTo(Constants.DRIVE_PRESENT_DEFAULT))); - //assertThat(config.liftIsPresent, is(equalTo(false))); - assertThat(config.drivebaseCanIdsLeftWithEncoders, is(equalTo(Constants.DRIVE_LEFT_TALON_WITH_ENCODERS_CAN_ID_LIST))); - } - - // Ensure all subsystems are enabled to their default values if the file is empty. - @Test - public void testEmptyFile() throws IOException { - File file = File.createTempFile("robot", ".config"); - String exampleFile = file.toString() + ".example"; - RobotConfiguration config = new RobotConfiguration(file.toString(), 3132, log); - assertThat(file.exists(), is(equalTo(true))); - - assertThat(config.drivebaseIsPresent, is(equalTo(Constants.DRIVE_PRESENT_DEFAULT))); - assertThat(config.drivebaseCanIdsLeftWithEncoders, is(equalTo(Constants.DRIVE_LEFT_TALON_WITH_ENCODERS_CAN_ID_LIST))); - assertThat(exampleFile.toString().length(), greaterThan(20)); // make sure the example file isn't empty after first run of update - - // Load in the example file to ensure it's valid. Cook one of the lines - // to ensure that it's actually read and not the default. - Path path = Paths.get(exampleFile); - String content = new String(Files.readAllBytes(path), StandardCharsets.UTF_8); - Files.write(path, content.getBytes(StandardCharsets.UTF_8)); - config = new RobotConfiguration(exampleFile, 3132, log); - assertThat(config.drivebaseIsPresent, is(equalTo(Constants.DRIVE_PRESENT_DEFAULT))); - assertThat(config.drivebaseCanIdsLeftWithEncoders, is(equalTo(Constants.DRIVE_LEFT_TALON_WITH_ENCODERS_CAN_ID_LIST))); - } - - @Test - public void testGetValueType() throws IOException { - File file = File.createTempFile("robot", ".config"); - assertThat(file.exists(), is(equalTo(true))); - - StringBuilder str = new StringBuilder(); - str.append("robot/name=\"firstValue\"\n"); // First value of a dup is ignored. - str.append("robot/name=\"unitTest\"\n"); - str.append(" drivebase/present=false\n"); // Leading spaces. - str.append("pcm/canID=210\n"); - str.append("drivebase/left/canIDs/withEncoders=7, 9,15\n"); - str.append("drivebase/left/canIDs/withoutEncoders=\n"); - Files.write(file.toPath(), str.toString().getBytes(StandardCharsets.UTF_8)); - - RobotConfiguration config = new RobotConfiguration(file.toString(), 31332, log); - - //assertThat(config.robotName, is(equalTo("unitTest"))); - assertThat(config.drivebaseIsPresent, is(equalTo(false))); - assertThat(config.pcmCanId, is(equalTo(210))); - assertThat(config.drivebaseCanIdsLeftWithEncoders, is(equalTo(new int[] { 7, 9, 15 }))); - assertThat(config.drivebaseCanIdsLeftWithoutEncoders, is(equalTo(new int[] {}))); - } -} diff --git a/src/test/java/frc/robot/lib/TestConfigReader.java b/src/test/java/frc/robot/lib/TestConfigReader.java new file mode 100644 index 0000000..218bb81 --- /dev/null +++ b/src/test/java/frc/robot/lib/TestConfigReader.java @@ -0,0 +1,69 @@ +package frc.robot.lib; + +import static org.hamcrest.MatcherAssert.assertThat; +import static org.hamcrest.Matchers.equalTo; +import static org.hamcrest.Matchers.greaterThan; +import static org.hamcrest.Matchers.is; + +import java.io.File; +import java.io.IOException; +import java.nio.charset.StandardCharsets; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; + +import org.junit.Test; + +public class TestConfigReader { + + // Ensure all subsystems are enabled to their default values if the file is empty. + @Test + public void testMissingFile() throws IOException { + Path tempDir = Files.createTempDirectory("robot"); + Path path = Paths.get(tempDir.toString(), "robot.config"); + ConfigReader config = new ConfigReader(path.toString()); + + assertThat(config.getString("missing/string", "default_value"), is(equalTo("default_value"))); + assertThat(config.getBoolean("missing/boolean", false), is(equalTo(false))); + assertThat(config.getInt("missing/int", 2), is(equalTo(2))); + assertThat(config.getIntArray("missing/int/array", new int[]{2, 3}), is(equalTo(new int[]{2, 3}))); + } + + // Ensure all subsystems are enabled to their default values if the file is empty. + @Test + public void testEmptyFile() throws IOException { + File file = File.createTempFile("robot", ".config"); + String exampleFile = file.toString() + ".example"; + ConfigReader config = new ConfigReader(file.toString()); + assertThat(file.exists(), is(equalTo(true))); + + assertThat(config.getString("missing/string", "default_value"), is(equalTo("default_value"))); + assertThat(config.getBoolean("missing/boolean", false), is(equalTo(false))); + assertThat(config.getInt("missing/int", 2), is(equalTo(2))); + assertThat(config.getIntArray("missing/int/array", new int[]{2, 3}), is(equalTo(new int[]{2, 3}))); + assertThat(exampleFile.toString().length(), greaterThan(20)); // make sure the example file isn't empty after first run of update + } + + @Test + public void testGetValueType() throws IOException { + File file = File.createTempFile("robot", ".config"); + assertThat(file.exists(), is(equalTo(true))); + + StringBuilder str = new StringBuilder(); + str.append("robot/name=\"firstValue\"\n"); // First value of a dup is ignored. + str.append("robot/name=\"unitTest\"\n"); + str.append(" drivebase/present=false\n"); // Leading spaces. + str.append("pcm/canID=210\n"); + str.append("drivebase/left/canIDs/withEncoders=7, 9,15\n"); + str.append("drivebase/left/canIDs/withoutEncoders=\n"); + Files.write(file.toPath(), str.toString().getBytes(StandardCharsets.UTF_8)); + + ConfigReader config = new ConfigReader(file.toString()); + + assertThat(config.getString("robot/name", "not found"), is(equalTo("unitTest"))); + assertThat(config.getBoolean("drivebase/present", true), is(equalTo(false))); + assertThat(config.getInt("pcm/canID", -1), is(equalTo(210))); + assertThat(config.getIntArray("drivebase/left/canIDs/withEncoders", new int[]{}), is(equalTo(new int[] {7, 9, 15}))); + assertThat(config.getIntArray("drivebase/left/canIDs/withoutEncoders", new int[]{-1}), is(equalTo(new int[] {}))); + } +} diff --git a/src/test/java/frc/robot/lib/log/TestTimestampedLogWriter.java b/src/test/java/frc/robot/lib/log/TestTimestampedLogWriter.java new file mode 100644 index 0000000..f7f4b49 --- /dev/null +++ b/src/test/java/frc/robot/lib/log/TestTimestampedLogWriter.java @@ -0,0 +1,43 @@ +package frc.robot.lib.log; + +import static org.hamcrest.MatcherAssert.assertThat; +import static org.hamcrest.Matchers.equalTo; +import static org.hamcrest.Matchers.is; + +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; + +import org.junit.Test; + +public class TestTimestampedLogWriter { + + // Ensure writing works. + @Test + public void testWrite() throws IOException { + Path tempDir = Files.createTempDirectory("timestamped"); + TimestampedLogWriter writer = new TimestampedLogWriter(tempDir.toString(), "test", 123, "txt"); + writer.write("Hello\n"); + writer.write("World!"); + writer.flush(); + Path expctedFile = Paths.get(tempDir.toString(), "data", "test_00123.txt"); + System.out.printf("Found: '" + Files.readAllLines(expctedFile)); + assertThat(Files.readString(expctedFile), is(equalTo("Hello\nWorld!"))); + } + + // Ensure symbolic links are created. + @Test + public void testSymbolicLink() throws IOException { + Path tempDir = Files.createTempDirectory("timestamped"); + TimestampedLogWriter writer = new TimestampedLogWriter(tempDir.toString(), "test", 123, "txt"); + writer.write("Hello\n"); + writer.write("World!"); + writer.flush(); + writer.createSymbolicLink("event", "SouthernCross_M1"); + Path expctedFile = Paths.get(tempDir.toString(), "event", "SouthernCross_M1_test.txt"); + System.out.printf("Found: '" + Files.readAllLines(expctedFile)); + assertThat(Files.readString(expctedFile), is(equalTo("Hello\nWorld!"))); + } + +} diff --git a/src/test/java/frc/robot/subsystems/TestColourwheel.java b/src/test/java/frc/robot/subsystems/TestColourwheel.java index c9686d3..42b6524 100644 --- a/src/test/java/frc/robot/subsystems/TestColourwheel.java +++ b/src/test/java/frc/robot/subsystems/TestColourwheel.java @@ -18,7 +18,6 @@ import frc.robot.interfaces.ColourWheelInterface.ColourAction.ColourWheelType; import frc.robot.lib.WheelColour; import frc.robot.mock.MockDashboard; -import frc.robot.mock.MockLog; public class TestColourwheel { @@ -36,7 +35,7 @@ public void setup() { solenoid = Mock.Solenoids.singleSolenoid(0); clock = Mock.clock(); ledStrip = Mock.ledStrip(); - colourWheel = new ColourWheel(motor, solenoid, () -> colour, ledStrip, clock, new MockDashboard(), new MockLog()); + colourWheel = new ColourWheel(motor, solenoid, () -> colour, ledStrip, clock, new MockDashboard()); } diff --git a/src/test/java/frc/robot/subsystems/TestDrivebase.java b/src/test/java/frc/robot/subsystems/TestDrivebase.java index d727051..17ade50 100644 --- a/src/test/java/frc/robot/subsystems/TestDrivebase.java +++ b/src/test/java/frc/robot/subsystems/TestDrivebase.java @@ -14,7 +14,6 @@ import frc.robot.interfaces.DrivebaseInterface.DriveRoutineParameters; import frc.robot.interfaces.DrivebaseInterface.DriveRoutineType; import frc.robot.mock.MockDashboard; -import frc.robot.mock.MockLog; public class TestDrivebase { @@ -25,7 +24,7 @@ class MockDriveRoutine extends DriveRoutine { public double rightPower = 0; public MockDriveRoutine(String name) { - super(name, ControlMode.DutyCycle, new MockLog()); + super(name, ControlMode.DutyCycle); this.name = name; } @@ -62,7 +61,7 @@ public void testDriveRoutine() { MockSolenoid brakeSolenoid = Mock.Solenoids.singleSolenoid(1); MockDriveRoutine arcade = new MockDriveRoutine("MockArcade"); DrivebaseInterface drive = new Drivebase(leftMotor, rightMotor, ptoSolenoid, brakeSolenoid, - new MockDashboard(), new MockLog(true)); + new MockDashboard()); // Register this drive routine so it can be used. drive.registerDriveRoutine(DriveRoutineType.ARCADE_DUTY_CYCLE, arcade); // Tell the drive subsystem to use it. diff --git a/src/test/java/frc/robot/subsystems/TestLoader.java b/src/test/java/frc/robot/subsystems/TestLoader.java index 9b71dfa..de6a11d 100644 --- a/src/test/java/frc/robot/subsystems/TestLoader.java +++ b/src/test/java/frc/robot/subsystems/TestLoader.java @@ -14,7 +14,6 @@ import frc.robot.interfaces.LEDStripInterface; import frc.robot.mock.MockDashboard; -import frc.robot.mock.MockLog; public class TestLoader { MockMotor spinner, passthrough; @@ -25,7 +24,6 @@ public class TestLoader { int outSensorCounts = 0; LEDStripInterface led; MockDashboard dashboard; - MockLog log; MockClock clock; @Before @@ -34,12 +32,11 @@ public void setup() { passthrough = Mock.stoppedMotor(); led = Mock.ledStrip(); clock = Mock.clock(); - log = new MockLog(); inSensor = () -> { if (inSensorCounts.size() == 0) return false; return inSensorCounts.get(0); }; - loader = new Loader(spinner, passthrough, paddle, inSensor, outSensor, led, dashboard, log); + loader = new Loader(spinner, passthrough, paddle, inSensor, outSensor, led, dashboard); } @Test diff --git a/src/test/java/frc/robot/subsystems/TestLocation.java b/src/test/java/frc/robot/subsystems/TestLocation.java index c4863e7..1bee9f0 100644 --- a/src/test/java/frc/robot/subsystems/TestLocation.java +++ b/src/test/java/frc/robot/subsystems/TestLocation.java @@ -9,7 +9,6 @@ import org.strongback.mock.MockGyroscope; import frc.robot.lib.MathUtil; import frc.robot.lib.Position; -import frc.robot.mock.MockLog; /** * Test the Location class. @@ -36,7 +35,7 @@ public void testLocation() { MockDoubleSupplier rightDistance = Mock.doubleSupplier(); MockGyroscope gyro = Mock.gyroscope(); MockClock clock = Mock.clock(); - Location location = new Location(()->{},leftDistance, rightDistance, gyro, clock, null, new MockLog()); + Location location = new Location(()->{},leftDistance, rightDistance, gyro, clock, null); // Initial location should always be 0,0,0 gyro.setAngle(0); // Facing towards the opposing alliances wall. diff --git a/src/test/java/frc/robot/subsystems/TestOverridableSubsystem.java b/src/test/java/frc/robot/subsystems/TestOverridableSubsystem.java index ff17d52..5e6b0f7 100644 --- a/src/test/java/frc/robot/subsystems/TestOverridableSubsystem.java +++ b/src/test/java/frc/robot/subsystems/TestOverridableSubsystem.java @@ -5,23 +5,20 @@ import org.junit.Test; import frc.robot.interfaces.IntakeInterface; -import frc.robot.interfaces.Log; import frc.robot.mock.MockIntake; -import frc.robot.mock.MockLog; public class TestOverridableSubsystem { - private Log log = new MockLog(); @Test public void testNormalMode() { // This is when the controller should talk through to the real // subsystem and the button box is passed through to the mock. // We only want to use mock here so that they can be checked. - IntakeInterface real = new MockIntake(log); - IntakeInterface simulator = new MockIntake(log); - IntakeInterface mock = new MockIntake(log); + IntakeInterface real = new MockIntake(); + IntakeInterface simulator = new MockIntake(); + IntakeInterface mock = new MockIntake(); // Create the sim and pass it the three different endpoints. - OverridableSubsystem intakeOverride = new OverridableSubsystem("intake", IntakeInterface.class, real, simulator, mock, log); + OverridableSubsystem intakeOverride = new OverridableSubsystem("intake", IntakeInterface.class, real, simulator, mock); // Get the endpoint that the controller would use. IntakeInterface normalIntake = intakeOverride.getNormalInterface(); // Get the endpoint that the diag box uses. diff --git a/src/test/java/frc/robot/subsystems/TestShooter.java b/src/test/java/frc/robot/subsystems/TestShooter.java index a9bd9e5..3223bf8 100644 --- a/src/test/java/frc/robot/subsystems/TestShooter.java +++ b/src/test/java/frc/robot/subsystems/TestShooter.java @@ -11,7 +11,6 @@ import org.strongback.mock.MockSolenoid; import frc.robot.mock.MockDashboard; -import frc.robot.mock.MockLog; public class TestShooter { @@ -25,7 +24,7 @@ public class TestShooter { public void setUp() { shooterMotor = Mock.stoppedMotor(); shooterSolenoid = Mock.Solenoids.singleSolenoid(0); - shooter = new Shooter(shooterMotor, shooterSolenoid, new MockDashboard(), new MockLog()); + shooter = new Shooter(shooterMotor, shooterSolenoid, new MockDashboard()); } @Test diff --git a/src/test/java/frc/robot/subsystems/TestVision.java b/src/test/java/frc/robot/subsystems/TestVision.java index d46d817..731892e 100644 --- a/src/test/java/frc/robot/subsystems/TestVision.java +++ b/src/test/java/frc/robot/subsystems/TestVision.java @@ -14,11 +14,9 @@ import org.strongback.mock.MockClock; import frc.robot.interfaces.DashboardInterface; import frc.robot.interfaces.LocationInterface; -import frc.robot.interfaces.Log; import frc.robot.mock.MockDashboard; import frc.robot.mock.MockJevois; import frc.robot.mock.MockLocation; -import frc.robot.mock.MockLog; public class TestVision { @@ -30,7 +28,6 @@ public class TestVision { public void testVision() throws UnknownHostException, IOException { System.out.println("testVision()"); MockClock clock = Mock.clock(); - Log log = new MockLog(true); MockJevois jevois = new MockJevois(); // Historic locations are based on the time it is requested at. @@ -38,7 +35,7 @@ public void testVision() throws UnknownHostException, IOException { DashboardInterface dashboard = new MockDashboard(); // Listen on a port assigned by the operating system. - Vision vision = new Vision(jevois, location, dashboard, clock, 0, 0, 0, 255, 255, 255, log); + Vision vision = new Vision(jevois, location, dashboard, clock, 0, 0, 0, 255, 255, 255); // Shouldn't have a target lock. assertThat(vision.getTargetDetails().targetFound, is(equalTo(false)));