From 790ababf8a08d8f088b4658cc26f04935cf10ad3 Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Tue, 27 Jun 2017 23:43:16 -0400 Subject: [PATCH 1/4] Annotate all objects to be jackson-compatible. --- .../frc/team449/robot/oi/buttons/FactoryButton.java | 6 ++---- .../robot/oi/buttons/FactoryJoystickButton.java | 12 +++++++++++- .../frc/team449/robot/oi/buttons/TriggerButton.java | 8 +++++++- .../frc/team449/robot/oi/buttons/dPadButton.java | 10 +++++++++- .../usfirst/frc/team449/robot/util/BufferTimer.java | 8 +++++++- .../org/usfirst/frc/team449/robot/util/LogEvent.java | 2 ++ 6 files changed, 38 insertions(+), 8 deletions(-) diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryButton.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryButton.java index 90459b13..206256a3 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryButton.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryButton.java @@ -1,9 +1,6 @@ package org.usfirst.frc.team449.robot.oi.buttons; -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 com.fasterxml.jackson.annotation.*; import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.buttons.JoystickButton; import org.jetbrains.annotations.NotNull; @@ -13,6 +10,7 @@ /** * A factory for constructing a button.. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") @JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public abstract class FactoryButton extends Button { diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryJoystickButton.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryJoystickButton.java index 34eacfb8..5dcf8dd2 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryJoystickButton.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryJoystickButton.java @@ -1,11 +1,19 @@ package org.usfirst.frc.team449.robot.oi.buttons; +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 edu.wpi.first.wpilibj.Joystick; import org.jetbrains.annotations.NotNull; +import org.usfirst.frc.team449.robot.components.MappedJoystick; + +import java.util.Map; /** * A version of {@link edu.wpi.first.wpilibj.buttons.JoystickButton} that is a FactoryButton. */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public class FactoryJoystickButton extends FactoryButton { /** @@ -25,7 +33,9 @@ public class FactoryJoystickButton extends FactoryButton { * @param joystick The joystick the button is on. * @param buttonNum The port of the button. Note that button numbers begin at 1, not 0. */ - FactoryJoystickButton(@NotNull Joystick joystick, int buttonNum) { + @JsonCreator + public FactoryJoystickButton(@NotNull @JsonProperty(required = true) MappedJoystick joystick, + @JsonProperty(required = true) int buttonNum) { this.joystick = joystick; this.buttonNum = buttonNum; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/TriggerButton.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/TriggerButton.java index 548b9be6..84e5741c 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/TriggerButton.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/TriggerButton.java @@ -1,5 +1,8 @@ package org.usfirst.frc.team449.robot.oi.buttons; +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.components.MappedJoystick; import org.usfirst.frc.team449.robot.components.MappedSmoothedThrottle; @@ -8,6 +11,7 @@ /** * A button that gets triggered by a specific throttle being held down at or over a certain amount. */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public class TriggerButton extends FactoryButton { /** @@ -28,7 +32,9 @@ public class TriggerButton extends FactoryButton { * @param axis The axis of the throttle. * @param triggerAt The percentage pressed to trigger at, from (0, 1] */ - TriggerButton(@NotNull MappedJoystick joystick, int axis, double triggerAt) { + public TriggerButton(@NotNull @JsonProperty(required = true) MappedJoystick joystick, + @JsonProperty(required = true) int axis, + @JsonProperty(required = true) double triggerAt) { throttle = new MappedSmoothedThrottle(joystick, axis, 0, 0, false); this.triggerAt = triggerAt; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/dPadButton.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/dPadButton.java index c9cd9660..34b558a9 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/dPadButton.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/dPadButton.java @@ -1,11 +1,17 @@ package org.usfirst.frc.team449.robot.oi.buttons; +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 edu.wpi.first.wpilibj.Joystick; import org.jetbrains.annotations.NotNull; +import org.usfirst.frc.team449.robot.components.MappedJoystick; /** * A Button triggered by pushing the D-pad to a specific angle. */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public class dPadButton extends FactoryButton { /** @@ -25,7 +31,9 @@ public class dPadButton extends FactoryButton { * @param joystick The joystick with the D-pad. * @param angle The angle that the D-pad must be pushed to to trigger this button. */ - dPadButton(@NotNull Joystick joystick, int angle) { + @JsonCreator + public dPadButton(@NotNull @JsonProperty(required = true) MappedJoystick joystick, + @JsonProperty(required = true) int angle) { this.angle = angle; this.joystick = joystick; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/BufferTimer.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/BufferTimer.java index e9981b0a..15e0238c 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/BufferTimer.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/BufferTimer.java @@ -1,10 +1,15 @@ package org.usfirst.frc.team449.robot.util; +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.usfirst.frc.team449.robot.Robot; /** * A timer that checks if condition has been true for the past n seconds/milliseconds. */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public class BufferTimer { /** @@ -27,7 +32,8 @@ public class BufferTimer { * * @param bufferTimeSeconds The amount of time the condition has to be true for, in seconds. */ - public BufferTimer(double bufferTimeSeconds) { + @JsonCreator + public BufferTimer(@JsonProperty(required = true) double bufferTimeSeconds) { bufferTime = (long) (bufferTimeSeconds * 1000.); } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/LogEvent.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/LogEvent.java index fd132773..33330029 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/LogEvent.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/LogEvent.java @@ -21,11 +21,13 @@ public class LogEvent { /** * The message of this event. */ + @NotNull private final String message; /** * The class that called this event. */ + @NotNull private final Class caller; /** From e60db91da7a2df0b7f8707983e1f0b40a1ac1e90 Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Tue, 27 Jun 2017 23:46:12 -0400 Subject: [PATCH 2/4] Cleanup. --- .../usfirst/frc/team449/robot/RobotMap.java | 19 +++++---- .../robot/components/MappedThrottle.java | 6 ++- .../UnidirectionalNavXDefaultDrive.java | 40 +++++++++---------- .../team449/robot/interfaces/oi/ArcadeOI.java | 6 ++- .../team449/robot/interfaces/oi/TankOI.java | 3 ++ .../robot/interfaces/oi/UnidirectionalOI.java | 1 + .../NavX/commands/PIDAngleCommand.java | 2 +- .../frc/team449/robot/oi/SimpleArcadeOI.java | 1 - .../frc/team449/robot/oi/SimpleTankOI.java | 18 ++++----- .../oi/buttons/FactoryJoystickButton.java | 2 - .../robot/util/AutoshiftProcessor.java | 21 ++++++---- .../frc/team449/robot/util/Logger.java | 7 +++- .../team449/robot/util/MotionProfileData.java | 3 +- .../frc/team449/robot/util/Polynomial.java | 10 ++++- .../team449/robot/vision/CameraSubsystem.java | 4 ++ 15 files changed, 88 insertions(+), 55 deletions(-) diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/RobotMap.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/RobotMap.java index cbe771c4..ca520178 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/RobotMap.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/RobotMap.java @@ -7,7 +7,6 @@ import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.components.MappedDigitalInput; import org.usfirst.frc.team449.robot.drive.talonCluster.TalonClusterDrive; -import org.usfirst.frc.team449.robot.drive.talonCluster.commands.UnidirectionalNavXDefaultDrive; import org.usfirst.frc.team449.robot.mechanism.activegear.ActiveGearSubsystem; import org.usfirst.frc.team449.robot.mechanism.climber.ClimberSubsystem; import org.usfirst.frc.team449.robot.mechanism.intake.Intake2017.Intake2017; @@ -119,18 +118,24 @@ public class RobotMap { * testMP is true, but otherwise must have a value. * @param dropGearSwitch The switch for deciding whether or not to drop the gear. Can be null if doMP is false * or testMP is true, but otherwise must have a value. - * @param locationDial The dial for selecting which side of the field the robot is on. Can be null if doMP is false or testMP is true, but otherwise must have a value. - * @param boilerAuto The command to run in autonomous on the boiler side of the field. Can be null if doMP is false or testMP is true, but otherwise must have a value. + * @param locationDial The dial for selecting which side of the field the robot is on. Can be null if doMP is + * false or testMP is true, but otherwise must have a value. + * @param boilerAuto The command to run in autonomous on the boiler side of the field. Can be null if doMP + * is false or testMP is true, but otherwise must have a value. * @param centerAuto The command to run in autonomous on the center of the field. Can be null if doMP is * false or testMP is true, but otherwise must have a value. - * @param feederAuto The command to run in autonomous on the feeding station side of the field. Can be null if doMP is false or testMP is true, but otherwise must have a value. + * @param feederAuto The command to run in autonomous on the feeding station side of the field. Can be null + * if doMP is false or testMP is true, but otherwise must have a value. * @param leftTestProfile The profile for the left side of the drive to run in test mode. Can be null if either * testMP or doMP are false, but otherwise must have a value. - * @param rightTestProfile The profile for the right side of the drive to run in test mode. Can be null if either testMP or doMP are false, but otherwise must have a value. + * @param rightTestProfile The profile for the right side of the drive to run in test mode. Can be null if either + * testMP or doMP are false, but otherwise must have a value. * @param leftProfiles The starting position to peg profiles for the left side. Should have options for - * "red_right", "red_center", "red_left", "blue_right", "blue_center", and "blue_left". Can be null if doMP is false or testMP is true, but otherwise must have a value. + * "red_right", "red_center", "red_left", "blue_right", "blue_center", and "blue_left". + * Can be null if doMP is false or testMP is true, but otherwise must have a value. * @param rightProfiles The starting position to peg profiles for the right side. Should have options for - * "red_right", "red_center", "red_left", "blue_right", "blue_center", and "blue_left". Can be null if doMP is false or testMP is true, but otherwise must have a value. + * "red_right", "red_center", "red_left", "blue_right", "blue_center", and "blue_left". + * Can be null if doMP is false or testMP is true, but otherwise must have a value. * @param nonMPAutoCommand The command to run during autonomous if doMP is false. Can be null, and if it is, no * command is run during autonomous. * @param testMP Whether to run the test or real motion profile during autonomous. Defaults to false. diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedThrottle.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedThrottle.java index 2c5c61f3..1388a8cf 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedThrottle.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedThrottle.java @@ -47,10 +47,12 @@ public MappedThrottle(@NotNull @JsonProperty(required = true) MappedJoystick sti } /** - * Gets the raw value from the stick and inverts it if necessary. This is private so it's not overriden, allowing it to be used by both getValue and pidGet without causing a circular reference. + * Gets the raw value from the stick and inverts it if necessary. This is private so it's not overriden, allowing it + * to be used by both getValue and pidGet without causing a circular reference. + * * @return The raw joystick output, on [-1, 1]. */ - private double getValuePrivate(){ + private double getValuePrivate() { return (inverted ? -1 : 1) * stick.getRawAxis(axis); } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/UnidirectionalNavXDefaultDrive.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/UnidirectionalNavXDefaultDrive.java index 4e08e2fe..bbc20f1b 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/UnidirectionalNavXDefaultDrive.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/UnidirectionalNavXDefaultDrive.java @@ -50,26 +50,26 @@ public class UnidirectionalNavXDefaultDrive shift) { if (shouldDownshift(leftThrottle, rightThrottle, leftVel, rightVel)) { diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/Logger.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/Logger.java index 6c4566f7..9e0b14c9 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/Logger.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/Logger.java @@ -62,8 +62,10 @@ public class Logger implements Runnable { * * @param subsystems The subsystems to log telemetry data from. * @param loopTimeSecs The period of the loop for collecting telemetry data, in seconds. - * @param eventLogFilename The filepath of the log for events. Will have the timestamp and file extension appended onto the end. - * @param telemetryLogFilename The filepath of the log for telemetry data. Will have the timestamp and file extension appended onto the end. + * @param eventLogFilename The filepath of the log for events. Will have the timestamp and file extension + * appended onto the end. + * @param telemetryLogFilename The filepath of the log for telemetry data. Will have the timestamp and file + * extension appended onto the end. * @throws IOException If the file names provided from the log can't be written to. */ @JsonCreator @@ -219,6 +221,7 @@ public void run() { /** * Getter for the loop time of this logger. + * * @return The map-specified loop period of this logger, in seconds. */ public double getLoopTimeSecs() { diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/MotionProfileData.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/MotionProfileData.java index a8ae10c6..ad63d4fe 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/MotionProfileData.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/MotionProfileData.java @@ -30,7 +30,8 @@ public class MotionProfileData { /** * Default constructor * - * @param filename The filename of the .csv with the motion profile data. The first line must be the number of other lines. + * @param filename The filename of the .csv with the motion profile data. The first line must be the number of other + * lines. * @param inverted Whether or not the profile is inverted (would be inverted if we're driving it backwards) */ @JsonCreator diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/Polynomial.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/Polynomial.java index 7dd8b279..c9dd39f8 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/Polynomial.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/Polynomial.java @@ -38,8 +38,11 @@ public class Polynomial { /** * Default constructor. - * @param powerToCoefficientMap A map of the powers and coefficients of each term. Defaults to [1:1] if null or 0-length. - * @param scaleCoefficientSumTo Scales each coefficient so they all add up to this number. Can be null to avoid scaling. + * + * @param powerToCoefficientMap A map of the powers and coefficients of each term. Defaults to [1:1] if null or + * 0-length. + * @param scaleCoefficientSumTo Scales each coefficient so they all add up to this number. Can be null to avoid + * scaling. */ @JsonCreator public Polynomial(@Nullable Map powerToCoefficientMap, @@ -60,6 +63,7 @@ public Polynomial(@Nullable Map powerToCoefficientMap, /** * Get the value of the polynomial given x. + * * @param x The variable to be given to the polynomial. * @return The value of the polynomial evaluated at |x|, then changed to the sign of x. */ @@ -75,6 +79,7 @@ public double get(double x) { /** * Scale each coefficient so they sum to a given number. + * * @param scaleTo The number to scale the sum of coefficients to. */ public void scaleCoefficientSum(double scaleTo) { @@ -90,6 +95,7 @@ public void scaleCoefficientSum(double scaleTo) { /** * Getter for the map of the powers and coefficients of each term. + * * @return powerToCoefficientMap. */ @NotNull diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/vision/CameraSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/vision/CameraSubsystem.java index 2f778774..64524f82 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/vision/CameraSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/vision/CameraSubsystem.java @@ -78,6 +78,7 @@ protected void initDefaultCommand() { /** * Getter for the server. + * * @return The server the camera feed streams to. */ @NotNull @@ -87,6 +88,7 @@ public MjpegServer getServer() { /** * Getter for the list of cameras. + * * @return The list of cameras the subsystem reads from. */ @NotNull @@ -96,6 +98,7 @@ public List getCameras() { /** * Getter for the number of the active camera. + * * @return The index of the active camera in the list of cameras. */ public int getCamNum() { @@ -104,6 +107,7 @@ public int getCamNum() { /** * Setter for the number of the active camera. + * * @param camNum The index of the active camera in the list of cameras. */ public void setCamNum(int camNum) { From e8e2f038eb0116e21e555bd14da03ecd03ba4509 Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Wed, 28 Jun 2017 00:11:53 -0400 Subject: [PATCH 3/4] Update map. --- ...hiftingUnidirectionalNavXDefaultDrive.java | 46 +++--- .../UnidirectionalNavXDefaultDrive.java | 47 +++--- .../mechanism/climber/ClimberSubsystem.java | 25 ++-- .../robot/util/AutoshiftProcessor.java | 33 ++--- .../frc/team449/robot/util/BufferTimer.java | 5 +- RoboRIO/src/main/resources/calcifer_map.yml | 135 ++++++++++-------- 6 files changed, 152 insertions(+), 139 deletions(-) diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ShiftingUnidirectionalNavXDefaultDrive.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ShiftingUnidirectionalNavXDefaultDrive.java index 99c728ed..454246d2 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ShiftingUnidirectionalNavXDefaultDrive.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ShiftingUnidirectionalNavXDefaultDrive.java @@ -8,6 +8,7 @@ import org.usfirst.frc.team449.robot.interfaces.oi.UnidirectionalOI; import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.NavxSubsystem; import org.usfirst.frc.team449.robot.util.AutoshiftProcessor; +import org.usfirst.frc.team449.robot.util.BufferTimer; import org.usfirst.frc.team449.robot.util.Logger; import org.usfirst.frc.team449.robot.util.YamlSubsystem; @@ -34,27 +35,26 @@ public class ShiftingUnidirectionalNavXDefaultDrive maxPower); + return powerLimitTimer.get(Math.abs(canTalonSRX.getPower()) > maxPower); } } \ No newline at end of file diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/AutoshiftProcessor.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/AutoshiftProcessor.java index 8cc9eae6..627c934f 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/AutoshiftProcessor.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/AutoshiftProcessor.java @@ -61,35 +61,32 @@ public class AutoshiftProcessor { /** * Default constructor * - * @param upshiftSpeed The minimum speed both sides the drive must be going at to shift to high - * gear. - * @param downshiftSpeed The maximum speed both sides must be going at to shift to low gear. - * @param delayAfterUpshiftConditionsMet How long, in seconds, the conditions to upshift have to be met for before - * upshifting happens. Defaults to 0. - * @param delayAfterDownshiftConditionsMet How long, in seconds, the conditions to downshift have to be met for - * before downshifting happens. Defaults to 0. - * @param cooldownAfterDownshift The minimum time, in seconds, between downshifting and then upshifting - * again. Defaults to 0. - * @param cooldownAfterUpshift The minimum time, in seconds, between upshifting and then downshifting - * again. Defaults to 0. - * @param upshiftFwdThresh The minimum amount the forward joystick must be pushed forward in order - * to upshift, on [0, 1]. Defaults to 0. + * @param upshiftSpeed The minimum speed both sides the drive must be going at to shift to high gear. + * @param downshiftSpeed The maximum speed both sides must be going at to shift to low gear. + * @param upshiftBufferTimer Buffer timer for upshifting. + * @param downshiftBufferTimer Buffer timer for downshifting. + * @param cooldownAfterDownshift The minimum time, in seconds, between downshifting and then upshifting again. + * Defaults to 0. + * @param cooldownAfterUpshift The minimum time, in seconds, between upshifting and then downshifting again. + * Defaults to 0. + * @param upshiftFwdThresh The minimum amount the forward joystick must be pushed forward in order to upshift, + * on [0, 1]. Defaults to 0. */ @JsonCreator public AutoshiftProcessor(@JsonProperty(required = true) double upshiftSpeed, @JsonProperty(required = true) double downshiftSpeed, + @NotNull @JsonProperty(required = true) BufferTimer upshiftBufferTimer, + @NotNull @JsonProperty(required = true) BufferTimer downshiftBufferTimer, double upshiftFwdThresh, double cooldownAfterUpshift, - double cooldownAfterDownshift, - double delayAfterUpshiftConditionsMet, - double delayAfterDownshiftConditionsMet) { + double cooldownAfterDownshift) { this.upshiftSpeed = upshiftSpeed; this.downshiftSpeed = downshiftSpeed; this.upshiftFwdThresh = upshiftFwdThresh; this.cooldownAfterUpshift = (long) (cooldownAfterUpshift * 1000.); this.cooldownAfterDownshift = (long) (cooldownAfterDownshift * 1000.); - this.upshiftBufferTimer = new BufferTimer(delayAfterUpshiftConditionsMet); - this.downshiftBufferTimer = new BufferTimer(delayAfterDownshiftConditionsMet); + this.upshiftBufferTimer = upshiftBufferTimer; + this.downshiftBufferTimer = downshiftBufferTimer; } /** diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/BufferTimer.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/BufferTimer.java index 15e0238c..c10a5057 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/BufferTimer.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/util/BufferTimer.java @@ -2,7 +2,6 @@ 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.usfirst.frc.team449.robot.Robot; @@ -30,10 +29,10 @@ public class BufferTimer { /** * Constructor for a time given in seconds. * - * @param bufferTimeSeconds The amount of time the condition has to be true for, in seconds. + * @param bufferTimeSeconds The amount of time the condition has to be true for, in seconds. Defaults to 0. */ @JsonCreator - public BufferTimer(@JsonProperty(required = true) double bufferTimeSeconds) { + public BufferTimer(double bufferTimeSeconds) { bufferTime = (long) (bufferTimeSeconds * 1000.); } diff --git a/RoboRIO/src/main/resources/calcifer_map.yml b/RoboRIO/src/main/resources/calcifer_map.yml index 2bf24aae..82bab412 100644 --- a/RoboRIO/src/main/resources/calcifer_map.yml +++ b/RoboRIO/src/main/resources/calcifer_map.yml @@ -170,7 +170,9 @@ defaultDriveCommand: deadband: 1 maxAngularVelToEnterLoop: 0.05 inverted: true - loopEntryDelay: 0.15 + driveStraightLoopEntryTimer: + '@id': driveStraightLoopEntryTimer + bufferTimeSeconds: 0.15 subsystem: org.usfirst.frc.team449.robot.drive.talonCluster.ShiftingTalonClusterDrive: drive @@ -181,8 +183,12 @@ defaultDriveCommand: '@id': autoshiftProcessor upshiftSpeed: 4 downshiftSpeed: 2 - delayAfterUpshiftConditionsMet: 0.25 - delayAfterDownshiftConditionsMet: 0.0 + upshiftBufferTimer: + '@id': upshiftBufferTimer + bufferTimeSeconds: 0.25 + downshiftBufferTimer: + '@id': downshiftBufferTimer + bufferTimeSeconds: 0.0 upshiftFwdThresh: 0.3 climber: org.usfirst.frc.team449.robot.mechanism.climber.ClimberSubsystem: @@ -199,7 +205,9 @@ climber: '@id': climberVictor port: 0 inverted: false - millisAboveMaxPower: 5 + powerLimitTimer: + '@id': powerLimitTimer + bufferTimeSeconds: 0.005 #shooter: # org.usfirst.frc.team449.robot.mechanism.singleflywheelshooter.SingleFlywheelShooter: # '@id': shooter @@ -411,11 +419,12 @@ nonMPAutoCommand: drive buttons: - button: - '@id': switchToLowButton - joystick: - '@id': gunnerGamepad - port: 3 - angle: 180 + org.usfirst.frc.team449.robot.oi.buttons.dPadButton: + '@id': switchToLowButton + joystick: + '@id': gunnerGamepad + port: 3 + angle: 180 command: org.usfirst.frc.team449.robot.interfaces.drive.shifting.commands.SwitchToLowGear: '@id': switchToLowCommand @@ -424,10 +433,11 @@ buttons: drive action: WHEN_PRESSED - button: - '@id': switchToHighButton - joystick: - gunnerGamepad - angle: 0 + org.usfirst.frc.team449.robot.oi.buttons.dPadButton: + '@id': switchToHighButton + joystick: + gunnerGamepad + angle: 0 command: org.usfirst.frc.team449.robot.interfaces.drive.shifting.commands.SwitchToHighGear: '@id': switchToHighCommand @@ -436,10 +446,11 @@ buttons: drive action: WHEN_PRESSED - button: - '@id': overrideNavXButton - joystick: - gunnerGamepad - buttonNumber: 1 + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + '@id': overrideNavXButton + joystick: + gunnerGamepad + buttonNumber: 1 command: org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.commands.OverrideNavX: '@id': overrideNavXCommand @@ -449,7 +460,8 @@ buttons: drive action: WHEN_PRESSED - button: - overrideNavXButton + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + overrideNavXButton command: org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.commands.OverrideNavX: '@id': unoverrideNavXCommand @@ -459,10 +471,11 @@ buttons: drive action: WHEN_RELEASED - button: - '@id': jiggleRobotButton - joystick: - gunnerGamepad - buttonNumber: 8 + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + '@id': jiggleRobotButton + joystick: + gunnerGamepad + buttonNumber: 8 command: org.usfirst.frc.team449.robot.drive.talonCluster.commands.JiggleRobot: '@id': jiggleRobotCommand @@ -478,11 +491,12 @@ buttons: drive action: WHEN_PRESSED - button: - '@id': climbButton - joystick: - driverGamepad - triggerAxis: 2 - triggerAt: 0.9 + org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: + '@id': climbButton + joystick: + driverGamepad + triggerAxis: 2 + triggerAt: 0.9 command: org.usfirst.frc.team449.robot.mechanism.climber.commands.RunMotorWhileConditonMet: '@id': climbCommand @@ -491,7 +505,8 @@ buttons: climber action: WHEN_PRESSED - button: - climbButton + org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: + climbButton command: org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands.TurnMotorOffWithRequires: '@id': stopClimbCommand @@ -500,10 +515,11 @@ buttons: climber action: WHEN_RELEASED - button: - '@id': manualClimbButton - joystick: - gunnerGamepad - buttonNumber: 3 + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + '@id': manualClimbButton + joystick: + gunnerGamepad + buttonNumber: 3 command: org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands.TurnMotorOn: '@id': manualClimbCommand @@ -512,16 +528,18 @@ buttons: climber action: WHEN_PRESSED - button: - manualClimbButton + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + manualClimbButton command: org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands.TurnMotorOffWithRequires: stopClimbCommand action: WHEN_RELEASED - button: - '@id': switchCameraButton - joystick: - gunnerGamepad - buttonNumber: 9 + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + '@id': switchCameraButton + joystick: + gunnerGamepad + buttonNumber: 9 command: org.usfirst.frc.team449.robot.vision.commands.ChangeCam: '@id': switchCameraCommand @@ -530,18 +548,20 @@ buttons: cameras action: WHEN_PRESSED - button: - '@id': pushGearButton - joystick: - driverGamepad - triggerAxis: 3 - triggerAt: 0.9 + org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: + '@id': pushGearButton + joystick: + driverGamepad + triggerAxis: 3 + triggerAt: 0.9 command: org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidReverse: <<: *openGearHandler '@id': openGearHandlerCommand action: WHEN_PRESSED - button: - pushGearButton + org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: + pushGearButton command: org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidForward: '@id': closeGearHandlerCommand @@ -550,10 +570,11 @@ buttons: gearHandler action: WHEN_RELEASED # - button: -# '@id': fireShooterButton -# joystick: -# gunnerGamepad -# buttonNumber: 6 +# org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: +# '@id': fireShooterButton +# joystick: +# gunnerGamepad +# buttonNumber: 6 # command: # org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.FireShooter: # '@id': fireShooterCommand @@ -562,11 +583,12 @@ buttons: # shooter # action: WHEN_PRESSED # - button: -# '@id': rackShooterButton -# joystick: -# gunnerGamepad -# triggerAxis: 3 -# triggerAt: 0.9 +# org.usfirstfirst.frc.team449.robot.oi.buttons.TriggerButton: +# '@id': rackShooterButton +# joystick: +# gunnerGamepad +# triggerAxis: 3 +# triggerAt: 0.9 # command: # org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.RackShooter: # '@id': rackShooterCommand @@ -575,10 +597,11 @@ buttons: # shooter # action: WHEN_PRESSED # - button: -# '@id': resetShooterButton -# joystick: -# gunnerGamepad -# buttonNumber: 5 +# org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: +# '@id': resetShooterButton +# joystick: +# gunnerGamepad +# buttonNumber: 5 # command: # org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.ResetShooter: # '@id': resetShooterCommand From 17f4018dde55cea7ff7f0278bc566d8bdd8c9a26 Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Wed, 28 Jun 2017 00:34:10 -0400 Subject: [PATCH 4/4] Have TriggerButton take a throttle. --- .../robot/oi/buttons/FactoryButton.java | 4 +- .../oi/buttons/FactoryJoystickButton.java | 10 ++-- .../robot/oi/buttons/TriggerButton.java | 12 ++-- RoboRIO/src/main/resources/calcifer_map.yml | 57 +++++++++++-------- 4 files changed, 46 insertions(+), 37 deletions(-) diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryButton.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryButton.java index 206256a3..f4a57778 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryButton.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryButton.java @@ -6,6 +6,8 @@ import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.components.MappedJoystick; +import org.usfirst.frc.team449.robot.components.MappedSmoothedThrottle; +import org.usfirst.frc.team449.robot.components.MappedThrottle; /** * A factory for constructing a button.. @@ -42,7 +44,7 @@ public static FactoryButton constructButton(@NotNull @JsonProperty(required = tr @Nullable Double triggerAt, @Nullable Integer angle) { if (triggerAxis != null) { - return new TriggerButton(joystick, triggerAxis, triggerAt); + return new TriggerButton(new MappedThrottle(joystick, triggerAxis, false), triggerAt); } else if (angle != null) { return new dPadButton(joystick, angle); } else { diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryJoystickButton.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryJoystickButton.java index 2a798a46..45e07bd9 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryJoystickButton.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/FactoryJoystickButton.java @@ -23,19 +23,19 @@ public class FactoryJoystickButton extends FactoryButton { /** * The port of the button on the joystick. */ - private final int buttonNum; + private final int buttonNumber; /** * Default constructor. * * @param joystick The joystick the button is on. - * @param buttonNum The port of the button. Note that button numbers begin at 1, not 0. + * @param buttonNumber The port of the button. Note that button numbers begin at 1, not 0. */ @JsonCreator public FactoryJoystickButton(@NotNull @JsonProperty(required = true) MappedJoystick joystick, - @JsonProperty(required = true) int buttonNum) { + @JsonProperty(required = true) int buttonNumber) { this.joystick = joystick; - this.buttonNum = buttonNum; + this.buttonNumber = buttonNumber; } /** @@ -45,6 +45,6 @@ public FactoryJoystickButton(@NotNull @JsonProperty(required = true) MappedJoyst */ @Override public boolean get() { - return joystick.getRawButton(buttonNum); + return joystick.getRawButton(buttonNumber); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/TriggerButton.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/TriggerButton.java index 84e5741c..69557074 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/TriggerButton.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/buttons/TriggerButton.java @@ -1,11 +1,10 @@ package org.usfirst.frc.team449.robot.oi.buttons; +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.components.MappedJoystick; -import org.usfirst.frc.team449.robot.components.MappedSmoothedThrottle; import org.usfirst.frc.team449.robot.components.MappedThrottle; /** @@ -28,14 +27,13 @@ public class TriggerButton extends FactoryButton { /** * Argument-based constructor. * - * @param joystick The the joystick containing the throttle. - * @param axis The axis of the throttle. + * @param throttle The relevant throttle. * @param triggerAt The percentage pressed to trigger at, from (0, 1] */ - public TriggerButton(@NotNull @JsonProperty(required = true) MappedJoystick joystick, - @JsonProperty(required = true) int axis, + @JsonCreator + public TriggerButton(@NotNull @JsonProperty(required = true) MappedThrottle throttle, @JsonProperty(required = true) double triggerAt) { - throttle = new MappedSmoothedThrottle(joystick, axis, 0, 0, false); + this.throttle = throttle; this.triggerAt = triggerAt; } diff --git a/RoboRIO/src/main/resources/calcifer_map.yml b/RoboRIO/src/main/resources/calcifer_map.yml index 82bab412..1e10d4bf 100644 --- a/RoboRIO/src/main/resources/calcifer_map.yml +++ b/RoboRIO/src/main/resources/calcifer_map.yml @@ -419,7 +419,7 @@ nonMPAutoCommand: drive buttons: - button: - org.usfirst.frc.team449.robot.oi.buttons.dPadButton: + org.usfirst.frc.team449.robot.oi.buttons.dPadButton: '@id': switchToLowButton joystick: '@id': gunnerGamepad @@ -433,7 +433,7 @@ buttons: drive action: WHEN_PRESSED - button: - org.usfirst.frc.team449.robot.oi.buttons.dPadButton: + org.usfirst.frc.team449.robot.oi.buttons.dPadButton: '@id': switchToHighButton joystick: gunnerGamepad @@ -446,7 +446,7 @@ buttons: drive action: WHEN_PRESSED - button: - org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: '@id': overrideNavXButton joystick: gunnerGamepad @@ -460,7 +460,7 @@ buttons: drive action: WHEN_PRESSED - button: - org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: overrideNavXButton command: org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.commands.OverrideNavX: @@ -471,7 +471,7 @@ buttons: drive action: WHEN_RELEASED - button: - org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: '@id': jiggleRobotButton joystick: gunnerGamepad @@ -491,11 +491,14 @@ buttons: drive action: WHEN_PRESSED - button: - org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: + org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: '@id': climbButton - joystick: - driverGamepad - triggerAxis: 2 + throttle: + org.usfirst.frc.team449.robot.components.MappedThrottle: + '@id': climbButtonThrottle + stick: + driverGamepad + axis: 2 triggerAt: 0.9 command: org.usfirst.frc.team449.robot.mechanism.climber.commands.RunMotorWhileConditonMet: @@ -505,7 +508,7 @@ buttons: climber action: WHEN_PRESSED - button: - org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: + org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: climbButton command: org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands.TurnMotorOffWithRequires: @@ -515,7 +518,7 @@ buttons: climber action: WHEN_RELEASED - button: - org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: '@id': manualClimbButton joystick: gunnerGamepad @@ -528,14 +531,14 @@ buttons: climber action: WHEN_PRESSED - button: - org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: manualClimbButton command: org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands.TurnMotorOffWithRequires: stopClimbCommand action: WHEN_RELEASED - button: - org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: + org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: '@id': switchCameraButton joystick: gunnerGamepad @@ -548,11 +551,14 @@ buttons: cameras action: WHEN_PRESSED - button: - org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: + org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: '@id': pushGearButton - joystick: - driverGamepad - triggerAxis: 3 + throttle: + org.usfirst.frc.team449.robot.components.MappedThrottle: + '@id': pushGearThrottle + stick: + driverGamepad + axis: 3 triggerAt: 0.9 command: org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidReverse: @@ -560,7 +566,7 @@ buttons: '@id': openGearHandlerCommand action: WHEN_PRESSED - button: - org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: + org.usfirst.frc.team449.robot.oi.buttons.TriggerButton: pushGearButton command: org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidForward: @@ -570,7 +576,7 @@ buttons: gearHandler action: WHEN_RELEASED # - button: -# org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: +# org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: # '@id': fireShooterButton # joystick: # gunnerGamepad @@ -583,11 +589,14 @@ buttons: # shooter # action: WHEN_PRESSED # - button: -# org.usfirstfirst.frc.team449.robot.oi.buttons.TriggerButton: +# org.usfirstfirst.frc.team449.robot.oi.buttons.TriggerButton: # '@id': rackShooterButton -# joystick: -# gunnerGamepad -# triggerAxis: 3 +# throttle: +# '@id': rackShooterThrottle +# org.usfirst.frc.team449.robot.components.MappedThrottle: +# stick: +# gunnerGamepad +# axis: 3 # triggerAt: 0.9 # command: # org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.RackShooter: @@ -597,7 +606,7 @@ buttons: # shooter # action: WHEN_PRESSED # - button: -# org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: +# org.usfirst.frc.team449.robot.oi.buttons.FactoryJoystickButton: # '@id': resetShooterButton # joystick: # gunnerGamepad