From c5a4ae2cad9c53da4d9428c390c3af0cc869258a Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Tue, 27 Jun 2017 22:03:50 -0400 Subject: [PATCH 1/3] Allow any command for default drive command. --- .../org/usfirst/frc/team449/robot/Robot.java | 6 +- .../usfirst/frc/team449/robot/RobotMap.java | 60 +++++++------------ 2 files changed, 25 insertions(+), 41 deletions(-) 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 3940f9de..18218974 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 @@ -230,11 +230,11 @@ public void robotInit() { robotMap.getRightProfiles().get(allianceString + "_" + position)); //Set the autonomousCommand to be the correct command for the current position and alliance. if (position.equals("center")) { - autonomousCommand = robotMap.getCenterAuto().getCommand(); + autonomousCommand = robotMap.getCenterAuto(); } else if ((position.equals("right") && redAlliance) || (position.equals("left") && !redAlliance)) { - autonomousCommand = robotMap.getBoilerAuto().getCommand(); + autonomousCommand = robotMap.getBoilerAuto(); } else { - autonomousCommand = robotMap.getFeederAuto().getCommand(); + autonomousCommand = robotMap.getFeederAuto(); } } } else { 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 1a4a3d53..cbe771c4 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 @@ -35,7 +35,7 @@ public class RobotMap { private final TalonClusterDrive drive; @NotNull - private final UnidirectionalNavXDefaultDrive defaultDriveCommand; + private final Command defaultDriveCommand; @Nullable private final ClimberSubsystem climber; @@ -71,13 +71,13 @@ public class RobotMap { private final MappedDigitalInput locationDial; @Nullable - private final YamlCommand boilerAuto; + private final Command boilerAuto; @Nullable - private final YamlCommand centerAuto; + private final Command centerAuto; @Nullable - private final YamlCommand feederAuto; + private final Command feederAuto; @Nullable private final MotionProfileData leftTestProfile; @@ -116,37 +116,21 @@ public class RobotMap { * @param gearHandler The gear handler on this robot. Can be null. * @param RIOduinoPort The I2C port of the RIOduino plugged into this robot. Can be null. * @param allianceSwitch The switch for selecting which alliance we're on. Can be null if doMP is false or - * 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 centerAuto The command to run in autonomous on the center of the field. Can be null if doMP is - * false + * @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 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 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 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. @@ -157,7 +141,7 @@ public RobotMap(@NotNull @JsonProperty(required = true) List butt @NotNull @JsonProperty(required = true) ArcadeOIWithDPad arcadeOI, @NotNull @JsonProperty(required = true) Logger logger, @NotNull @JsonProperty(required = true) TalonClusterDrive drive, - @NotNull @JsonProperty(required = true) UnidirectionalNavXDefaultDrive defaultDriveCommand, + @NotNull @JsonProperty(required = true) YamlCommand defaultDriveCommand, @Nullable ClimberSubsystem climber, @Nullable SingleFlywheelShooter shooter, @Nullable CameraSubsystem camera, @@ -190,14 +174,14 @@ public RobotMap(@NotNull @JsonProperty(required = true) List butt this.allianceSwitch = allianceSwitch; this.dropGearSwitch = dropGearSwitch; this.locationDial = locationDial; - this.boilerAuto = boilerAuto; - this.centerAuto = centerAuto; - this.feederAuto = feederAuto; + this.boilerAuto = boilerAuto != null ? boilerAuto.getCommand() : null; + this.centerAuto = centerAuto != null ? centerAuto.getCommand() : null; + this.feederAuto = feederAuto != null ? feederAuto.getCommand() : null; this.leftTestProfile = leftTestProfile; this.rightTestProfile = rightTestProfile; this.leftProfiles = leftProfiles; this.rightProfiles = rightProfiles; - this.defaultDriveCommand = defaultDriveCommand; + this.defaultDriveCommand = defaultDriveCommand.getCommand(); if (nonMPAutoCommand != null) { this.nonMPAutoCommand = nonMPAutoCommand.getCommand(); } else { @@ -299,17 +283,17 @@ public Map getRightProfiles() { } @Nullable - public YamlCommand getBoilerAuto() { + public Command getBoilerAuto() { return boilerAuto; } @Nullable - public YamlCommand getCenterAuto() { + public Command getCenterAuto() { return centerAuto; } @Nullable - public YamlCommand getFeederAuto() { + public Command getFeederAuto() { return feederAuto; } @@ -319,7 +303,7 @@ public Command getNonMPAutoCommand() { } @NotNull - public UnidirectionalNavXDefaultDrive getDefaultDriveCommand() { + public Command getDefaultDriveCommand() { return defaultDriveCommand; } From 1e13119e213343c87935ea256d86c11e7e7e34ed Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Tue, 27 Jun 2017 22:45:47 -0400 Subject: [PATCH 2/3] Make all defaults 1 line if possible. --- .../components/MappedSmoothedThrottle.java | 13 +++++++++++ .../components/RotPerSecCANTalonSRX.java | 23 ++++--------------- .../ShiftingTalonClusterDrive.java | 6 +---- .../drive/talonCluster/TalonClusterDrive.java | 5 +--- .../UnidirectionalNavXDefaultDrive.java | 5 +--- .../NavX/commands/PIDAngleCommand.java | 2 +- .../commands/RunMotorWhileConditonMet.java | 1 - .../pneumatics/PneumaticsSubsystem.java | 1 - .../pneumatics/commands/StartCompressor.java | 1 - .../SingleFlywheelShooter.java | 1 - .../team449/robot/oi/ArcadeOIWithDPad.java | 17 ++++++++++---- .../robot/oi/buttons/FactoryButton.java | 1 + 12 files changed, 34 insertions(+), 42 deletions(-) diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedSmoothedThrottle.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedSmoothedThrottle.java index 39dea2e6..c190a128 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedSmoothedThrottle.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedSmoothedThrottle.java @@ -11,12 +11,25 @@ @JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public class MappedSmoothedThrottle extends MappedThrottle { + /** + * The value below which the joystick input is considered 0. + */ protected final double deadband; + /** + * The smoothing filter for the joystick input. + */ + @NotNull private final LinearDigitalFilter filter; + /** + * The input from the joystick. Declared outside of getValue to avoid garbage collection. + */ private double input; + /** + * The sign of the input from the joystick. Declared outside of getValue to avoid garbage collection. + */ private double sign; /** diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/RotPerSecCANTalonSRX.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/RotPerSecCANTalonSRX.java index 0510d17b..5bef5b39 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/RotPerSecCANTalonSRX.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/RotPerSecCANTalonSRX.java @@ -213,20 +213,12 @@ public RotPerSecCANTalonSRX(@JsonProperty(required = true) int port, //Set high/only gear nominal voltages this.highGearFwdNominalOutputVoltage = highGearFwdNominalOutputVoltage; //If no reverse nominal voltage is given, assume symmetry. - if (highGearRevNominalOutputVoltage == null) { - this.highGearRevNominalOutputVoltage = highGearFwdNominalOutputVoltage; - } else { - this.highGearRevNominalOutputVoltage = highGearRevNominalOutputVoltage; - } + this.highGearRevNominalOutputVoltage = highGearRevNominalOutputVoltage != null ? highGearRevNominalOutputVoltage : highGearFwdNominalOutputVoltage; //Set low gear nominal voltages this.lowGearFwdNominalOutputVoltage = lowGearFwdNominalOutputVoltage; //If no reverse nominal voltage is given, assume symmetry. - if (lowGearRevNominalOutputVoltage == null) { - this.lowGearRevNominalOutputVoltage = lowGearFwdNominalOutputVoltage; - } else { - this.lowGearRevNominalOutputVoltage = lowGearRevNominalOutputVoltage; - } + this.lowGearRevNominalOutputVoltage = lowGearRevNominalOutputVoltage != null ? lowGearRevNominalOutputVoltage : lowGearFwdNominalOutputVoltage; //Set to high gear by default. canTalon.configNominalOutputVoltage(this.highGearFwdNominalOutputVoltage, -this.highGearRevNominalOutputVoltage); @@ -269,17 +261,10 @@ public RotPerSecCANTalonSRX(@JsonProperty(required = true) int port, } //postEncoderGearing defaults to 1 - if (postEncoderGearing == null) { - this.postEncoderGearing = 1.; - } else { - this.postEncoderGearing = postEncoderGearing; - } + this.postEncoderGearing = postEncoderGearing != null ? postEncoderGearing : 1.; //Configure the maximum output voltage. If only forward voltage was given, use it for both forward and reverse. - if (revPeakOutputVoltage == null) { - revPeakOutputVoltage = fwdPeakOutputVoltage; - } - canTalon.configPeakOutputVoltage(fwdPeakOutputVoltage, -revPeakOutputVoltage); + canTalon.configPeakOutputVoltage(fwdPeakOutputVoltage, revPeakOutputVoltage != null ? -revPeakOutputVoltage : -fwdPeakOutputVoltage); //Set the current limit if it was given if (currentLimit != null) { diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/ShiftingTalonClusterDrive.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/ShiftingTalonClusterDrive.java index b56ed619..68d19c4d 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/ShiftingTalonClusterDrive.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/ShiftingTalonClusterDrive.java @@ -67,11 +67,7 @@ public ShiftingTalonClusterDrive(@NotNull @JsonProperty(required = true) RotPerS this.shifter = shifter; //Default to low - if (startingGear == null) { - this.startingGear = gear.LOW; - } else { - this.startingGear = startingGear; - } + this.startingGear = startingGear != null ? startingGear : gear.LOW; currentGear = this.startingGear; // Initialize shifting constants, assuming robot is stationary. diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/TalonClusterDrive.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/TalonClusterDrive.java index a95262bc..0cccc4f4 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/TalonClusterDrive.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/TalonClusterDrive.java @@ -75,10 +75,7 @@ public TalonClusterDrive(@NotNull @JsonProperty(required = true) RotPerSecCANTal @Nullable Double PIDScale) { super(); //Initialize stuff - if (PIDScale == null) { - PIDScale = 1.; - } - PID_SCALE = PIDScale; + this.PID_SCALE = PIDScale != null ? PIDScale : 1.; this.rightMaster = rightMaster; this.leftMaster = leftMaster; this.mpHandler = MPHandler; 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 e6216519..4e08e2fe 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 @@ -90,10 +90,7 @@ public UnidirectionalNavXDefaultDrive(@JsonProperty(required = true) double abso this.subsystem = subsystem; driveStraightLoopEntryTimer = new BufferTimer(loopEntryDelay); - if (maxAngularVelToEnterLoop == null) { - maxAngularVelToEnterLoop = 180.; - } - this.maxAngularVelToEnterLoop = maxAngularVelToEnterLoop; + this.maxAngularVelToEnterLoop = maxAngularVelToEnterLoop != null ? maxAngularVelToEnterLoop : 180; //Needs a requires because it's a default command. requires(this.subsystem); diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/PIDAngleCommand.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/PIDAngleCommand.java index c9f72037..a248da10 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/PIDAngleCommand.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/PIDAngleCommand.java @@ -89,7 +89,7 @@ public PIDAngleCommand(@JsonProperty(required = true) double absoluteTolerance, //This caps the output we can give. One way to set up closed-loop is to make P large and then use this to // prevent overshoot. if (maximumOutput != null) { - this.getPIDController().setOutputRange(-minimumOutput, maximumOutput); + this.getPIDController().setOutputRange(-maximumOutput, maximumOutput); } //Set a deadband around the setpoint, in degrees, within which don't move, to avoid "dancing" diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/RunMotorWhileConditonMet.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/RunMotorWhileConditonMet.java index 2e69b08a..832e0792 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/RunMotorWhileConditonMet.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/commands/RunMotorWhileConditonMet.java @@ -32,7 +32,6 @@ public class RunMotorWhileConditonMet Date: Tue, 27 Jun 2017 23:24:47 -0400 Subject: [PATCH 3/3] Fix joystick inversion. --- RoboRIO/src/main/resources/calcifer_map.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/RoboRIO/src/main/resources/calcifer_map.yml b/RoboRIO/src/main/resources/calcifer_map.yml index a37eff38..2bf24aae 100644 --- a/RoboRIO/src/main/resources/calcifer_map.yml +++ b/RoboRIO/src/main/resources/calcifer_map.yml @@ -1,5 +1,5 @@ --- -doMP: false +doMP: true testMP: false leftTestProfile: &leftTest @@ -83,12 +83,13 @@ arcadeOI: axis: 0 smoothingTimeConstantSecs: 0.04 deadband: 0.05 - inverted: true + inverted: false fwdThrottle: org.usfirst.frc.team449.robot.components.MappedSmoothedThrottle: <<: *rotThrottle '@id': fwdThrottle axis: 5 + inverted: true invertDPad: true dPadShift: 0.1 drive: @@ -156,7 +157,7 @@ drive: module: 15 forward: 2 reverse: 3 - startingGear: HIGH + startingGear: LOW defaultDriveCommand: org.usfirst.frc.team449.robot.drive.talonCluster.commands.ShiftingUnidirectionalNavXDefaultDrive: '@id': defaultDriveCommand