diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/Robot.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/Robot.java index f0e682fb..048261a1 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/Robot.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/Robot.java @@ -107,6 +107,10 @@ public void robotInit() { System.out.println("Config file is bad/nonexistent!"); e.printStackTrace(); } + + //Read sensors + this.robotMap.getUpdater().run(); + //Set fields from the map. this.loggerNotifier = new Notifier(robotMap.getLogger()); this.driveSubsystem = robotMap.getDrive(); @@ -179,6 +183,12 @@ public void robotInit() { */ @Override public void teleopInit() { + //Do the startup tasks + doStartupTasks(); + + //Read sensors + this.robotMap.getUpdater().run(); + //Run startup command if we start in teleop. if (!enabled) { if (robotMap.getStartupCommand() != null) { @@ -187,9 +197,8 @@ public void teleopInit() { enabled = true; } - //Do the startup tasks driveSubsystem.stopMPProcesses(); - doStartupTasks(); + if (robotMap.getTeleopStartupCommand() != null) { robotMap.getTeleopStartupCommand().start(); } @@ -208,6 +217,10 @@ public void teleopInit() { public void teleopPeriodic() { //Refresh the current time. Clock.updateTime(); + + //Read sensors + this.robotMap.getUpdater().run(); + //Run all commands. This is a WPILib thing you don't really have to worry about. Scheduler.getInstance().run(); } @@ -217,6 +230,12 @@ public void teleopPeriodic() { */ @Override public void autonomousInit() { + //Do startup tasks + doStartupTasks(); + + //Read sensors + this.robotMap.getUpdater().run(); + //Run startup command if we start in auto. if (!enabled) { if (robotMap.getStartupCommand() != null) { @@ -225,8 +244,6 @@ public void autonomousInit() { enabled = true; } - //Do startup tasks - doStartupTasks(); if (robotMap.getAutoStartupCommand() != null) { robotMap.getAutoStartupCommand().start(); } @@ -247,6 +264,8 @@ public void autonomousInit() { public void autonomousPeriodic() { //Update the current time Clock.updateTime(); + //Read sensors + this.robotMap.getUpdater().run(); //Run all commands. This is a WPILib thing you don't really have to worry about. Scheduler.getInstance().run(); } @@ -282,6 +301,8 @@ public void testInit() { @Override public void disabledPeriodic() { Clock.updateTime(); + //Read sensors + this.robotMap.getUpdater().run(); } /** diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/RobotMap2017.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/RobotMap2017.java index 207cceea..57e42bf4 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/RobotMap2017.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/RobotMap2017.java @@ -7,6 +7,7 @@ import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.drive.unidirectional.DriveTalonCluster; import org.usfirst.frc.team449.robot.jacksonWrappers.MappedDigitalInput; +import org.usfirst.frc.team449.robot.jacksonWrappers.MappedRunnable; import org.usfirst.frc.team449.robot.jacksonWrappers.YamlCommand; import org.usfirst.frc.team449.robot.oi.buttons.CommandButton; import org.usfirst.frc.team449.robot.oi.unidirectional.OIUnidirectional; @@ -58,6 +59,12 @@ public class RobotMap2017 { @NotNull private final Command defaultDriveCommand; + /** + * A runnable that updates cached variables. + */ + @NotNull + private final Runnable updater; + /** * The climber for boarding the airship. Can be null. */ @@ -215,6 +222,7 @@ public class RobotMap2017 { * @param logger The logger for recording events and telemetry data. * @param drive The drive. * @param defaultDriveCommand The command for the drive to run during the teleoperated period. + * @param updater A runnable that updates cached variables. * @param climber The climber for boarding the airship. Can be null. * @param shooter The multiSubsystem for shooting fuel. Can be null. * @param camera The cameras on this robot. Can be null. @@ -258,6 +266,7 @@ public RobotMap2017(@Nullable List buttons, @NotNull @JsonProperty(required = true) Logger logger, @NotNull @JsonProperty(required = true) DriveTalonCluster drive, @NotNull @JsonProperty(required = true) YamlCommand defaultDriveCommand, + @NotNull @JsonProperty(required = true) MappedRunnable updater, @Nullable ClimberCurrentLimited climber, @Nullable LoggingShooter shooter, @Nullable CameraNetwork camera, @@ -289,6 +298,7 @@ public RobotMap2017(@Nullable List buttons, this.pneumatics = pneumatics; this.gearHandler = gearHandler; this.logger = logger; + this.updater = updater; this.RIOduinoPort = RIOduinoPort; this.allianceSwitch = allianceSwitch; this.dropGearSwitch = dropGearSwitch; @@ -542,4 +552,12 @@ public boolean getDoMP() { public Command getStartupCommand() { return startupCommand; } + + /** + * @return A runnable that updates cached variables. + */ + @NotNull + public Runnable getUpdater() { + return updater; + } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/generalInterfaces/updatable/Updatable.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/generalInterfaces/updatable/Updatable.java index f219b6f3..60047045 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/generalInterfaces/updatable/Updatable.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/generalInterfaces/updatable/Updatable.java @@ -1,8 +1,11 @@ package org.usfirst.frc.team449.robot.generalInterfaces.updatable; +import com.fasterxml.jackson.annotation.JsonTypeInfo; + /** * An interface for any object that caches values. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") public interface Updatable { /** diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/Updater.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/Updater.java new file mode 100644 index 00000000..0d2c81b3 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/Updater.java @@ -0,0 +1,42 @@ +package org.usfirst.frc.team449.robot.other; + +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIdentityInfo; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.annotation.ObjectIdGenerators; +import org.jetbrains.annotations.NotNull; +import org.usfirst.frc.team449.robot.generalInterfaces.updatable.Updatable; +import org.usfirst.frc.team449.robot.jacksonWrappers.MappedRunnable; + +/** + * A Runnable for updating cached variables. + */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class Updater implements MappedRunnable{ + + /** + * The objects to update. + */ + @NotNull + private final Updatable[] updatables; + + /** + * Default constructor + * + * @param updatables The objects to update. + */ + @JsonCreator + public Updater(@NotNull @JsonProperty(required = true) Updatable[] updatables) { + this.updatables = updatables; + } + + /** + * Update all the updatables. + */ + @Override + public void run() { + for (Updatable updatable : updatables){ + updatable.update(); + } + } +} diff --git a/RoboRIO/src/main/resources/ballbasaur_map.yml b/RoboRIO/src/main/resources/ballbasaur_map.yml index 0ae0953c..017c3fef 100644 --- a/RoboRIO/src/main/resources/ballbasaur_map.yml +++ b/RoboRIO/src/main/resources/ballbasaur_map.yml @@ -179,4 +179,12 @@ logger: eliPoseEstimator loopTimeSecs: 0.02 eventLogFilename: "/home/lvuser/logs/eventLog-" - telemetryLogFilename: "/home/lvuser/logs/telemetryLog-" \ No newline at end of file + telemetryLogFilename: "/home/lvuser/logs/telemetryLog-" +updater: + org.usfirst.frc.team449.robot.other.Updater: + '@id': updater + updatables: + - org.usfirst.frc.team449.robot.drive.unidirectional.DriveTalonClusterShiftable: + drive + - org.usfirst.frc.team449.robot.oi.unidirectional.arcade.OIArcadeWithDPad: + oi \ No newline at end of file diff --git a/RoboRIO/src/main/resources/calcifer_outreach_map.yml b/RoboRIO/src/main/resources/calcifer_outreach_map.yml index 4ad68581..4734e4da 100644 --- a/RoboRIO/src/main/resources/calcifer_outreach_map.yml +++ b/RoboRIO/src/main/resources/calcifer_outreach_map.yml @@ -354,4 +354,22 @@ buttons: subsystem: org.usfirst.frc.team449.robot.subsystem.interfaces.solenoid.SolenoidSimple: gearHandler - action: WHEN_RELEASED \ No newline at end of file + action: WHEN_RELEASED +updater: + org.usfirst.frc.team449.robot.other.Updater: + '@id': updater + updatables: + - org.usfirst.frc.team449.robot.drive.unidirectional.DriveTalonClusterShiftable: + drive + - org.usfirst.frc.team449.robot.subsystem.complex.climber.ClimberCurrentLimited: + climber + - org.usfirst.frc.team449.robot.oi.unidirectional.OIOutreach: + oi + - org.usfirst.frc.team449.robot.oi.throttles.ThrottleBasic: + overridenPushGearThrottle + - org.usfirst.frc.team449.robot.oi.throttles.ThrottleBasic: + overridenClimbButtonThrottle + - org.usfirst.frc.team449.robot.oi.throttles.ThrottleBasic: + overridingPushGearThrottle + - org.usfirst.frc.team449.robot.oi.throttles.ThrottleBasic: + overridingClimbButtonThrottle \ No newline at end of file diff --git a/RoboRIO/src/main/resources/nate_map.yml b/RoboRIO/src/main/resources/nate_map.yml index aec07b64..78a85832 100644 --- a/RoboRIO/src/main/resources/nate_map.yml +++ b/RoboRIO/src/main/resources/nate_map.yml @@ -662,7 +662,7 @@ buttons: climber action: WHEN_PRESSED - button: - org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: + org.usfirst.frc.team449.robot.oi.buttons.SimpleButton: climbButton command: org.usfirst.frc.team449.robot.subsystem.interfaces.binaryMotor.commands.TurnMotorOffWithRequires: @@ -719,7 +719,7 @@ buttons: '@id': openGearHandlerCommand action: WHEN_PRESSED - button: - org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: + org.usfirst.frc.team449.robot.oi.buttons.SimpleButton: pushGearButton command: org.usfirst.frc.team449.robot.subsystem.interfaces.solenoid.commands.SolenoidForward: @@ -727,4 +727,14 @@ buttons: subsystem: org.usfirst.frc.team449.robot.subsystem.interfaces.solenoid.SolenoidSimple: gearHandler - action: WHEN_RELEASED \ No newline at end of file + action: WHEN_RELEASED +updater: + org.usfirst.frc.team449.robot.other.Updater: + '@id': updater + updatables: + - org.usfirst.frc.team449.robot.drive.unidirectional.DriveTalonClusterShiftable: + drive + - org.usfirst.frc.team449.robot.subsystem.complex.climber.ClimberCurrentLimited: + climber + - org.usfirst.frc.team449.robot.oi.unidirectional.arcade.OIArcadeWithDPad: + oi \ No newline at end of file diff --git a/RoboRIO/src/main/resources/naveen_map.yml b/RoboRIO/src/main/resources/naveen_map.yml index 95233714..ca8deccc 100644 --- a/RoboRIO/src/main/resources/naveen_map.yml +++ b/RoboRIO/src/main/resources/naveen_map.yml @@ -723,4 +723,18 @@ buttons: subsystem: org.usfirst.frc.team449.robot.subsystem.interfaces.solenoid.SolenoidSimple: gearHandler - action: WHEN_RELEASED \ No newline at end of file + action: WHEN_RELEASED +updater: + org.usfirst.frc.team449.robot.other.Updater: + '@id': updater + updatables: + - org.usfirst.frc.team449.robot.drive.unidirectional.DriveTalonClusterShiftable: + drive + - org.usfirst.frc.team449.robot.subsystem.complex.climber.ClimberCurrentLimited: + climber + - org.usfirst.frc.team449.robot.oi.unidirectional.arcade.OIArcadeWithDPad: + oi + - org.usfirst.frc.team449.robot.oi.throttles.ThrottleBasic: + pushGearThrottle + - org.usfirst.frc.team449.robot.oi.throttles.ThrottleBasic: + climbButtonThrottle \ No newline at end of file