Skip to content

Commit 465ce86

Browse files
author
Noah Gleason
authored
Merge pull request #106 from blair-robot-project/cache_values
Cache values
2 parents 5208aa3 + 72477f8 commit 465ce86

File tree

54 files changed

+920
-266
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

54 files changed

+920
-266
lines changed

‎RoboRIO/src/main/java/org/usfirst/frc/team449/robot/Robot.java‎

Lines changed: 27 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -93,8 +93,8 @@ public void robotInit() {
9393
//Read the yaml file with SnakeYaml so we can use anchors and merge syntax.
9494
// Map<?, ?> normalized = (Map<?, ?>) yaml.load(new FileReader(RESOURCES_PATH+"ballbasaur_map.yml"));
9595
// Map<?, ?> normalized = (Map<?, ?>) yaml.load(new FileReader(RESOURCES_PATH + "naveen_map.yml"));
96-
// Map<?, ?> normalized = (Map<?, ?>) yaml.load(new FileReader(RESOURCES_PATH + "nate_map.yml"));
97-
Map<?, ?> normalized = (Map<?, ?>) yaml.load(new FileReader(RESOURCES_PATH + "calcifer_outreach_map.yml"));
96+
Map<?, ?> normalized = (Map<?, ?>) yaml.load(new FileReader(RESOURCES_PATH + "nate_map.yml"));
97+
// Map<?, ?> normalized = (Map<?, ?>) yaml.load(new FileReader(RESOURCES_PATH + "calcifer_outreach_map.yml"));
9898
YAMLMapper mapper = new YAMLMapper();
9999
//Turn the Map read by SnakeYaml into a String so Jackson can read it.
100100
String fixed = mapper.writeValueAsString(normalized);
@@ -107,6 +107,10 @@ public void robotInit() {
107107
System.out.println("Config file is bad/nonexistent!");
108108
e.printStackTrace();
109109
}
110+
111+
//Read sensors
112+
this.robotMap.getUpdater().run();
113+
110114
//Set fields from the map.
111115
this.loggerNotifier = new Notifier(robotMap.getLogger());
112116
this.driveSubsystem = robotMap.getDrive();
@@ -179,6 +183,12 @@ public void robotInit() {
179183
*/
180184
@Override
181185
public void teleopInit() {
186+
//Do the startup tasks
187+
doStartupTasks();
188+
189+
//Read sensors
190+
this.robotMap.getUpdater().run();
191+
182192
//Run startup command if we start in teleop.
183193
if (!enabled) {
184194
if (robotMap.getStartupCommand() != null) {
@@ -187,9 +197,8 @@ public void teleopInit() {
187197
enabled = true;
188198
}
189199

190-
//Do the startup tasks
191200
driveSubsystem.stopMPProcesses();
192-
doStartupTasks();
201+
193202
if (robotMap.getTeleopStartupCommand() != null) {
194203
robotMap.getTeleopStartupCommand().start();
195204
}
@@ -208,6 +217,10 @@ public void teleopInit() {
208217
public void teleopPeriodic() {
209218
//Refresh the current time.
210219
Clock.updateTime();
220+
221+
//Read sensors
222+
this.robotMap.getUpdater().run();
223+
211224
//Run all commands. This is a WPILib thing you don't really have to worry about.
212225
Scheduler.getInstance().run();
213226
}
@@ -217,6 +230,12 @@ public void teleopPeriodic() {
217230
*/
218231
@Override
219232
public void autonomousInit() {
233+
//Do startup tasks
234+
doStartupTasks();
235+
236+
//Read sensors
237+
this.robotMap.getUpdater().run();
238+
220239
//Run startup command if we start in auto.
221240
if (!enabled) {
222241
if (robotMap.getStartupCommand() != null) {
@@ -225,8 +244,6 @@ public void autonomousInit() {
225244
enabled = true;
226245
}
227246

228-
//Do startup tasks
229-
doStartupTasks();
230247
if (robotMap.getAutoStartupCommand() != null) {
231248
robotMap.getAutoStartupCommand().start();
232249
}
@@ -247,6 +264,8 @@ public void autonomousInit() {
247264
public void autonomousPeriodic() {
248265
//Update the current time
249266
Clock.updateTime();
267+
//Read sensors
268+
this.robotMap.getUpdater().run();
250269
//Run all commands. This is a WPILib thing you don't really have to worry about.
251270
Scheduler.getInstance().run();
252271
}
@@ -282,6 +301,8 @@ public void testInit() {
282301
@Override
283302
public void disabledPeriodic() {
284303
Clock.updateTime();
304+
//Read sensors
305+
this.robotMap.getUpdater().run();
285306
}
286307

287308
/**

‎RoboRIO/src/main/java/org/usfirst/frc/team449/robot/RobotMap2017.java‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
import org.jetbrains.annotations.Nullable;
88
import org.usfirst.frc.team449.robot.drive.unidirectional.DriveTalonCluster;
99
import org.usfirst.frc.team449.robot.jacksonWrappers.MappedDigitalInput;
10+
import org.usfirst.frc.team449.robot.jacksonWrappers.MappedRunnable;
1011
import org.usfirst.frc.team449.robot.jacksonWrappers.YamlCommand;
1112
import org.usfirst.frc.team449.robot.oi.buttons.CommandButton;
1213
import org.usfirst.frc.team449.robot.oi.unidirectional.OIUnidirectional;
@@ -58,6 +59,12 @@ public class RobotMap2017 {
5859
@NotNull
5960
private final Command defaultDriveCommand;
6061

62+
/**
63+
* A runnable that updates cached variables.
64+
*/
65+
@NotNull
66+
private final Runnable updater;
67+
6168
/**
6269
* The climber for boarding the airship. Can be null.
6370
*/
@@ -215,6 +222,7 @@ public class RobotMap2017 {
215222
* @param logger The logger for recording events and telemetry data.
216223
* @param drive The drive.
217224
* @param defaultDriveCommand The command for the drive to run during the teleoperated period.
225+
* @param updater A runnable that updates cached variables.
218226
* @param climber The climber for boarding the airship. Can be null.
219227
* @param shooter The multiSubsystem for shooting fuel. Can be null.
220228
* @param camera The cameras on this robot. Can be null.
@@ -258,6 +266,7 @@ public RobotMap2017(@Nullable List<CommandButton> buttons,
258266
@NotNull @JsonProperty(required = true) Logger logger,
259267
@NotNull @JsonProperty(required = true) DriveTalonCluster drive,
260268
@NotNull @JsonProperty(required = true) YamlCommand defaultDriveCommand,
269+
@NotNull @JsonProperty(required = true) MappedRunnable updater,
261270
@Nullable ClimberCurrentLimited climber,
262271
@Nullable LoggingShooter shooter,
263272
@Nullable CameraNetwork camera,
@@ -289,6 +298,7 @@ public RobotMap2017(@Nullable List<CommandButton> buttons,
289298
this.pneumatics = pneumatics;
290299
this.gearHandler = gearHandler;
291300
this.logger = logger;
301+
this.updater = updater;
292302
this.RIOduinoPort = RIOduinoPort;
293303
this.allianceSwitch = allianceSwitch;
294304
this.dropGearSwitch = dropGearSwitch;
@@ -542,4 +552,12 @@ public boolean getDoMP() {
542552
public Command getStartupCommand() {
543553
return startupCommand;
544554
}
555+
556+
/**
557+
* @return A runnable that updates cached variables.
558+
*/
559+
@NotNull
560+
public Runnable getUpdater() {
561+
return updater;
562+
}
545563
}

‎RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/RunMotorWhileConditonMet.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ protected void execute() {
5757
*/
5858
@Override
5959
protected boolean isFinished() {
60-
return subsystem.isConditionTrue();
60+
return subsystem.isConditionTrueCached();
6161
}
6262

6363
/**

‎RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/JiggleRobot.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,13 +9,13 @@
99
import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional;
1010
import org.usfirst.frc.team449.robot.jacksonWrappers.YamlCommandGroupWrapper;
1111
import org.usfirst.frc.team449.robot.jacksonWrappers.YamlSubsystem;
12-
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemNavX;
12+
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemAHRS;
1313

1414
/**
1515
* Rotates the robot back and forth in order to dislodge any stuck balls.
1616
*/
1717
@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class)
18-
public class JiggleRobot <T extends YamlSubsystem & DriveUnidirectional & SubsystemNavX> extends YamlCommandGroupWrapper {
18+
public class JiggleRobot <T extends YamlSubsystem & DriveUnidirectional & SubsystemAHRS> extends YamlCommandGroupWrapper {
1919

2020
/**
2121
* Instantiate the CommandGroup

‎RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/NavXDriveStraight.java‎

Lines changed: 4 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -10,14 +10,14 @@
1010
import org.usfirst.frc.team449.robot.jacksonWrappers.YamlSubsystem;
1111
import org.usfirst.frc.team449.robot.oi.unidirectional.tank.OITank;
1212
import org.usfirst.frc.team449.robot.other.Logger;
13-
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemNavX;
13+
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemAHRS;
1414
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.commands.PIDAngleCommand;
1515

1616
/**
1717
* Drives straight using the NavX gyro to keep a constant alignment.
1818
*/
1919
@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class)
20-
public class NavXDriveStraight <T extends YamlSubsystem & DriveUnidirectional & SubsystemNavX> extends PIDAngleCommand {
20+
public class NavXDriveStraight <T extends YamlSubsystem & DriveUnidirectional & SubsystemAHRS> extends PIDAngleCommand {
2121

2222
/**
2323
* The drive subsystem to give output to.
@@ -87,15 +87,11 @@ protected void usePIDOutput(double output) {
8787
output = processPIDOutput(output);
8888

8989
//Set throttle to the specified stick.
90-
double throttle;
9190
if (useLeft) {
92-
throttle = oi.getLeftThrottle();
91+
subsystem.setOutput(oi.getLeftOutputCached() - output, oi.getLeftOutputCached() + output);
9392
} else {
94-
throttle = oi.getRightThrottle();
93+
subsystem.setOutput(oi.getRightOutputCached() - output, oi.getRightOutputCached() + output);
9594
}
96-
97-
//Set the output to the throttle velocity adjusted by the PID output.
98-
subsystem.setOutput(throttle - output, throttle + output);
9995
}
10096

10197
/**

‎RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/NavXTurnToAngle.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -11,14 +11,14 @@
1111
import org.usfirst.frc.team449.robot.jacksonWrappers.YamlSubsystem;
1212
import org.usfirst.frc.team449.robot.other.Clock;
1313
import org.usfirst.frc.team449.robot.other.Logger;
14-
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemNavX;
14+
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemAHRS;
1515
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.commands.PIDAngleCommand;
1616

1717
/**
1818
* Turns to a specified angle, relative to the angle the navX was at when the robot was turned on.
1919
*/
2020
@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class)
21-
public class NavXTurnToAngle <T extends YamlSubsystem & DriveUnidirectional & SubsystemNavX> extends PIDAngleCommand {
21+
public class NavXTurnToAngle <T extends YamlSubsystem & DriveUnidirectional & SubsystemAHRS> extends PIDAngleCommand {
2222

2323
/**
2424
* The drive subsystem to execute this command on and to get the gyro reading from.

‎RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/NavXTurnToAngleRelative.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,13 +10,13 @@
1010
import org.usfirst.frc.team449.robot.jacksonWrappers.YamlSubsystem;
1111
import org.usfirst.frc.team449.robot.other.Clock;
1212
import org.usfirst.frc.team449.robot.other.Logger;
13-
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemNavX;
13+
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemAHRS;
1414

1515
/**
1616
* Turn a certain number of degrees from the current heading.
1717
*/
1818
@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class)
19-
public class NavXTurnToAngleRelative <T extends YamlSubsystem & DriveUnidirectional & SubsystemNavX> extends NavXTurnToAngle {
19+
public class NavXTurnToAngleRelative <T extends YamlSubsystem & DriveUnidirectional & SubsystemAHRS> extends NavXTurnToAngle {
2020

2121
/**
2222
* Default constructor.
@@ -62,7 +62,7 @@ protected void initialize() {
6262
this.startTime = Clock.currentTimeMillis();
6363
Logger.addEvent("NavXRelativeTurnToAngle init.", this.getClass());
6464
//Do math to setup the setpoint.
65-
this.setSetpoint(clipTo180(((SubsystemNavX) subsystem).getGyroHeading() + setpoint));
65+
this.setSetpoint(clipTo180(((SubsystemAHRS) subsystem).getHeadingCached() + setpoint));
6666
//Make sure to enable the controller!
6767
this.getPIDController().enable();
6868
}

‎RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/UnidirectionalNavXDefaultDrive.java‎

Lines changed: 9 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -8,15 +8,15 @@
88
import org.usfirst.frc.team449.robot.oi.unidirectional.OIUnidirectional;
99
import org.usfirst.frc.team449.robot.other.BufferTimer;
1010
import org.usfirst.frc.team449.robot.other.Logger;
11-
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemNavX;
11+
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemAHRS;
1212
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.commands.PIDAngleCommand;
1313

1414
/**
1515
* Drive with arcade drive setup, and when the driver isn't turning, use a NavX to stabilize the robot's alignment.
1616
*/
1717
@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class")
1818
@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class)
19-
public class UnidirectionalNavXDefaultDrive <T extends YamlSubsystem & DriveUnidirectional & SubsystemNavX> extends PIDAngleCommand {
19+
public class UnidirectionalNavXDefaultDrive <T extends YamlSubsystem & DriveUnidirectional & SubsystemAHRS> extends PIDAngleCommand {
2020

2121
/**
2222
* The drive this command is controlling.
@@ -116,23 +116,20 @@ protected void initialize() {
116116
*/
117117
@Override
118118
protected void execute() {
119-
//Check whether we're commanding to drive straight or turn.
120-
boolean commandingStraight = oi.commandingStraight();
121-
122119
//If we're driving straight but the driver tries to turn or overrides the navX:
123-
if (drivingStraight && (!commandingStraight || subsystem.getOverrideNavX())) {
120+
if (drivingStraight && (!oi.commandingStraightCached() || subsystem.getOverrideGyro())) {
124121
//Switch to free drive
125122
drivingStraight = false;
126123
Logger.addEvent("Switching to free drive.", this.getClass());
127124
}
128125
//If we're free driving and the driver stops turning:
129-
else if (driveStraightLoopEntryTimer.get(!(subsystem.getOverrideNavX()) && !(drivingStraight) &&
130-
commandingStraight && Math.abs(subsystem.getNavX().getRate()) <= maxAngularVelToEnterLoop)) {
126+
else if (driveStraightLoopEntryTimer.get(!(subsystem.getOverrideGyro()) && !(drivingStraight) &&
127+
oi.commandingStraightCached() && Math.abs(subsystem.getAngularVelCached()) <= maxAngularVelToEnterLoop)) {
131128
//Switch to driving straight
132129
drivingStraight = true;
133130
//Set the setpoint to the current heading and reset the navX
134131
this.getPIDController().reset();
135-
this.getPIDController().setSetpoint(subsystem.getGyroHeading());
132+
this.getPIDController().setSetpoint(subsystem.getHeadingCached());
136133
this.getPIDController().enable();
137134
Logger.addEvent("Switching to DriveStraight.", this.getClass());
138135
}
@@ -178,17 +175,17 @@ protected void usePIDOutput(double output) {
178175
output = processPIDOutput(output);
179176

180177
//Deadband if we're stationary
181-
if (oi.getLeftOutput() == 0 || oi.getRightOutput() == 0) {
178+
if (oi.getLeftOutputCached() == 0 || oi.getRightOutputCached() == 0) {
182179
output = deadbandOutput(output);
183180
}
184181

185182
//Adjust the heading according to the PID output, it'll be positive if we want to go right.
186-
subsystem.setOutput(oi.getLeftOutput() - output, oi.getRightOutput() + output);
183+
subsystem.setOutput(oi.getLeftOutputCached() - output, oi.getRightOutputCached() + output);
187184
}
188185
//If we're free driving...
189186
else {
190187
//Set the throttle to normal arcade throttle.
191-
subsystem.setOutput(oi.getLeftOutput(), oi.getRightOutput());
188+
subsystem.setOutput(oi.getLeftOutputCached(), oi.getRightOutputCached());
192189
}
193190
}
194191
}

‎RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/UnidirectionalNavXShiftingDefaultDrive.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -11,15 +11,15 @@
1111
import org.usfirst.frc.team449.robot.oi.unidirectional.OIUnidirectional;
1212
import org.usfirst.frc.team449.robot.other.BufferTimer;
1313
import org.usfirst.frc.team449.robot.other.Logger;
14-
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemNavX;
14+
import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemAHRS;
1515

1616
/**
1717
* Drive with arcade drive setup, autoshift, and when the driver isn't turning, use a NavX to stabilize the robot's
1818
* alignment.
1919
*/
2020
@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class")
2121
@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class)
22-
public class UnidirectionalNavXShiftingDefaultDrive <T extends YamlSubsystem & DriveUnidirectional & SubsystemNavX & DriveShiftable> extends UnidirectionalNavXDefaultDrive {
22+
public class UnidirectionalNavXShiftingDefaultDrive <T extends YamlSubsystem & DriveUnidirectional & SubsystemAHRS & DriveShiftable> extends UnidirectionalNavXDefaultDrive {
2323

2424
/**
2525
* The drive to execute this command on.
@@ -92,8 +92,8 @@ public UnidirectionalNavXShiftingDefaultDrive(@JsonProperty(required = true) dou
9292
public void execute() {
9393
//Auto-shifting
9494
if (!subsystem.getOverrideAutoshift()) {
95-
autoshiftComponent.autoshift(oi.getLeftOutput(), oi.getRightOutput(), subsystem.getLeftVel(),
96-
subsystem.getRightVel(), gear -> subsystem.setGear(gear));
95+
autoshiftComponent.autoshift(oi.getLeftOutputCached(), oi.getRightOutputCached(), subsystem.getLeftVelCached(),
96+
subsystem.getRightVelCached(), gear -> subsystem.setGear(gear));
9797
}
9898
super.execute();
9999
}

0 commit comments

Comments
 (0)