diff --git a/RoboRIO/build.gradle b/RoboRIO/build.gradle index 51ee6d8c..56eae012 100644 --- a/RoboRIO/build.gradle +++ b/RoboRIO/build.gradle @@ -4,7 +4,6 @@ plugins { id "eclipse" id "idea" id "jaci.openrio.gradle.GradleRIO" version "2017.1.5" - id "com.google.protobuf" version "0.8.1" } group 'org.usfirst.frc.team449.robot2017' @@ -21,56 +20,21 @@ repositories { } dependencies { - compile 'com.google.protobuf:protobuf-java:3.0.0' - compile 'io.grpc:grpc-stub:1.0.0-pre2' - compile 'io.grpc:grpc-protobuf:1.0.0-pre2' - compile 'com.github.blair-robot-project:449-central-repo:v4.0.0-WPILib2017' + // https://mvnrepository.com/artifact/com.fasterxml.jackson.core/jackson-databind + compile group: 'com.fasterxml.jackson.core', name: 'jackson-databind', version: '2.9.0.pr3' + // https://mvnrepository.com/artifact/com.fasterxml.jackson.core/jackson-core + compile group: 'com.fasterxml.jackson.core', name: 'jackson-core', version: '2.9.0.pr3' + // https://mvnrepository.com/artifact/com.fasterxml.jackson.core/jackson-annotations + compile group: 'com.fasterxml.jackson.core', name: 'jackson-annotations', version: '2.9.0.pr3' + compile group: 'com.fasterxml.jackson.dataformat', name: 'jackson-dataformat-yaml', version: '2.9.0.pr3' + //com.fasterxml.jackson.module:jackson-module-parameter-names:2.9.0.pr3 + compile group: 'com.fasterxml.jackson.module', name: 'jackson-module-parameter-names', version: '2.9.0.pr3' + compile 'org.jetbrains:annotations:13.0' compile wpilib() compile talonSrx() - compile fileTree(dir: "lib", include: "*.jar") + compile navx() } - - -/* Protobuf stuff */ - -protobuf { - generatedFilesBaseDir = "$projectDir/gen" - protoc { - // The artifact spec for the Protobuf Compiler - artifact = 'com.google.protobuf:protoc:3.0.0' - } - plugins { - // Optional: an artifact spec for a protoc plugin, with "grpc" as - // the identifier, which can be referred to in the "plugins" - // container of the "generateProtoTasks" closure. - grpc { - artifact = 'io.grpc:protoc-gen-grpc-java:1.0.0-pre2' - } - } - generateProtoTasks { - ofSourceSet('main')*.plugins { - // Apply the "grpc" plugin whose spec is defined above, without - // options. Note the braces cannot be omitted, otherwise the - // plugin will not be added. This is because of the implicit way - // NamedDomainObjectContainer binds the methods. - grpc {} - } - } -} - -/* Utils */ -clean { - delete protobuf.generatedFilesBaseDir -} - -idea { - module { - sourceDirs += file("${protobuf.generatedFilesBaseDir}/main/java") - } -} - - task convertFiles { doLast { fileTree("src/main/resources").matching { include "*.txt" }.each { aFile -> @@ -136,4 +100,8 @@ task genJavadoc(type: Jar, dependsOn: javadoc) { artifacts { archives genJavadoc +} + +compileJava { + options.compilerArgs << '-parameters' } \ No newline at end of file diff --git a/RoboRIO/lib/CTRLib-javadoc.jar b/RoboRIO/lib/CTRLib-javadoc.jar deleted file mode 100644 index a6835455..00000000 Binary files a/RoboRIO/lib/CTRLib-javadoc.jar and /dev/null differ diff --git a/RoboRIO/lib/CTRLib.jar b/RoboRIO/lib/CTRLib.jar deleted file mode 100644 index 84fb6973..00000000 Binary files a/RoboRIO/lib/CTRLib.jar and /dev/null differ diff --git a/RoboRIO/lib/annotations-java8.jar b/RoboRIO/lib/annotations-java8.jar new file mode 100644 index 00000000..ba946be4 Binary files /dev/null and b/RoboRIO/lib/annotations-java8.jar differ diff --git a/RoboRIO/lib/navx_frc.jar b/RoboRIO/lib/navx_frc.jar deleted file mode 100644 index 67b2ee0e..00000000 Binary files a/RoboRIO/lib/navx_frc.jar and /dev/null differ 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 e003b0ea..3940f9de 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 @@ -1,21 +1,21 @@ package org.usfirst.frc.team449.robot; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIdentityInfo; +import com.fasterxml.jackson.annotation.ObjectIdGenerators; +import com.fasterxml.jackson.dataformat.yaml.YAMLMapper; +import com.fasterxml.jackson.module.paramnames.ParameterNamesModule; import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; -import maps.org.usfirst.frc.team449.robot.Robot2017Map; -import maps.org.usfirst.frc.team449.robot.util.MotionProfileMap; -import org.usfirst.frc.team449.robot.autonomous.BoilerAuto2017; -import org.usfirst.frc.team449.robot.autonomous.CenterAuto2017; -import org.usfirst.frc.team449.robot.autonomous.FeederAuto2017; -import org.usfirst.frc.team449.robot.components.MappedDigitalInput; +import org.jetbrains.annotations.Contract; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; +import org.usfirst.frc.team449.robot.drive.talonCluster.ShiftingTalonClusterDrive; import org.usfirst.frc.team449.robot.drive.talonCluster.TalonClusterDrive; -import org.usfirst.frc.team449.robot.drive.talonCluster.commands.ShiftingUnidirectionalNavXArcadeDrive; -import org.usfirst.frc.team449.robot.interfaces.drive.shifting.ShiftingDrive; import org.usfirst.frc.team449.robot.interfaces.drive.shifting.commands.SwitchToGear; -import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.commands.PIDTest; import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.commands.RunLoadedProfile; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidForward; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidReverse; @@ -25,33 +25,28 @@ import org.usfirst.frc.team449.robot.mechanism.pneumatics.PneumaticsSubsystem; import org.usfirst.frc.team449.robot.mechanism.pneumatics.commands.StartCompressor; import org.usfirst.frc.team449.robot.mechanism.singleflywheelshooter.SingleFlywheelShooter; -import org.usfirst.frc.team449.robot.oi.OI2017ArcadeGamepad; -import org.usfirst.frc.team449.robot.util.Loggable; +import org.usfirst.frc.team449.robot.oi.ArcadeOIWithDPad; import org.usfirst.frc.team449.robot.util.Logger; -import org.usfirst.frc.team449.robot.util.MotionProfileData; import org.usfirst.frc.team449.robot.vision.CameraSubsystem; +import org.yaml.snakeyaml.Yaml; +import java.io.FileReader; import java.io.IOException; -import java.util.ArrayList; -import java.util.HashMap; import java.util.List; import java.util.Map; /** * The main class of the robot, constructs all the subsystems and initializes default commands. */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public class Robot extends IterativeRobot { /** * The absolute filepath to the resources folder containing the config files. */ + @NotNull public static final String RESOURCES_PATH = "/home/lvuser/449_resources/"; - /** - * The instance of this object that exists. This is a static field so that the subsystems don't have to be. - */ - public static Robot instance; - /** * The current time in milliseconds as it was stored the last time a method in robot was run. */ @@ -65,84 +60,72 @@ public class Robot extends IterativeRobot { /** * The shooter subsystem (flywheel and feeder) */ - public SingleFlywheelShooter singleFlywheelShooterSubsystem; + @Nullable + private SingleFlywheelShooter singleFlywheelShooterSubsystem; /** * The intake subsystem (intake motors and pistons) */ - public Intake2017 intakeSubsystem; + @Nullable + private Intake2017 intakeSubsystem; /** * The climber */ - public ClimberSubsystem climberSubsystem; + @Nullable + private ClimberSubsystem climberSubsystem; /** * The compressor and pressure sensor */ - public PneumaticsSubsystem pneumaticsSubsystem; + @Nullable + private PneumaticsSubsystem pneumaticsSubsystem; /** * The drive */ - public TalonClusterDrive driveSubsystem; + private TalonClusterDrive driveSubsystem; /** - * OI, using an Xbox-style controller and arcade drive. + * The OI containing the joysticks to get input from. */ - public OI2017ArcadeGamepad oiSubsystem; + private ArcadeOIWithDPad arcadeOI; /** * The cameras on the robot and the code to stream them to SmartDashboard (NOT computer vision!) */ - public CameraSubsystem cameraSubsystem; + @Nullable + private CameraSubsystem cameraSubsystem; /** * The active gear subsystem. */ - public ActiveGearSubsystem gearSubsystem; + @Nullable + private ActiveGearSubsystem gearSubsystem; /** - * The object constructed directly from map.cfg. + * The object constructed directly from the yaml map. */ - private Robot2017Map.Robot2017 cfg; + private RobotMap robotMap; /** * The Notifier running the logging thread. */ private Notifier loggerNotifier; - /** - * Whether or not we're on the red alliance, according to the dIO switch. - */ - private boolean redAlliance; - - /** - * Whether or not to drop the gear during autonomous, according to the dIO switch. - */ - private boolean dropGear; - - /** - * The position we're in, according to the dIO switch. - */ - private String position; - /** * The string version of the alliance we're on ("red" or "blue"). Used for string concatenation to pick which * profile to execute. */ + @Nullable private String allianceString; /** * The I2C channel for communicating with the RIOduino. */ + @Nullable private I2C robotInfo; - /** - * The gear to start both autonomous and teleop in. - */ - private ShiftingDrive.gear startingGear; - /** * The logger for the robot. */ @@ -158,6 +141,7 @@ public class Robot extends IterativeRobot { * * @return current time in milliseconds. */ + @Contract(pure = true) public static long currentTimeMillis() { return currentTimeMillis - startTime; } @@ -169,157 +153,94 @@ public void robotInit() { //Set up start time currentTimeMillis = System.currentTimeMillis(); startTime = currentTimeMillis; + //Yes this should be a print statement, it's useful to know that robotInit started. System.out.println("Started robotInit."); - //Set up instance. - instance = this; - + Yaml yaml = new Yaml(); try { - //Try to construct map from the cfg file -// cfg = (Robot2017Map.Robot2017) MappedSubsystem.readConfig(RESOURCES_PATH+"balbasaur_map.cfg", - cfg = (Robot2017Map.Robot2017) MappedSubsystem.readConfig(RESOURCES_PATH + "fancy_map.cfg", - Robot2017Map.Robot2017.newBuilder()); + //Read the yaml file with SnakeYaml so we can use anchors and merge syntax. +// Map normalized = (Map) yaml.load(new FileReader(RESOURCES_PATH+"ballbasaur_map.yml")); + Map normalized = (Map) yaml.load(new FileReader(RESOURCES_PATH + "calcifer_map.yml")); + YAMLMapper mapper = new YAMLMapper(); + //Turn the Map read by SnakeYaml into a String so Jackson can read it. + String fixed = mapper.writeValueAsString(normalized); + //Use a parameter name module so we don't have to specify name for every field. + mapper.registerModule(new ParameterNamesModule(JsonCreator.Mode.PROPERTIES)); + //Deserialize the map into an object. + robotMap = mapper.readValue(fixed, RobotMap.class); } catch (IOException e) { //This is either the map file not being in the file system OR it being improperly formatted. System.out.println("Config file is bad/nonexistent!"); e.printStackTrace(); } - - //Instantiate the list of loggable subsystems. - List loggables = new ArrayList<>(); - - //Set up starting gear from the map. - startingGear = cfg.getStartLowGear() ? ShiftingDrive.gear.LOW : ShiftingDrive.gear.HIGH; - - //Construct the OI (has to be done first because other subsystems take the OI as an argument.) - oiSubsystem = new OI2017ArcadeGamepad(cfg.getArcadeOi()); - Logger.addEvent("Constructed OI", this.getClass()); - - //Construct the drive (not in an "if null" block because you kind of need it.) - driveSubsystem = new TalonClusterDrive(cfg.getDrive(), oiSubsystem, startingGear); - Logger.addEvent("Constructed Drive", this.getClass()); - loggables.add(driveSubsystem); - - //Construct camera if it's in the map. - if (cfg.hasCamera()) { - cameraSubsystem = new CameraSubsystem(cfg.getCamera()); - } - - //Construct climber if it's in the map. - if (cfg.hasClimber()) { - climberSubsystem = new ClimberSubsystem(cfg.getClimber()); - loggables.add(climberSubsystem); - } - - //Construct shooter if it's in the map. - if (cfg.hasShooter()) { - singleFlywheelShooterSubsystem = new SingleFlywheelShooter(cfg.getShooter()); - loggables.add(singleFlywheelShooterSubsystem); - Logger.addEvent("Constructed SingleFlywheelShooter", this.getClass()); - } - - //Construct pneumatics if it's in the map. - if (cfg.hasPneumatics()) { - pneumaticsSubsystem = new PneumaticsSubsystem(cfg.getPneumatics()); - loggables.add(pneumaticsSubsystem); - Logger.addEvent("Constructed PneumaticsSubsystem", this.getClass()); - } - - //Construct intake if it's in the map. - if (cfg.hasIntake()) { - intakeSubsystem = new Intake2017(cfg.getIntake()); - } - - //Construct active gear if it's in the map. - if (cfg.hasGear()) { - gearSubsystem = new ActiveGearSubsystem(cfg.getGear()); - } - - //Map the buttons (has to be done last because all the subsystems need to have been instantiated.) - oiSubsystem.mapButtons(); - Logger.addEvent("Mapped buttons", this.getClass()); - - //Try to instantiate the logger. - try { - logger = new Logger(cfg.getLogger(), loggables); - //Set up the Notifier that runs the logging thread. - loggerNotifier = new Notifier(logger); - } catch (IOException e) { - System.out.println("Instantiating logger failed!"); - e.printStackTrace(); - } + //Set fields from the map. + this.logger = robotMap.getLogger(); + this.loggerNotifier = new Notifier(this.logger); + this.climberSubsystem = robotMap.getClimber(); + this.singleFlywheelShooterSubsystem = robotMap.getShooter(); + this.cameraSubsystem = robotMap.getCamera(); + this.intakeSubsystem = robotMap.getIntake(); + this.pneumaticsSubsystem = robotMap.getPneumatics(); + this.gearSubsystem = robotMap.getGearHandler(); + this.arcadeOI = robotMap.getArcadeOI(); + this.driveSubsystem = robotMap.getDrive(); //Set up RIOduino I2C channel if it's in the map. - if (cfg.hasRioduinoPort()) { - robotInfo = new I2C(I2C.Port.kOnboard, cfg.getRioduinoPort()); + if (robotMap.getRIOduinoPort() != null) { + robotInfo = new I2C(I2C.Port.kOnboard, robotMap.getRIOduinoPort()); } - //Read the dIO pins if they're in the map - if (cfg.hasBlueRed()) { - //Read from the pins - redAlliance = new MappedDigitalInput(cfg.getBlueRed()).getStatus().get(0); - dropGear = new MappedDigitalInput(cfg.getDropGear()).getStatus().get(0); - List tmp = new MappedDigitalInput(cfg.getLocation()).getStatus(); - - //Interpret the pin input from the three-way side selection switch. - if (!tmp.get(0) && !tmp.get(1)) { - position = "center"; - } else if (tmp.get(0)) { - position = "left"; - } else { - position = "right"; - } - - //Set up allianceString to use for concatenation. - if (redAlliance) { - allianceString = "red"; + //Set up the motion profiles if we're doing motion profiling + if (robotMap.getDoMP()) { + //Load the test profiles if we're testing. + if (robotMap.getTestMP()) { + driveSubsystem.loadMotionProfile(robotMap.getLeftTestProfile(), robotMap.getRightTestProfile()); + autonomousCommand = new RunLoadedProfile<>(driveSubsystem, 15, true); } else { - allianceString = "blue"; - } - } - - //Log the data read from the switches - Logger.addEvent("redAlliance: " + redAlliance, this.getClass()); - Logger.addEvent("dropGear: " + dropGear, this.getClass()); - Logger.addEvent("position: " + position, this.getClass()); + //Read the data from the input switches + boolean redAlliance = robotMap.getAllianceSwitch().getStatus().get(0); + boolean dropGear = robotMap.getDropGearSwitch().getStatus().get(0); + List tmp = robotMap.getLocationDial().getStatus(); + + String position; + //Interpret the pin input from the three-way side selection switch. + if (!tmp.get(0) && !tmp.get(1)) { + position = "center"; + } else if (tmp.get(0)) { + position = "left"; + } else { + position = "right"; + } - //Instantiate the profile maps. - Map rightProfiles = new HashMap<>(), leftProfiles = new HashMap<>(); + //Set up the alliance strings for easily selecting profiles. + if (redAlliance) { + allianceString = "red"; + } else { + allianceString = "blue"; + } - //Fill the profile data with the profiles. This part takes a little while because we have to read all the files. - for (MotionProfileMap.MotionProfile profile : cfg.getLeftMotionProfileList()) { - leftProfiles.put(profile.getName(), new MotionProfileData(profile)); - } - for (MotionProfileMap.MotionProfile profile : cfg.getRightMotionProfileList()) { - rightProfiles.put(profile.getName(), new MotionProfileData(profile)); - } + //Log the data read from the switches + Logger.addEvent("redAlliance: " + redAlliance, this.getClass()); + Logger.addEvent("dropGear: " + dropGear, this.getClass()); + Logger.addEvent("position: " + position, this.getClass()); - //Set up the motion profiles if we're doing motion profiling - if (cfg.getDoMP()) { - //Load the test profiles if we just want to run one. - if (cfg.getTestMP()) { - driveSubsystem.loadMotionProfile(leftProfiles.get("test"), rightProfiles.get("test")); - autonomousCommand = new RunLoadedProfile(driveSubsystem, 15, true); - } else { //Load the first profile we want to run - driveSubsystem.loadMotionProfile(leftProfiles.get(allianceString + "_" + position), rightProfiles.get(allianceString + "_" + position)); + driveSubsystem.loadMotionProfile(robotMap.getLeftProfiles().get(allianceString + "_" + position), + robotMap.getRightProfiles().get(allianceString + "_" + position)); //Set the autonomousCommand to be the correct command for the current position and alliance. if (position.equals("center")) { - autonomousCommand = new CenterAuto2017(driveSubsystem, gearSubsystem, dropGear, cfg.getDriveBackTime()); + autonomousCommand = robotMap.getCenterAuto().getCommand(); } else if ((position.equals("right") && redAlliance) || (position.equals("left") && !redAlliance)) { - autonomousCommand = new BoilerAuto2017(driveSubsystem, gearSubsystem, dropGear, - leftProfiles.get(allianceString + "_shoot"), rightProfiles.get(allianceString + "_shoot"), - singleFlywheelShooterSubsystem); + autonomousCommand = robotMap.getBoilerAuto().getCommand(); } else { - autonomousCommand = new FeederAuto2017(driveSubsystem, gearSubsystem, dropGear, - leftProfiles.get(allianceString + "_backup"), rightProfiles.get(allianceString + "_backup"), - leftProfiles.get("forward")); + autonomousCommand = robotMap.getFeederAuto().getCommand(); } } } else { - autonomousCommand = new PIDTest(driveSubsystem, cfg.getDriveBackTime()); + autonomousCommand = robotMap.getNonMPAutoCommand(); } + //Run the logger to write all the events that happened during initialization to a file. logger.run(); } @@ -332,14 +253,12 @@ public void teleopInit() { //Stop the drive for safety reasons driveSubsystem.stopMPProcesses(); driveSubsystem.fullStop(); - //Set up whether to override the NavX based on whether it's overriden by default. - driveSubsystem.setOverrideNavX(!cfg.getArcadeOi().getOverrideNavXWhileHeld()); //Enable the motors in case they got disabled somehow driveSubsystem.enableMotors(); //Set the default command - driveSubsystem.setDefaultCommandManual(new ShiftingUnidirectionalNavXArcadeDrive(driveSubsystem.straightPID, driveSubsystem, oiSubsystem)); + driveSubsystem.setDefaultCommandManual(robotMap.getDefaultDriveCommand()); //Do the startup tasks doStartupTasks(); @@ -425,11 +344,13 @@ private void sendModeOverI2C(I2C i2C, String mode) { */ private void doStartupTasks() { //Start running the logger - loggerNotifier.startPeriodic(cfg.getLogger().getLoopTimeSecs()); + loggerNotifier.startPeriodic(robotMap.getLogger().getLoopTimeSecs()); //Refresh the current time. currentTimeMillis = System.currentTimeMillis(); //Switch to starting gear - Scheduler.getInstance().add(new SwitchToGear(driveSubsystem, startingGear)); + if (driveSubsystem.getClass().equals(ShiftingTalonClusterDrive.class)) { + Scheduler.getInstance().add(new SwitchToGear((ShiftingTalonClusterDrive) driveSubsystem, ((ShiftingTalonClusterDrive) driveSubsystem).getStartingGear())); + } //Start the compressor if it exists if (pneumaticsSubsystem != null) { 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 new file mode 100644 index 00000000..1a4a3d53 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/RobotMap.java @@ -0,0 +1,335 @@ +package org.usfirst.frc.team449.robot; + +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.wpilibj.command.Command; +import org.jetbrains.annotations.NotNull; +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; +import org.usfirst.frc.team449.robot.mechanism.pneumatics.PneumaticsSubsystem; +import org.usfirst.frc.team449.robot.mechanism.singleflywheelshooter.SingleFlywheelShooter; +import org.usfirst.frc.team449.robot.oi.ArcadeOIWithDPad; +import org.usfirst.frc.team449.robot.oi.buttons.CommandButton; +import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.MotionProfileData; +import org.usfirst.frc.team449.robot.util.YamlCommand; +import org.usfirst.frc.team449.robot.vision.CameraSubsystem; + +import java.util.List; +import java.util.Map; + +/** + * The Jackson-compatible object representing the entire robot. + */ +public class RobotMap { + + @NotNull + private final ArcadeOIWithDPad arcadeOI; + + @NotNull + private final TalonClusterDrive drive; + + @NotNull + private final UnidirectionalNavXDefaultDrive defaultDriveCommand; + + @Nullable + private final ClimberSubsystem climber; + + @Nullable + private final SingleFlywheelShooter shooter; + + @Nullable + private final CameraSubsystem camera; + + @Nullable + private final Intake2017 intake; + + @Nullable + private final PneumaticsSubsystem pneumatics; + + @Nullable + private final ActiveGearSubsystem gearHandler; + + @NotNull + private final Logger logger; + + @Nullable + private final Integer RIOduinoPort; + + @Nullable + private final MappedDigitalInput allianceSwitch; + + @Nullable + private final MappedDigitalInput dropGearSwitch; + + @Nullable + private final MappedDigitalInput locationDial; + + @Nullable + private final YamlCommand boilerAuto; + + @Nullable + private final YamlCommand centerAuto; + + @Nullable + private final YamlCommand feederAuto; + + @Nullable + private final MotionProfileData leftTestProfile; + + @Nullable + private final MotionProfileData rightTestProfile; + + @Nullable + private final Map leftProfiles; + + @Nullable + private final Map rightProfiles; + + @Nullable + private final Command nonMPAutoCommand; + + private final boolean testMP; + + private final boolean doMP; + + @NotNull + private final List buttons; + + /** + * Default constructor. + * + * @param buttons The buttons for controlling this robot. + * @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 climber The climber for boarding the airship. Can be null. + * @param shooter The shooter for shooting fuel. Can be null. + * @param camera The cameras on this robot. Can be null. + * @param intake The intake for picking up and agitating balls. Can be null. + * @param pneumatics The pneumatics on this robot. Can be null. + * @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 + * 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 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. + * @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. + * @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. + * @param doMP Whether to run a motion profile during autonomous. Defaults to true. + */ + @JsonCreator + public RobotMap(@NotNull @JsonProperty(required = true) List buttons, + @NotNull @JsonProperty(required = true) ArcadeOIWithDPad arcadeOI, + @NotNull @JsonProperty(required = true) Logger logger, + @NotNull @JsonProperty(required = true) TalonClusterDrive drive, + @NotNull @JsonProperty(required = true) UnidirectionalNavXDefaultDrive defaultDriveCommand, + @Nullable ClimberSubsystem climber, + @Nullable SingleFlywheelShooter shooter, + @Nullable CameraSubsystem camera, + @Nullable Intake2017 intake, + @Nullable PneumaticsSubsystem pneumatics, + @Nullable ActiveGearSubsystem gearHandler, + @Nullable Integer RIOduinoPort, + @Nullable MappedDigitalInput allianceSwitch, + @Nullable MappedDigitalInput dropGearSwitch, + @Nullable MappedDigitalInput locationDial, + @Nullable YamlCommand boilerAuto, + @Nullable YamlCommand centerAuto, + @Nullable YamlCommand feederAuto, + @Nullable MotionProfileData leftTestProfile, @Nullable MotionProfileData rightTestProfile, + @Nullable Map leftProfiles, @Nullable Map rightProfiles, + @Nullable YamlCommand nonMPAutoCommand, + boolean testMP, + @Nullable Boolean doMP) { + this.buttons = buttons; + this.arcadeOI = arcadeOI; + this.drive = drive; + this.climber = climber; + this.shooter = shooter; + this.camera = camera; + this.intake = intake; + this.pneumatics = pneumatics; + this.gearHandler = gearHandler; + this.logger = logger; + this.RIOduinoPort = RIOduinoPort; + this.allianceSwitch = allianceSwitch; + this.dropGearSwitch = dropGearSwitch; + this.locationDial = locationDial; + this.boilerAuto = boilerAuto; + this.centerAuto = centerAuto; + this.feederAuto = feederAuto; + this.leftTestProfile = leftTestProfile; + this.rightTestProfile = rightTestProfile; + this.leftProfiles = leftProfiles; + this.rightProfiles = rightProfiles; + this.defaultDriveCommand = defaultDriveCommand; + if (nonMPAutoCommand != null) { + this.nonMPAutoCommand = nonMPAutoCommand.getCommand(); + } else { + this.nonMPAutoCommand = null; + } + this.testMP = testMP; + if (doMP == null) { + doMP = true; + } + this.doMP = doMP; + } + + @NotNull + public TalonClusterDrive getDrive() { + return drive; + } + + @Nullable + public ClimberSubsystem getClimber() { + return climber; + } + + @Nullable + public SingleFlywheelShooter getShooter() { + return shooter; + } + + @Nullable + public CameraSubsystem getCamera() { + return camera; + } + + @Nullable + public Intake2017 getIntake() { + return intake; + } + + @Nullable + public PneumaticsSubsystem getPneumatics() { + return pneumatics; + } + + @Nullable + public ActiveGearSubsystem getGearHandler() { + return gearHandler; + } + + @NotNull + public Logger getLogger() { + return logger; + } + + @Nullable + public Integer getRIOduinoPort() { + return RIOduinoPort; + } + + @Nullable + public MappedDigitalInput getAllianceSwitch() { + return allianceSwitch; + } + + @Nullable + public MappedDigitalInput getDropGearSwitch() { + return dropGearSwitch; + } + + @Nullable + public MappedDigitalInput getLocationDial() { + return locationDial; + } + + @Nullable + public MotionProfileData getLeftTestProfile() { + return leftTestProfile; + } + + @Nullable + public MotionProfileData getRightTestProfile() { + return rightTestProfile; + } + + public boolean getTestMP() { + return testMP; + } + + public boolean getDoMP() { + return doMP; + } + + @Nullable + public Map getLeftProfiles() { + return leftProfiles; + } + + @Nullable + public Map getRightProfiles() { + return rightProfiles; + } + + @Nullable + public YamlCommand getBoilerAuto() { + return boilerAuto; + } + + @Nullable + public YamlCommand getCenterAuto() { + return centerAuto; + } + + @Nullable + public YamlCommand getFeederAuto() { + return feederAuto; + } + + @Nullable + public Command getNonMPAutoCommand() { + return nonMPAutoCommand; + } + + @NotNull + public UnidirectionalNavXDefaultDrive getDefaultDriveCommand() { + return defaultDriveCommand; + } + + @NotNull + public ArcadeOIWithDPad getArcadeOI() { + return arcadeOI; + } + + @NotNull + public List getButtons() { + return buttons; + } +} diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/BoilerAuto2017.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/BoilerAuto2017.java index 9181bb08..cf0098ed 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/BoilerAuto2017.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/BoilerAuto2017.java @@ -1,45 +1,62 @@ package org.usfirst.frc.team449.robot.autonomous; -import edu.wpi.first.wpilibj.command.CommandGroup; -import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; -import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem.TwoSideMPSubsystem; +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.jetbrains.annotations.Nullable; +import org.usfirst.frc.team449.robot.components.MappedDigitalInput; import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem.commands.RunProfileTwoSides; import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.commands.RunLoadedProfile; -import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.ShooterSubsystem; -import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands.SpinUpShooter; -import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands.TurnAllOn; -import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidReverse; -import org.usfirst.frc.team449.robot.mechanism.activegear.ActiveGearSubsystem; -import org.usfirst.frc.team449.robot.util.MotionProfileData; +import org.usfirst.frc.team449.robot.util.YamlCommand; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; /** * The autonomous routine to deliver a gear to the center gear. */ -public class BoilerAuto2017 extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class BoilerAuto2017 extends YamlCommandGroupWrapper { /** * Default constructor. * - * @param drive The drive subsystem to execute this command on. Must also be a {@link - * UnidirectionalDrive}, and - * needs to have the profile to drive up to the peg already loaded into it. - * @param gearHandler The gear handler to execute this command on. - * @param dropGear Whether or not to drop the gear. - * @param leftPegToKeyProfile The motion profile for the left side of the drive to execute to get from the peg to - * the key. - * @param rightPegToKeyProfile The motion profile for the right side of the drive to execute to get from the peg to - * the key. - * @param shooter The shooter subsystem to execute this command on. + * @param runWallToPegProfile The command for running the profile for going from the wall to the peg, which has + * already been loaded. + * @param dropGear The command for dropping the held gear. + * @param dropGearSwitch The switch deciding whether or not to drop the gear. + * @param allianceSwitch The switch indicating which alliance we're on. + * @param runRedPegToKeyProfile The command for moving from the peg to the key, on the red side of the field. + * @param runBluePegToKeyProfile The command for moving from the peg to the key, on the blue side of the field. + * @param spinUpShooter The command for revving up the shooter. Can be null. + * @param fireShooter The command for firing the shooter. Can be null. */ - public BoilerAuto2017(TwoSideMPSubsystem drive, ActiveGearSubsystem gearHandler, boolean dropGear, - MotionProfileData leftPegToKeyProfile, MotionProfileData rightPegToKeyProfile, - ShooterSubsystem shooter) { - addParallel(new SpinUpShooter(shooter)); - addSequential(new RunLoadedProfile(drive, 15, true)); - if (dropGear) { - addSequential(new SolenoidReverse(gearHandler)); + @JsonCreator + public BoilerAuto2017(@NotNull @JsonProperty(required = true) RunLoadedProfile runWallToPegProfile, + @NotNull @JsonProperty(required = true) YamlCommand dropGear, + @NotNull @JsonProperty(required = true) MappedDigitalInput dropGearSwitch, + @NotNull @JsonProperty(required = true) MappedDigitalInput allianceSwitch, + @NotNull @JsonProperty(required = true) RunProfileTwoSides runRedPegToKeyProfile, + @NotNull @JsonProperty(required = true) RunProfileTwoSides runBluePegToKeyProfile, + @Nullable YamlCommand spinUpShooter, + @Nullable YamlCommand fireShooter) { + if (spinUpShooter != null) { + addParallel(spinUpShooter.getCommand()); + } + addSequential(runWallToPegProfile); + if (dropGearSwitch.getStatus().get(0)) { + addSequential(dropGear.getCommand()); + } + + //Red is true, blue is false + if (allianceSwitch.getStatus().get(0)) { + addSequential(runRedPegToKeyProfile); + } else { + addSequential(runBluePegToKeyProfile); + } + + if (fireShooter != null) { + addSequential(fireShooter.getCommand()); } - addSequential(new RunProfileTwoSides(drive, leftPegToKeyProfile, rightPegToKeyProfile, 10)); - addSequential(new TurnAllOn(shooter)); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/CenterAuto2017.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/CenterAuto2017.java index 7218eb85..dad37617 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/CenterAuto2017.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/CenterAuto2017.java @@ -1,33 +1,40 @@ package org.usfirst.frc.team449.robot.autonomous; -import edu.wpi.first.wpilibj.command.CommandGroup; -import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; -import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.commands.DriveAtSpeed; -import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem.TwoSideMPSubsystem; +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.MappedDigitalInput; import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.commands.RunLoadedProfile; -import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidReverse; -import org.usfirst.frc.team449.robot.mechanism.activegear.ActiveGearSubsystem; +import org.usfirst.frc.team449.robot.util.YamlCommand; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; /** * The autonomous routine to deliver a gear to the center gear. */ -public class CenterAuto2017 extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class CenterAuto2017 extends YamlCommandGroupWrapper { /** * Default constructor. * - * @param drive The drive subsystem to execute this command on. Must also be a {@link UnidirectionalDrive}, - * and - * needs to have the profile to drive up to the peg already loaded into it. - * @param gearHandler The gear handler to execute this command on. - * @param dropGear Whether or not to drop the gear. - * @param driveBackTime How long, in seconds, to drive back from the peg for. + * @param runWallToPegProfile The command for running the profile for going from the wall to the peg, which has + * already been loaded. + * @param dropGear The command for dropping the held gear. + * @param dropGearSwitch The switch deciding whether or not to drop the gear. + * @param driveBack The command for backing up away from the peg. */ - public CenterAuto2017(TwoSideMPSubsystem drive, ActiveGearSubsystem gearHandler, boolean dropGear, double driveBackTime) { - addSequential(new RunLoadedProfile(drive, 15, true)); - if (dropGear) { - addSequential(new SolenoidReverse(gearHandler)); + @JsonCreator + public CenterAuto2017( + @NotNull @JsonProperty(required = true) RunLoadedProfile runWallToPegProfile, + @NotNull @JsonProperty(required = true) YamlCommand dropGear, + @NotNull @JsonProperty(required = true) MappedDigitalInput dropGearSwitch, + @NotNull @JsonProperty(required = true) YamlCommand driveBack) { + addSequential(runWallToPegProfile); + if (dropGearSwitch.getStatus().get(0)) { + addSequential(dropGear.getCommand()); } - addSequential(new DriveAtSpeed((UnidirectionalDrive) drive, -0.3, driveBackTime)); + addSequential(driveBack.getCommand()); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/FeederAuto2017.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/FeederAuto2017.java index 4bc17456..1e48c51f 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/FeederAuto2017.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/autonomous/FeederAuto2017.java @@ -1,40 +1,52 @@ package org.usfirst.frc.team449.robot.autonomous; -import edu.wpi.first.wpilibj.command.CommandGroup; -import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; -import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem.TwoSideMPSubsystem; +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.MappedDigitalInput; import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem.commands.RunProfileTwoSides; import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.commands.RunLoadedProfile; -import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.commands.RunProfile; -import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidReverse; -import org.usfirst.frc.team449.robot.mechanism.activegear.ActiveGearSubsystem; -import org.usfirst.frc.team449.robot.util.MotionProfileData; +import org.usfirst.frc.team449.robot.util.YamlCommand; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; /** * The autonomous routine to deliver a gear to the center gear. */ -public class FeederAuto2017 extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class FeederAuto2017 extends YamlCommandGroupWrapper { /** * Default constructor. * - * @param drive The drive subsystem to execute this command on. Must also be a {@link - * UnidirectionalDrive}, and - * needs to have the profile to drive up to the peg already loaded into it. - * @param gearHandler The gear handler to execute this command on. - * @param dropGear Whether or not to drop the gear. - * @param leftBackupProfile The motion profile for the left side of the drive to execute to back up from the peg. - * @param rightBackupProfile The motion profile for the right side of the drive to execute to back up from the peg. - * @param forwardsProfile The motion profile for both sides to drive forwards after backing up from the peg. + * @param runWallToPegProfile The command for running the profile for going from the wall to the peg, which has + * already been loaded. + * @param dropGear The command for dropping the held gear. + * @param dropGearSwitch The switch deciding whether or not to drop the gear. + * @param allianceSwitch The switch indicating which alliance we're on. + * @param runRedBackupProfile The command for away from the peg, on the red side of the field. + * @param runBlueBackupProfile The command for moving away from the peg, on the blue side of the field. + * @param driveForwards The command for moving forwards towards the feeder station.. */ - public FeederAuto2017(TwoSideMPSubsystem drive, ActiveGearSubsystem gearHandler, boolean dropGear, - MotionProfileData leftBackupProfile, MotionProfileData rightBackupProfile, - MotionProfileData forwardsProfile) { - addSequential(new RunLoadedProfile(drive, 15, true)); - if (dropGear) { - addSequential(new SolenoidReverse(gearHandler)); + @JsonCreator + public FeederAuto2017(@NotNull @JsonProperty(required = true) RunLoadedProfile runWallToPegProfile, + @NotNull @JsonProperty(required = true) YamlCommand dropGear, + @NotNull @JsonProperty(required = true) MappedDigitalInput dropGearSwitch, + @NotNull @JsonProperty(required = true) MappedDigitalInput allianceSwitch, + @NotNull @JsonProperty(required = true) RunProfileTwoSides runRedBackupProfile, + @NotNull @JsonProperty(required = true) RunProfileTwoSides runBlueBackupProfile, + @NotNull @JsonProperty(required = true) YamlCommand driveForwards) { + addSequential(runWallToPegProfile); + if (dropGearSwitch.getStatus().get(0)) { + addSequential(dropGear.getCommand()); } - addSequential(new RunProfileTwoSides(drive, leftBackupProfile, rightBackupProfile, 10)); - addSequential(new RunProfile(drive, forwardsProfile, 5)); + //Red is true + if (allianceSwitch.getStatus().get(0)) { + addSequential(runRedBackupProfile); + } else { + addSequential(runBlueBackupProfile); + } + addSequential(driveForwards.getCommand()); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedAHRS.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedAHRS.java new file mode 100644 index 00000000..30ffd288 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedAHRS.java @@ -0,0 +1,25 @@ +package org.usfirst.frc.team449.robot.components; + +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.kauailabs.navx.frc.AHRS; +import edu.wpi.first.wpilibj.SPI; + +/** + * A Jackson-compatible wrapper for the NavX. + */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class MappedAHRS extends AHRS { + + /** + * Default constructor. + * + * @param port The port the NavX is plugged into. It seems like only kMXP (the port on the RIO) works. + */ + @JsonCreator + public MappedAHRS(@JsonProperty(required = true) SPI.Port port) { + super(port); + } +} diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedDigitalInput.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedDigitalInput.java index 6b47cfac..539c4ccb 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedDigitalInput.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedDigitalInput.java @@ -1,7 +1,8 @@ package org.usfirst.frc.team449.robot.components; +import com.fasterxml.jackson.annotation.*; import edu.wpi.first.wpilibj.DigitalInput; -import maps.org.usfirst.frc.team449.robot.components.DigitalInputMap; +import org.jetbrains.annotations.NotNull; import java.util.ArrayList; import java.util.List; @@ -9,21 +10,24 @@ /** * A series of roboRIO digital input pins. */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public class MappedDigitalInput { /** * The digitalInputs this class represents */ - private List digitalInputs; + @JsonIgnore + private final List digitalInputs; /** * Construct a MappedDigitalInput. * - * @param map the map to construct this from. + * @param ports The ports to read from, in order. */ - public MappedDigitalInput(DigitalInputMap.DigitalInput map) { + @JsonCreator + public MappedDigitalInput(@NotNull @JsonProperty(required = true) int[] ports) { digitalInputs = new ArrayList<>(); - for (int portNum : map.getPortList()) { + for (int portNum : ports) { DigitalInput tmp = new DigitalInput(portNum); digitalInputs.add(tmp); } @@ -34,9 +38,12 @@ public MappedDigitalInput(DigitalInputMap.DigitalInput map) { * * @return A list of booleans where 1 represents the input receiving a signal and 0 represents no signal. */ + @JsonIgnore + @NotNull public List getStatus() { List digitalValues = new ArrayList<>(); for (DigitalInput digitalInput : digitalInputs) { + //Negated because, by default, false means signal and true means no signal, and that's dumb. digitalValues.add(!digitalInput.get()); } return digitalValues; diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedDoubleSolenoid.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedDoubleSolenoid.java index e620b750..32eaf857 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedDoubleSolenoid.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedDoubleSolenoid.java @@ -1,19 +1,28 @@ package org.usfirst.frc.team449.robot.components; +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.DoubleSolenoid; -import maps.org.usfirst.frc.team449.robot.components.ModuleDoubleSolenoidMap; /** - * A wrapper on the {@link DoubleSolenoid} that can be constructed from a map object. + * A Jackson-compatible wrapper on the {@link DoubleSolenoid}. */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public class MappedDoubleSolenoid extends DoubleSolenoid { /** * Default constructor. * - * @param map A map containing module number, forward port, and reverse port. + * @param module The module number of the PCM. Defaults to 0. + * @param forward The forward port on the PCM. + * @param reverse The reverse port on the PCM. */ - public MappedDoubleSolenoid(ModuleDoubleSolenoidMap.ModuleDoubleSolenoid map) { - super(map.getModule(), map.getForward(), map.getReverse()); + @JsonCreator + public MappedDoubleSolenoid(int module, + @JsonProperty(required = true) int forward, + @JsonProperty(required = true) int reverse) { + super(module, forward, reverse); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedExpThrottle.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedExpThrottle.java new file mode 100644 index 00000000..ac3f3d63 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedExpThrottle.java @@ -0,0 +1,69 @@ +package org.usfirst.frc.team449.robot.components; + +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; + +/** + * An exponentially-scaled throttle. + */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class MappedExpThrottle extends MappedSmoothedThrottle { + + /** + * The base that is raised to the power of the joystick input. + */ + protected final double base; + + /** + * 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; + + /** + * A basic constructor. + * + * @param stick The Joystick object being used + * @param axis The axis being used. + * @param smoothingTimeConstantSecs How many seconds of past input strongly effect the smoothing algorithm. + * @param deadband The deadband below which the input will be read as 0, on [0, 1]. Defaults to 0. + * @param inverted Whether or not to invert the joystick input. Defaults to false. + * @param base The base that is raised to the power of the joystick input. + */ + @JsonCreator + public MappedExpThrottle(@NotNull @JsonProperty(required = true) MappedJoystick stick, + @JsonProperty(required = true) int axis, + double smoothingTimeConstantSecs, + double deadband, + boolean inverted, + @JsonProperty(required = true) double base) { + super(stick, axis, smoothingTimeConstantSecs, deadband, inverted); + this.base = base; + } + + /** + * Raises the base to the value of the smoothed joystick output, adjusting for sign. + * + * @return The processed value of the joystick + */ + @Override + public double getValue() { + input = super.getValue(); + + //Extract the sign + sign = Math.signum(input); + input = Math.abs(input); + + //Exponentially scale + input = (Math.pow(base, input) - 1.) / (base - 1.); + + return sign * input; + } +} \ No newline at end of file diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedJoystick.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedJoystick.java new file mode 100644 index 00000000..6844d708 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedJoystick.java @@ -0,0 +1,24 @@ +package org.usfirst.frc.team449.robot.components; + +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; + +/** + * A Jackson-compatible wrapper on a {@link Joystick}. + */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class MappedJoystick extends Joystick { + + /** + * Default constructor + * + * @param port The USB port of this joystick, on [0, 5]. + */ + @JsonCreator + public MappedJoystick(@JsonProperty(required = true) int port) { + super(port); + } +} diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedPolyThrottle.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedPolyThrottle.java new file mode 100644 index 00000000..150eda9a --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedPolyThrottle.java @@ -0,0 +1,64 @@ +package org.usfirst.frc.team449.robot.components; + +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.util.Polynomial; + +/** + * A polynomially scaled throttle. + */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class MappedPolyThrottle extends MappedSmoothedThrottle { + + /** + * The polynomially that scales the throttle. + */ + @NotNull + protected final Polynomial polynomial; + + /** + * A basic constructor. + * + * @param stick The Joystick object being used + * @param axis The axis being used. 0 is X, 1 is Y, 2 is Z. + * @param smoothingTimeConstantSecs How many seconds of past input strongly effect the smoothing algorithm. + * @param deadband The deadband below which the input will be read as 0, on [0, 1]. Defaults to 0. + * @param inverted Whether or not to invert the joystick input. Defaults to false. + * @param polynomial The polynomially that scales the throttle. Must not have any negative + * exponents. + */ + @JsonCreator + public MappedPolyThrottle(@NotNull @JsonProperty(required = true) MappedJoystick stick, + @JsonProperty(required = true) int axis, + double smoothingTimeConstantSecs, + double deadband, + boolean inverted, + @NotNull @JsonProperty(required = true) Polynomial polynomial) { + super(stick, axis, smoothingTimeConstantSecs, deadband, inverted); + + //Check for negative exponents + for (Double power : polynomial.getPowerToCoefficientMap().keySet()) { + if (power < 0) { + throw new IllegalArgumentException("Negative exponents are not allowed!"); + } + } + + //Scale coefficient sum to 1 + polynomial.scaleCoefficientSum(1); + + this.polynomial = polynomial; + } + + /** + * Passes the smoothed joystick output to the polynomial, while preserving sign. + * + * @return The processed value of the joystick + */ + @Override + public double getValue() { + return polynomial.get(super.getValue()); + } +} \ No newline at end of file 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 new file mode 100644 index 00000000..39dea2e6 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedSmoothedThrottle.java @@ -0,0 +1,65 @@ +package org.usfirst.frc.team449.robot.components; + +import com.fasterxml.jackson.annotation.*; +import edu.wpi.first.wpilibj.filters.LinearDigitalFilter; +import org.jetbrains.annotations.NotNull; + +/** + * A smoothed throttle with a deadband. + */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class MappedSmoothedThrottle extends MappedThrottle { + + protected final double deadband; + + private final LinearDigitalFilter filter; + + private double input; + + private double sign; + + /** + * A basic constructor. + * + * @param stick The Joystick object being used + * @param axis The axis being used. 0 is X, 1 is Y, 2 is Z. + * @param smoothingTimeConstantSecs How many seconds of past input strongly effect the smoothing algorithm. + * @param deadband The deadband below which the input will be read as 0, on [0, 1]. Defaults to 0. + * @param inverted Whether or not to invert the joystick input. Defaults to false. + */ + @JsonCreator + public MappedSmoothedThrottle(@NotNull @JsonProperty(required = true) MappedJoystick stick, + @JsonProperty(required = true) int axis, + double smoothingTimeConstantSecs, + double deadband, + boolean inverted) { + super(stick, axis, inverted); + this.deadband = deadband; + filter = LinearDigitalFilter.singlePoleIIR(this, smoothingTimeConstantSecs, 0.02); + } + + /** + * Gets the value from the joystick and smoothes it. + * + * @return The joystick's value, after processed with a smoothing function. + */ + @Override + public double getValue() { + //Get the smoothed value + input = filter.pidGet(); + + sign = Math.signum(input); + input = Math.abs(input); + + //apply the deadband. + if (input < deadband) { + return 0; + } + + //scale so f(deadband) is 0 and f(1) is 1. + input = (input - deadband) / (1. - deadband); + + return sign * input; + } +} \ No newline at end of file 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 new file mode 100644 index 00000000..2c5c61f3 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedThrottle.java @@ -0,0 +1,96 @@ +package org.usfirst.frc.team449.robot.components; + +import com.fasterxml.jackson.annotation.*; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; + +/** + * A class representing a single axis on a joystick. + */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class MappedThrottle implements PIDSource { + + /** + * The stick we're using + */ + @NotNull + protected final Joystick stick; + + /** + * The axis on the joystick we care about. + */ + private final int axis; + + /** + * Whether or not the controls should be inverted + */ + private final boolean inverted; + + /** + * A basic constructor. + * + * @param stick The Joystick object being used + * @param axis The axis being used. 0 is X, 1 is Y, 2 is Z. + * @param inverted Whether or not to invert the joystick input. Defaults to false. + */ + @JsonCreator + public MappedThrottle(@NotNull @JsonProperty(required = true) MappedJoystick stick, + @JsonProperty(required = true) int axis, + boolean inverted) { + this.stick = stick; + this.axis = axis; + this.inverted = inverted; + } + + /** + * 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(){ + return (inverted ? -1 : 1) * stick.getRawAxis(axis); + } + + /** + * Gets the raw value from the stick and inverts it if necessary. + * + * @return The raw joystick output, on [-1, 1]. + */ + public double getValue() { + return getValuePrivate(); + } + + /** + * Get which parameter of the device you are using as a process control variable. We don't use this. + * + * @return the currently selected PID source parameter + */ + @Override + @Nullable + public PIDSourceType getPIDSourceType() { + return null; + } + + /** + * Set which parameter of the device you are using as a process control variable. We don't use this. + * + * @param pidSource An enum to select the parameter. + */ + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + //Do nothing! + } + + /** + * Get the result to use in PIDController. + * + * @return the result to use in PIDController + */ + @Override + public double pidGet() { + return getValuePrivate(); + } +} \ No newline at end of file diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedUsbCamera.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedUsbCamera.java new file mode 100644 index 00000000..8925f4d5 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedUsbCamera.java @@ -0,0 +1,44 @@ +package org.usfirst.frc.team449.robot.components; + +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.cscore.UsbCamera; +import org.jetbrains.annotations.NotNull; + +/** + * A Jackson-compatible wrapper on the {@link UsbCamera}. + */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class MappedUsbCamera extends UsbCamera { + + /** + * Default constructor + * + * @param name The human-friendly name for this camera. + * @param devAddress The address of this device in /dev/, e.g. this would be 0 for /dev/video0, 1 for /dev/video1. + * @param width The width of this camera's output, in pixels. There's a minimum value for this that WPILib + * won't let + * us go below, but I don't know what it is. + * @param height The height of this camera's output, in pixels. There's a minimum value for this that WPILib + * won't + * let us go below, but I don't know what it is. + * @param fps The frames per second this camera tries to transmit. There's a minimum value for this that + * WPILib + * won't let us go below, but I don't know what it is. + */ + @JsonCreator + public MappedUsbCamera(@NotNull @JsonProperty(required = true) String name, + @JsonProperty(required = true) int devAddress, + @JsonProperty(required = true) int width, + @JsonProperty(required = true) int height, + @JsonProperty(required = true) int fps) { + super(name, devAddress); + setResolution(width, height); + setFPS(fps); + + //If we don't have the exposure be automatic, the camera will be super laggy. No idea why. + setExposureAuto(); + } +} diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedVictor.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedVictor.java index 1e10138d..89d45522 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedVictor.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/MappedVictor.java @@ -1,20 +1,27 @@ package org.usfirst.frc.team449.robot.components; +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.VictorSP; -import maps.org.usfirst.frc.team449.robot.components.MotorMap; /** - * A wrapper for a VictorSP allowing it to be easily constructed from a map object. + * A wrapper for a {@link VictorSP} allowing it to be easily constructed from a map object. */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public class MappedVictor extends VictorSP { /** - * Construct a {@link VictorSP} from a {@link MotorMap.Motor}. + * Json constructor using a port and inversion. * - * @param map a motor map specifying port and inversion. + * @param port The port number of the Victor. + * @param inverted Whether the motor is inverted. Defaults to false. */ - public MappedVictor(MotorMap.Motor map) { - super(map.getPort()); - this.setInverted(map.getInverted()); + @JsonCreator + public MappedVictor(@JsonProperty(required = true) int port, + boolean inverted) { + super(port); + this.setInverted(inverted); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/PressureSensor.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/PressureSensor.java index 9b253650..373a1f63 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/PressureSensor.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/components/PressureSensor.java @@ -1,26 +1,37 @@ package org.usfirst.frc.team449.robot.components; +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.AnalogInput; -import maps.org.usfirst.frc.team449.robot.components.AnalogPressureSensorMap; /** * Wrapper for an {@link AnalogInput} pressure sensor that returns a voltage linearly proportional to pressure. */ -public class PressureSensor extends Component { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class PressureSensor { + /** * The AnalogInput this is a wrapper on. */ - private AnalogInput sensor; + private final AnalogInput sensor; + /** - * Default constructor + * Default constructor. * - * @param map map of this object. + * @param port The port of the sensor. + * @param oversampleBits The number of oversample bits. + * @param averageBits The number of averaging bits. */ - public PressureSensor(AnalogPressureSensorMap.AnalogPressureSensor map) { - sensor = new AnalogInput(map.getPort()); - sensor.setOversampleBits(map.getOversampleBits()); - sensor.setAverageBits(map.getAverageBits()); + @JsonCreator + public PressureSensor(@JsonProperty(required = true) int port, + @JsonProperty(required = true) int oversampleBits, + @JsonProperty(required = true) int averageBits) { + sensor = new AnalogInput(port); + sensor.setOversampleBits(oversampleBits); + sensor.setAverageBits(averageBits); } /** 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 55073371..0510d17b 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 @@ -1,151 +1,360 @@ package org.usfirst.frc.team449.robot.components; import com.ctre.CANTalon; -import maps.org.usfirst.frc.team449.robot.components.MotorMap; -import maps.org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRXMap; +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.Contract; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.util.Logger; -import java.util.ArrayList; import java.util.List; /** * Component wrapper on the CTRE {@link CANTalon}, with unit conversions to/from RPS built in. Every * non-unit-conversion in this class takes arguments in post-gearing RPS. */ -public class RotPerSecCANTalonSRX extends Component { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class RotPerSecCANTalonSRX { /** * The CTRE CAN Talon SRX that this class is a wrapper on */ - public CANTalon canTalon; + @NotNull + private final CANTalon canTalon; /** - * The counts per rotation of the encoder being used. + * The counts per rotation of the encoder being used, or null if there is no encoder. */ - private int encoderCPR; + @Nullable + private final Integer encoderCPR; /** - * The maximum speed of the motor, in RPS. + * The type of encoder the talon uses, or null if there is no encoder. */ - private double maxSpeed; + @Nullable + private final CANTalon.FeedbackDevice feedbackDevice; /** - * The type of encoder the talon uses. + * The coefficient the output changes by after being measured by the encoder, e.g. this would be 1/70 if + * there was a 70:1 gearing between the encoder and the final output. */ - private CANTalon.FeedbackDevice feedbackDevice; + private final double postEncoderGearing; /** - * The map used to construct this object. + * The number of inches travelled per rotation of the motor this is attached to, or null if there is no encoder. + * Only used for Motion Profile unit conversions. + * {@link Double} so it throws a nullPointer if you try to use it without a value in the map. */ - private RotPerSecCANTalonSRXMap.RotPerSecCANTalonSRX map; + @Nullable + private final Double inchesPerRotation; /** - * The coefficient the output changes by after being measured by the encoder, e.g. this would be 1/70 if - * there was a 70:1 gearing between the encoder and the final output. + * The max speed of this motor, in RPS, when in high gear, or if the output motor doesn't have gears, just the max + * speed. May be null. */ - private double postEncoderGearing; + @Nullable + private final Double maxSpeedHigh; /** - * The diameter of the wheel this is attached to. Only used for Motion Profile unit conversions. - * {@link Double} so it throws a nullPointer if you try to use it without a value in the map. + * If this motor has a low gear, this is the max speed of this motor when in that gear. Otherwise, null. */ - private Double wheelDiameterInches; + @Nullable + private final Double maxSpeedLow; /** - * Construct the CANTalonSRX from its map object + * The PID constants for high gear or, if this motor does not have gears, just the PID constants. + */ + private final int highGearP, highGearI, highGearD; + + /** + * The PID constants for low gear if this motor has a low gear. + */ + private final int lowGearP, lowGearI, lowGearD; + + /** + * The forward and reverse nominal voltages for high gear, or if this motor has no gears, just the nominal voltages. + */ + private final double highGearFwdNominalOutputVoltage, highGearRevNominalOutputVoltage; + + /** + * The forward and reverse nominal voltages for low gear, or null if this motor has no gears. + */ + @Nullable + private final Double lowGearFwdNominalOutputVoltage, lowGearRevNominalOutputVoltage; + + /** + * The maximum speed of the motor, in RPS, or null if not using PID. + */ + @Nullable + private Double maxSpeed; + + /** + * Default constructor. * - * @param map CANTalonSRX map object - */ - public RotPerSecCANTalonSRX(RotPerSecCANTalonSRXMap.RotPerSecCANTalonSRX map) { - // Configure stuff - this.map = map; - canTalon = new CANTalon(map.getPort()); - encoderCPR = map.getEncoderCPR(); - canTalon.reverseSensor(map.getReverseSensor()); - canTalon.reverseOutput(map.getReverseOutput()); - canTalon.setInverted(map.getIsInverted()); - canTalon.ConfigFwdLimitSwitchNormallyOpen(map.getFwdLimNormOpen()); - canTalon.ConfigRevLimitSwitchNormallyOpen(map.getRevLimNormOpen()); - canTalon.enableLimitSwitch(map.getFwdLimEnabled(), map.getRevLimEnabled()); - canTalon.enableForwardSoftLimit(map.getFwdSoftLimEnabled()); - canTalon.setForwardSoftLimit(map.getFwdSoftLimVal()); - canTalon.enableReverseSoftLimit(map.getRevSoftLimEnabled()); - canTalon.setReverseSoftLimit(map.getRevSoftLimVal()); - canTalon.enableBrakeMode(map.getBrakeMode()); - postEncoderGearing = map.getPostEncoderGearing(); - - //High gear speed is the default - maxSpeed = map.getMaxSpeedHg(); - - //We have to do some weird things to convert out of the enum in the proto and into the CANTalon enum. - feedbackDevice = CANTalon.FeedbackDevice.valueOf(map.getFeedbackDevice().getNumber()); - canTalon.setFeedbackDevice(feedbackDevice); - - //Configure the minimum output voltage. Should be symmetrical because it can go forwards and back. - canTalon.configNominalOutputVoltage(+map.getNominalOutVoltage(), -map.getNominalOutVoltage()); - - //Configure the maximum output voltage. Should be symmetrical because it can go forwards and back. - canTalon.configPeakOutputVoltage(+map.getPeakOutVoltage(), -map.getPeakOutVoltage()); - - //Initialize the PID constants in slot 0 to the high gear ones. - setPIDF(map.getKPHg(), map.getKIHg(), map.getKDHg(), maxSpeed, 0, 0, 0); - - //Assume regular driving profile by default. - canTalon.setProfile(0); - - //Configure optional parameters - if (map.hasClosedLoopRampRate()) { - canTalon.setCloseLoopRampRate(map.getClosedLoopRampRate()); + * @param port CAN port of this Talon. + * @param inverted Whether this Talon is inverted. + * @param enableBrakeMode Whether to brake or coast when stopped. + * @param fwdPeakOutputVoltage The peak voltage in the forward direction, in volts. If + * revPeakOutputVoltage is null, this is used for peak voltage in both + * directions. Should be a positive or zero. + * @param revPeakOutputVoltage The peak voltage in the reverse direction. Can be null, and if it is, + * fwdPeakOutputVoltage is used as the peak voltage in both directions. + * Should be positive or zero. + * @param highGearFwdNominalOutputVoltage The minimum non-zero voltage in the forward direction in the high/only + * gear, in volts. If highGearRevNominalOutputVoltage is null, this is used + * for nominal voltage in both directions. Should be a positive or zero. + * @param highGearRevNominalOutputVoltage The minimum non-zero voltage in the reverse direction in the high/only + * gear. Can be null, and if it is, highGearFwdNominalOutputVoltage is used + * as the nominal voltage in both directions. Should be positive or zero. + * @param lowGearFwdNominalOutputVoltage The minimum non-zero voltage in the forward direction in the high/only + * gear, in volts. If highGearRevNominalOutputVoltage is null, this is used + * for nominal voltage in both directions. Should be a positive or zero. Can + * be null if this Talon doesn't have a low gear. + * @param lowGearRevNominalOutputVoltage The minimum non-zero voltage in the reverse direction in the high/only + * gear. Can be null, and if it is, highGearFwdNominalOutputVoltage is used + * as the nominal voltage in both directions. Should be positive or zero. + * @param fwdLimitSwitchNormallyOpen Whether the forward limit switch is normally open or closed. If this is + * null, the forward limit switch is disabled. + * @param revLimitSwitchNormallyOpen Whether the reverse limit switch is normally open or closed. If this is + * null, the reverse limit switch is disabled. + * @param fwdSoftLimit The forward software limit. If this is null, the forward software limit + * is + * disabled. + * TODO figure out units + * @param revSoftLimit The reverse software limit. If this is null, the reverse software limit + * is + * disabled. + * TODO figure out units + * @param postEncoderGearing The coefficient the output changes by after being measured by the + * encoder, + * e.g. this would be 1/70 if there was a 70:1 gearing between the encoder + * and the final output. Defaults to 1. + * @param closedLoopRampRate The voltage ramp rate for closed-loop velocity control. Can be null, and + * if it is, no ramp rate is used. + * @param inchesPerRotation The number of inches travelled per rotation of the motor this is attached + * to. + * @param currentLimit The max amps this device can draw. If this is null, no current limit is + * used. + * @param feedbackDevice The type of encoder used to measure the output velocity of this motor. + * Can + * be null if there is no encoder attached to this Talon. + * @param encoderCPR The counts per rotation of the encoder on this Talon. Can be null if + * feedbackDevice is, but otherwise must have a value. + * @param reverseSensor Whether or not to reverse the reading from the encoder on this Talon. Can + * be null if feedbackDevice is, but otherwise must have a value. + * @param maxSpeedHigh The high gear max speed, in RPS. If this motor doesn't have gears, then + * this is just the max speed. Used to calculate velocity PIDF feed-forward. + * Can be null, and if it is, it's assumed that this motor won't use + * velocity + * closed-loop control. + * @param highGearP The proportional gain for high gear. Defaults to 0. + * @param highGearI The integral gain for high gear. Defaults to 0. + * @param highGearD The derivative gain for high gear. Defaults to 0. + * @param maxSpeedLow The low gear max speed in RPS. Used to calculate velocity PIDF + * feed-forward. Can be null, and if it is, it's assumed that either this + * motor doesn't have a low gear or the low gear won't use velocity + * closed-loop control. + * @param lowGearP The proportional gain for low gear. Defaults to 0. + * @param lowGearI The integral gain for low gear. Defaults to 0. + * @param lowGearD The derivative gain for low gear. Defaults to 0. + * @param motionProfileP The proportional gain for motion profiles. Defaults to 0. + * @param motionProfileI The integral gain for high motion profiles. Defaults to 0. + * @param motionProfileD The derivative gain for high motion profiles. Defaults to 0. + * @param MPUseLowGear Whether this motor uses high or low gear for running motion profiles. + * Defaults to false. + * @param slaves The other {@link CANTalon}s that are slaved to this one. + */ + @JsonCreator + public RotPerSecCANTalonSRX(@JsonProperty(required = true) int port, + @JsonProperty(required = true) boolean inverted, + @JsonProperty(required = true) boolean enableBrakeMode, + @JsonProperty(required = true) double fwdPeakOutputVoltage, + @Nullable Double revPeakOutputVoltage, + @JsonProperty(required = true) double highGearFwdNominalOutputVoltage, + @Nullable Double highGearRevNominalOutputVoltage, + @Nullable Double lowGearFwdNominalOutputVoltage, + @Nullable Double lowGearRevNominalOutputVoltage, + @Nullable Boolean fwdLimitSwitchNormallyOpen, + @Nullable Boolean revLimitSwitchNormallyOpen, + @Nullable Double fwdSoftLimit, + @Nullable Double revSoftLimit, + @Nullable Double postEncoderGearing, + @Nullable Double closedLoopRampRate, + @Nullable Double inchesPerRotation, + @Nullable Integer currentLimit, + @Nullable CANTalon.FeedbackDevice feedbackDevice, + @Nullable Integer encoderCPR, + @Nullable Boolean reverseSensor, + @Nullable Double maxSpeedHigh, + int highGearP, + int highGearI, + int highGearD, + @Nullable Double maxSpeedLow, + int lowGearP, + int lowGearI, + int lowGearD, + int motionProfileP, + int motionProfileI, + int motionProfileD, + boolean MPUseLowGear, + @Nullable List slaves) { + //Instantiate the base CANTalon this is a wrapper on. + canTalon = new CANTalon(port); + //Set this to false because we only use reverseOutput for slaves. + canTalon.reverseOutput(false); + //Set inversion + canTalon.setInverted(inverted); + //Set brake mode + canTalon.enableBrakeMode(enableBrakeMode); + + //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; } - if (map.hasWheelDiameterInches()) { - wheelDiameterInches = map.getWheelDiameterInches(); + //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; } - //If we have motion profile PID constants, put them in slot 1. - if (map.hasKPMp()) { - //If we have a low gear and want to use it for MP, set the MP max speed to the low gear max. - if (map.hasMaxSpeedLg() && map.getMpUseLowGear()) { - setPIDF(map.getKPMp(), map.getKIMp(), map.getKDMp(), map.getMaxSpeedLg(), 0, 0, 1); - } else { - //Otherwise, use high gear. - setPIDF(map.getKPMp(), map.getKIMp(), map.getKDMp(), map.getMaxSpeedHg(), 0, 0, 1); - } + //Set to high gear by default. + canTalon.configNominalOutputVoltage(this.highGearFwdNominalOutputVoltage, -this.highGearRevNominalOutputVoltage); + + //Only enable the limit switches if it was specified if they're normally open or closed. + boolean fwdSwitchEnable = false, revSwitchEnable = false; + if (fwdLimitSwitchNormallyOpen != null) { + canTalon.ConfigFwdLimitSwitchNormallyOpen(fwdLimitSwitchNormallyOpen); + fwdSwitchEnable = true; + } + if (revLimitSwitchNormallyOpen != null) { + canTalon.ConfigRevLimitSwitchNormallyOpen(revLimitSwitchNormallyOpen); + revSwitchEnable = true; + } + canTalon.enableLimitSwitch(fwdSwitchEnable, revSwitchEnable); + + //Only enable the software limits if they were given a value. + if (fwdSoftLimit != null) { + canTalon.enableForwardSoftLimit(true); + canTalon.setForwardSoftLimit(fwdSoftLimit); + } else { + canTalon.enableForwardSoftLimit(false); + } + if (revSoftLimit != null) { + canTalon.enableReverseSoftLimit(true); + canTalon.setReverseSoftLimit(revSoftLimit); + } else { + canTalon.enableReverseSoftLimit(false); } - if (map.hasCurrentLimit()) { - canTalon.setCurrentLimit(map.getCurrentLimit()); + //Set up the feedback device if it exists. + if (feedbackDevice != null) { + this.feedbackDevice = feedbackDevice; + canTalon.setFeedbackDevice(feedbackDevice); + this.encoderCPR = encoderCPR; + canTalon.reverseSensor(reverseSensor); + } else { + this.feedbackDevice = null; + this.encoderCPR = null; + } + + //postEncoderGearing defaults to 1 + if (postEncoderGearing == null) { + this.postEncoderGearing = 1.; + } else { + this.postEncoderGearing = postEncoderGearing; + } + + //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); + + //Set the current limit if it was given + if (currentLimit != null) { + canTalon.setCurrentLimit(currentLimit); canTalon.EnableCurrentLimit(true); } else { //If we don't have a current limit, disable current limiting. canTalon.EnableCurrentLimit(false); } - //Set up slaves. - for (MotorMap.Motor slave : map.getSlaveList()) { - CANTalon tmp = new CANTalon(slave.getPort()); - tmp.reverseSensor(false); - tmp.setInverted(false); - tmp.ConfigFwdLimitSwitchNormallyOpen(map.getFwdLimNormOpen()); - tmp.ConfigRevLimitSwitchNormallyOpen(map.getRevLimNormOpen()); - tmp.enableLimitSwitch(map.getFwdLimEnabled(), map.getRevLimEnabled()); - tmp.enableForwardSoftLimit(map.getFwdSoftLimEnabled()); - tmp.setForwardSoftLimit(map.getFwdSoftLimVal()); - tmp.enableReverseSoftLimit(map.getRevSoftLimEnabled()); - tmp.setReverseSoftLimit(map.getRevSoftLimVal()); - tmp.enableBrakeMode(map.getBrakeMode()); - tmp.enable(); - if (map.hasCurrentLimit()) { - tmp.setCurrentLimit(map.getCurrentLimit()); - tmp.EnableCurrentLimit(true); - } else { - //If we don't have a current limit, disable current limiting. - tmp.EnableCurrentLimit(false); + //Configure ramp rate + if (closedLoopRampRate != null) { + canTalon.setCloseLoopRampRate(closedLoopRampRate); + } + + //Set fields + this.maxSpeedHigh = maxSpeedHigh; + this.highGearP = highGearP; + this.highGearI = highGearI; + this.highGearD = highGearD; + this.maxSpeedLow = maxSpeedLow; + this.lowGearP = lowGearP; + this.lowGearI = lowGearI; + this.lowGearD = lowGearD; + this.inchesPerRotation = inchesPerRotation; + + //Set up PID constants. + if (maxSpeedHigh != null) { + //High gear speed is the default + maxSpeed = maxSpeedHigh; + + //Initialize the PID constants in slot 0 to the high gear ones. + setPIDF(highGearP, highGearI, highGearD, maxSpeed, 0, 0, 0); + + //Assume regular driving profile by default. + canTalon.setProfile(0); + + //Add Motion Profile PID constants if we have them. + if (motionProfileP != 0) { + //If we have a low gear and want to use it for MP, set the MP max speed to the low gear max. + if (maxSpeedLow != null && MPUseLowGear) { + setPIDF(motionProfileP, motionProfileI, motionProfileD, maxSpeedLow, 0, 0, 1); + } else { + //Otherwise, use high gear. + setPIDF(motionProfileP, motionProfileI, motionProfileD, maxSpeed, 0, 0, 1); + } + } + } + + if (slaves != null) { + //Set up slaves. + for (SlaveTalon slave : slaves) { + CANTalon tmp = new CANTalon(slave.getPort()); + //To invert slaves, use reverseOutput. See section 9.1.4 of the TALON SRX Software Reference Manual. + tmp.reverseOutput(slave.isInverted()); + //Don't use the other inversion options + tmp.reverseSensor(false); + tmp.setInverted(false); + + //Brake mode and current limiting don't automatically follow master, so we set them up for each slave. + tmp.enableBrakeMode(enableBrakeMode); + if (currentLimit != null) { + canTalon.setCurrentLimit(currentLimit); + canTalon.EnableCurrentLimit(true); + } else { + //If we don't have a current limit, disable current limiting. + canTalon.EnableCurrentLimit(false); + } + + //Don't forget to enable! + tmp.enable(); + //Set the slave up to follow this talon. + tmp.changeControlMode(CANTalon.TalonControlMode.Follower); + tmp.set(port); } - //To invert slaves, use reverseOutput. See section 9.1.4 of the TALON SRX Software Reference Manual. - tmp.reverseOutput(slave.getInverted()); - tmp.changeControlMode(CANTalon.TalonControlMode.Follower); - tmp.set(map.getPort()); } } @@ -182,42 +391,51 @@ public void setPercentVbus(double percentVbus) { */ private void setPIDF(double p, double i, double d, double maxSpeed, int iZone, double closeLoopRampRate, int profile) { - this.canTalon.setPID(p, i, d, 1023 / RPSToNative(maxSpeed), iZone, closeLoopRampRate, profile); + try { + this.canTalon.setPID(p, i, d, 1023 / RPSToNative(maxSpeed), iZone, closeLoopRampRate, profile); + } catch (NullPointerException e) { + System.out.println("Tried to set F value, but no encoder CPR given!"); + e.printStackTrace(); + } + } /** - * Switch to using the high gear PID constants. + * Switch to using the high gear PID constants and nominal voltage. */ public void switchToHighGear() { - //Switch max speed to high gear max speed - maxSpeed = map.getMaxSpeedHg(); - //Set the slot 0 constants to the high gear ones. - setPIDF(map.getKPHg(), map.getKIHg(), map.getKDHg(), maxSpeed, 0, 0, 0); + canTalon.configNominalOutputVoltage(highGearFwdNominalOutputVoltage, -highGearRevNominalOutputVoltage); + if (maxSpeedHigh != null) { + //Switch max speed to high gear max speed + maxSpeed = maxSpeedHigh; + //Set the slot 0 constants to the high gear ones. + setPIDF(highGearP, highGearI, highGearD, maxSpeed, 0, 0, 0); + } } /** - * Switch to using the low gear PID constants if we have them. + * Switch to using the low gear PID constants and nominal output voltages if we have them. */ public void switchToLowGear() { + if (lowGearFwdNominalOutputVoltage != null) { + canTalon.configNominalOutputVoltage(lowGearFwdNominalOutputVoltage, -lowGearRevNominalOutputVoltage); + } //If there are low gear constants in the map - if (map.hasKPLg()) { + if (maxSpeedLow != null) { //Switch max speed to low gear max speed - maxSpeed = map.getMaxSpeedLg(); + maxSpeed = maxSpeedLow; //Set the slot 0 constants to the low gear ones. - setPIDF(map.getKPLg(), map.getKILg(), map.getKDLg(), maxSpeed, 0, 0, 0); - } else { - //Warn the user if they're trying to do this but don't have the low gear constants in the map. - Logger.addEvent("You're trying to switch your PIDF constants to low gear, but you don't have low gear " + - "constants.", this.getClass()); + setPIDF(lowGearP, lowGearI, lowGearD, maxSpeed, 0, 0, 0); } } /** * Get the max speed of the gear the talon is currently in. * - * @return max speed, in RPS, as given in the map. + * @return max speed, in RPS, as given in the map, or null if no value given. */ - public double getMaxSpeed() { + @Nullable + public Double getMaxSpeed() { return maxSpeed; } @@ -226,13 +444,21 @@ public double getMaxSpeed() { * Note this DOES account for post-encoder gearing. * * @param encoderReading The velocity read from the encoder with no conversions. - * @return The velocity of the output shaft, in RPS, when the encoder has that reading. + * @return The velocity of the output shaft, in RPS, when the encoder has that reading, or null if no encoder CPR + * was given. */ - public double encoderToRPS(double encoderReading) { + @Nullable + public Double encoderToRPS(double encoderReading) { if (feedbackDevice == CANTalon.FeedbackDevice.CtreMagEncoder_Absolute || feedbackDevice == CANTalon.FeedbackDevice.CtreMagEncoder_Relative) { + //CTRE encoders use RPM return RPMToRPS(encoderReading) * postEncoderGearing; } else { - return nativeToRPS(encoderReading) * postEncoderGearing; + //All other feedback devices use native units. + Double RPS = nativeToRPS(encoderReading); + if (RPS == null) { + return null; + } + return RPS * postEncoderGearing; } } @@ -241,13 +467,20 @@ public double encoderToRPS(double encoderReading) { * Note this DOES account for post-encoder gearing. * * @param RPS The velocity of the output shaft, in RPS. - * @return What the raw encoder reading would be at that velocity. + * @return What the raw encoder reading would be at that velocity, or null if no encoder CPR was given. */ - public double RPSToEncoder(double RPS) { + @Nullable + public Double RPSToEncoder(double RPS) { if (feedbackDevice == CANTalon.FeedbackDevice.CtreMagEncoder_Absolute || feedbackDevice == CANTalon.FeedbackDevice.CtreMagEncoder_Relative) { + //CTRE encoders use RPM return RPSToRPM(RPS) / postEncoderGearing; } else { - return RPSToNative(RPS) / postEncoderGearing; + //All other feedback devices use native units. + Double encoderReading = RPSToNative(RPS); + if (encoderReading == null) { + return null; + } + return encoderReading / postEncoderGearing; } } @@ -256,9 +489,14 @@ public double RPSToEncoder(double RPS) { * gearing. * * @param RPS The RPS velocity you want to convert. - * @return That velocity in CANTalon native units. + * @return That velocity in CANTalon native units, or null if no encoder CPR was given. */ - private double RPSToNative(double RPS) { + @Contract(pure = true) + @Nullable + private Double RPSToNative(double RPS) { + if (encoderCPR == null) { + return null; + } return (RPS / 10) * (encoderCPR * 4); //4 edges per count, and 10 100ms per second. } @@ -267,9 +505,14 @@ private double RPSToNative(double RPS) { * post-encoder gearing. * * @param nat A velocity in CANTalon native units. - * @return That velocity in RPS. + * @return That velocity in RPS, or null if no encoder CPR was given. */ - private double nativeToRPS(double nat) { + @Contract(pure = true) + @Nullable + private Double nativeToRPS(double nat) { + if (encoderCPR == null) { + return null; + } return (nat / (encoderCPR * 4)) * 10; //4 edges per count, and 10 100ms per second. } @@ -279,6 +522,7 @@ private double nativeToRPS(double nat) { * @param rpm A velocity in RPM. * @return That velocity in RPS. */ + @Contract(pure = true) private double RPMToRPS(double rpm) { return rpm / 60.; } @@ -289,6 +533,7 @@ private double RPMToRPS(double rpm) { * @param rps A velocity in RPS. * @return That velocity in RPM. */ + @Contract(pure = true) private double RPSToRPM(double rps) { return rps * 60.; } @@ -296,19 +541,20 @@ private double RPSToRPM(double rps) { /** * Get the velocity of the CANTalon in RPS *

- * Note: This method is called getMode since the TalonControlMode enum is called speed. However, the output - * is signed and is actually a velocity. + * Note: This method is called getSpeed since the {@link CANTalon} method is called getSpeed. However, the output is + * signed and is actually a velocity. * - * @return The CANTalon's velocity in RPS + * @return The CANTalon's velocity in RPS, or null if no encoder CPR was given. */ - public double getSpeed() { + @Nullable + public Double getSpeed() { return encoderToRPS(canTalon.getSpeed()); } /** * Give a velocity closed loop setpoint in RPS *

- * Note: This method is called setSpeed since the TalonControlMode enum is called speed. However, the input + * Note: This method is called setSpeed since the {@link CANTalon} method is called getSpeed. However, the input * argument is signed and is actually a velocity. * * @param velocitySp velocity setpoint in revolutions per second @@ -322,28 +568,31 @@ public void setSpeed(double velocitySp) { /** * Get the current closed-loop velocity error in RPS. WARNING: will give garbage if not in velocity mode. * - * @return The closed-loop error in RPS + * @return The closed-loop error in RPS, or null if no encoder CPR was given. */ - public double getError() { + @Nullable + public Double getError() { return encoderToRPS(canTalon.getError()); } /** * Get the current velocity setpoint of the Talon in RPS. WARNING: will give garbage if not in velocity mode. * - * @return The closed-loop velocity setpoint in RPS. + * @return The closed-loop velocity setpoint in RPS, or null if no encoder CPR was given. */ - public double getSetpoint() { + @Nullable + public Double getSetpoint() { return encoderToRPS(canTalon.getSetpoint()); } /** - * Get the high gear max speed. Sometimes useful for scaling joystick output. + * Get the high gear max speed. Used for for scaling joystick output. * - * @return The high gear max speed in RPS. + * @return The high gear max speed in RPS, or null if none was given. */ - public double getMaxSpeedHG() { - return map.getMaxSpeedHg(); + @Nullable + public Double getMaxSpeedHG() { + return maxSpeedHigh; } /** @@ -360,11 +609,15 @@ public double getPower() { * Note this DOES account for post-encoder gearing. * * @param nativeUnits A distance native units as measured by the encoder. - * @return That distance in feet. + * @return That distance in feet, or null if no encoder CPR or inches per rotation was given. */ - public double nativeToFeet(double nativeUnits) { + @Nullable + public Double nativeToFeet(double nativeUnits) { + if (encoderCPR == null || inchesPerRotation == null) { + return null; + } double rotations = nativeUnits / (encoderCPR * 4) * postEncoderGearing; - return rotations * (wheelDiameterInches * Math.PI); + return rotations * (inchesPerRotation / 12.); } /** @@ -372,10 +625,15 @@ public double nativeToFeet(double nativeUnits) { * Note this DOES account for post-encoder gearing. * * @param feet A distance in feet. - * @return That distance in native units as measured by the encoder. + * @return That distance in native units as measured by the encoder, or null if no encoder CPR or inches per + * rotation was given. */ - public double feetToNative(double feet) { - double rotations = feet / (wheelDiameterInches * Math.PI); + @Nullable + public Double feetToNative(double feet) { + if (encoderCPR == null || inchesPerRotation == null) { + return null; + } + double rotations = feet / (inchesPerRotation / 12.); return rotations * (encoderCPR * 4) / postEncoderGearing; } @@ -384,10 +642,15 @@ public double feetToNative(double feet) { * Note this DOES account for post-encoder gearing. * * @param fps A velocity in feet per second - * @return That velocity in either native units or RPS, depending on the type of encoder. + * @return That velocity in either native units or RPS, depending on the type of encoder, or null if no encoder CPR + * or inches per rotation was given. */ - public double feetPerSecToNative(double fps) { - return RPSToEncoder(fps / (wheelDiameterInches * Math.PI)); + @Nullable + public Double feetPerSecToNative(double fps) { + if (inchesPerRotation == null) { + return null; + } + return RPSToEncoder(fps / (inchesPerRotation / 12.)); } /** @@ -395,9 +658,72 @@ public double feetPerSecToNative(double fps) { * Note this DOES account for post-encoder gearing. * * @param nativeUnits A velocity in either native units or RPS, depending on the type of encoder. - * @return That velocity in feet per second. + * @return That velocity in feet per second, or null if no encoder CPR or inches per rotation was given. + */ + @Nullable + public Double nativeToFeetPerSec(double nativeUnits) { + Double RPS = encoderToRPS(nativeUnits); + if (inchesPerRotation == null || RPS == null) { + return null; + } + return RPS * inchesPerRotation / 12.; + } + + /** + * Get the CANTalon this is a wrapper on. + * + * @return The canTalon this wraps. */ - public double nativeToFeetPerSec(double nativeUnits) { - return encoderToRPS(nativeUnits) * (wheelDiameterInches * Math.PI); + @NotNull + public CANTalon getCanTalon() { + return canTalon; + } + + /** + * An object representing a slave {@link CANTalon} for use in the map. + */ + @JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) + private static class SlaveTalon { + + /** + * The port number of this Talon. + */ + private final int port; + + /** + * Whether this Talon is inverted compared to its master. + */ + private final boolean inverted; + + /** + * Default constructor. + * + * @param port The port number of this Talon. + * @param inverted Whether this Talon is inverted compared to its master. + */ + @JsonCreator + public SlaveTalon(@JsonProperty(required = true) int port, + @JsonProperty(required = true) boolean inverted) { + this.port = port; + this.inverted = inverted; + } + + /** + * Getter for port number. + * + * @return The port number of this Talon. + */ + public int getPort() { + return port; + } + + /** + * Getter for inversion. + * + * @return true if this Talon is inverted compared to its master, false otherwise. + */ + public boolean isInverted() { + return inverted; + } } } 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 new file mode 100644 index 00000000..b56ed619 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/ShiftingTalonClusterDrive.java @@ -0,0 +1,171 @@ +package org.usfirst.frc.team449.robot.drive.talonCluster; + +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.DoubleSolenoid; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; +import org.usfirst.frc.team449.robot.components.MappedAHRS; +import org.usfirst.frc.team449.robot.components.MappedDoubleSolenoid; +import org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRX; +import org.usfirst.frc.team449.robot.interfaces.drive.shifting.ShiftingDrive; +import org.usfirst.frc.team449.robot.util.CANTalonMPHandler; + + +/** + * A drive with a cluster of any number of CANTalonSRX controlled motors on each side and a high and low gear. + */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ShiftingTalonClusterDrive extends TalonClusterDrive implements ShiftingDrive { + + /** + * The solenoid that shifts between gears + */ + @NotNull + private final DoubleSolenoid shifter; + + /** + * The gear to start teleop and autonomous in. + */ + @NotNull + private final gear startingGear; + + /** + * Whether not to override auto shifting + */ + private boolean overrideAutoshift; + + /** + * What gear we're in + */ + @NotNull + private gear currentGear; + + /** + * Default constructor. + * + * @param leftMaster The master talon on the left side of the drive. + * @param rightMaster The master talon on the right side of the drive. + * @param navX The NavX on this drive. + * @param MPHandler The motion profile handler that runs this drive's motion profiles. + * @param PIDScale The amount to scale the output to the PID loop by. Defaults to 1. + * @param shifter The piston that shifts between gears. + * @param startingGear The gear the drive starts in. Defaults to low. + */ + @JsonCreator + public ShiftingTalonClusterDrive(@NotNull @JsonProperty(required = true) RotPerSecCANTalonSRX leftMaster, + @NotNull @JsonProperty(required = true) RotPerSecCANTalonSRX rightMaster, + @NotNull @JsonProperty(required = true) MappedAHRS navX, + @NotNull @JsonProperty(required = true) CANTalonMPHandler MPHandler, + @Nullable Double PIDScale, + @NotNull @JsonProperty(required = true) MappedDoubleSolenoid shifter, + @Nullable gear startingGear) { + super(leftMaster, rightMaster, navX, MPHandler, PIDScale); + //Initialize stuff + this.shifter = shifter; + + //Default to low + if (startingGear == null) { + this.startingGear = gear.LOW; + } else { + this.startingGear = startingGear; + } + currentGear = this.startingGear; + + // Initialize shifting constants, assuming robot is stationary. + overrideAutoshift = false; + } + + /** + * A getter for whether we're currently overriding autoshifting. + * + * @return true if overriding, false otherwise. + */ + @Override + public boolean getOverrideAutoshift() { + return overrideAutoshift; + } + + /** + * A setter for overriding the autoshifting. + * + * @param override Whether or not to override autoshifting. + */ + @Override + public void setOverrideAutoshift(boolean override) { + this.overrideAutoshift = override; + } + + /** + * Sets left and right wheel PID velocity setpoint as a percent of max setpoint. Defaults to voltage setpoint if the + * talons don't have PID constants. + * + * @param left The left PID velocity setpoint as a percent [-1, 1] + * @param right The right PID velocity setpoint as a percent [-1, 1] + */ + @Override + protected void setPIDThrottle(double left, double right) { + //If we're not shifting or we're just turning in place, scale by the max speed in the current gear + if (overrideAutoshift || left == right) { + super.setPIDThrottle(left, right); + } + //If we are shifting, scale by the high gear max speed to make acceleration smoother and faster. + else { + if (leftMaster.getMaxSpeedHG() == null || rightMaster.getMaxSpeedHG() == null) { + setVBusThrottle(left, right); + System.out.println("You're trying to set PID throttle, but the drive talons don't have PID constants defined. Using voltage control instead."); + } else { + leftMaster.setSpeed(PID_SCALE * (left * leftMaster.getMaxSpeedHG())); + rightMaster.setSpeed(PID_SCALE * (right * rightMaster.getMaxSpeedHG())); + } + } + } + + /** + * @return The gear this subsystem is currently in. + */ + @Override + @NotNull + public gear getGear() { + return currentGear; + } + + /** + * Shift to a specific gear. + * + * @param gear Which gear to shift to. + */ + @Override + public void setGear(@NotNull gear gear) { + //If we want to downshift + if (gear == ShiftingDrive.gear.LOW) { + //Physically shift gears + shifter.set(DoubleSolenoid.Value.kForward); + //Switch the PID constants + rightMaster.switchToLowGear(); + leftMaster.switchToLowGear(); + //Record the current time + } else { + //Physically shift gears + shifter.set(DoubleSolenoid.Value.kReverse); + //Switch the PID constants + rightMaster.switchToHighGear(); + leftMaster.switchToHighGear(); + //Record the current time. + } + //Set logging var + currentGear = gear; + } + + /** + * A getter for the starting gear. + * + * @return The gear this subsystem starts auto and teleop in. + */ + @NotNull + public gear getStartingGear() { + return startingGear; + } +} 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 46bdf84c..a95262bc 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 @@ -1,67 +1,58 @@ package org.usfirst.frc.team449.robot.drive.talonCluster; +import com.fasterxml.jackson.annotation.*; import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.command.Command; -import maps.org.usfirst.frc.team449.robot.util.ToleranceBufferAnglePIDMap; -import org.usfirst.frc.team449.robot.Robot; -import org.usfirst.frc.team449.robot.components.MappedDoubleSolenoid; +import org.jetbrains.annotations.Contract; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; +import org.usfirst.frc.team449.robot.components.MappedAHRS; import org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRX; -import org.usfirst.frc.team449.robot.drive.DriveSubsystem; -import org.usfirst.frc.team449.robot.interfaces.drive.shifting.ShiftingDrive; import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem.TwoSideMPSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.NavxSubsystem; -import org.usfirst.frc.team449.robot.oi.OI2017ArcadeGamepad; -import org.usfirst.frc.team449.robot.util.*; +import org.usfirst.frc.team449.robot.util.CANTalonMPHandler; +import org.usfirst.frc.team449.robot.util.Loggable; +import org.usfirst.frc.team449.robot.util.MotionProfileData; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * A drive with a cluster of any number of CANTalonSRX controlled motors on each side. */ -public class TalonClusterDrive extends DriveSubsystem implements NavxSubsystem, UnidirectionalDrive, ShiftingDrive, Loggable, TwoSideMPSubsystem { - - /** - * The PIDAngleCommand constants for turning to an angle with the NavX - */ - public ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID turnPID; - - /** - * The PIDAngleCommand constants for using the NavX to drive straight - */ - public ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID straightPID; +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class TalonClusterDrive extends YamlSubsystem implements NavxSubsystem, UnidirectionalDrive, Loggable, TwoSideMPSubsystem { /** * Joystick scaling constant. Joystick output is scaled by this before being handed to the PID loop to give the * loop space to compensate. */ - private double PID_SCALE; + protected final double PID_SCALE; /** * Right master Talon */ - private RotPerSecCANTalonSRX rightMaster; + @NotNull + protected final RotPerSecCANTalonSRX rightMaster; /** * Left master Talon */ - private RotPerSecCANTalonSRX leftMaster; + @NotNull + protected final RotPerSecCANTalonSRX leftMaster; /** * The NavX gyro */ - private AHRS navx; + @NotNull + private final AHRS navX; /** - * The oi used to drive the robot - */ - private OI2017ArcadeGamepad oi; - - /** - * The solenoid that shifts between gears + * A helper class that loads and runs profiles on the Talons. */ - private DoubleSolenoid shifter; + @NotNull + private final CANTalonMPHandler mpHandler; /** * Whether or not to use the NavX for driving straight @@ -69,102 +60,31 @@ public class TalonClusterDrive extends DriveSubsystem implements NavxSubsystem, private boolean overrideNavX; /** - * Whether not to override auto shifting - */ - private boolean overrideAutoshift; - - /** - * The forward velocity setpoint (on a 0-1 scale) below which we stay in low gear - */ - private double upshiftFwdThresh; - - /** - * The time we last upshifted (milliseconds) - */ - private long timeLastUpshifted; - - /** - * The time we last downshifted (milliseconds) - */ - private long timeLastDownshifted; - - /** - * What gear we're in - */ - private ShiftingDrive.gear currentGear; - - /** - * The speed setpoint at the upshift break - */ - private double upshiftSpeed; - - /** - * The speed setpoint at the downshift break - */ - private double downshiftSpeed; - - /** - * The robot isn't eligible to shift again for this many milliseconds after upshifting. - */ - private long cooldownAfterUpshift; - - /** - * The robot isn't eligible to shift again for this many milliseconds after downshifting. - */ - private long cooldownAfterDownshift; - - /** - * BufferTimers for shifting that make it so all the other conditions to shift must be met for some amount of time - * before shifting actually happens. - */ - private BufferTimer upshiftBufferTimer, downshiftBufferTimer; - - /** - * A helper class that loads and runs profiles on the Talons. - */ - private CANTalonMPHandler mpHandler; - - /** - * Construct a TalonClusterDrive + * Default constructor. * - * @param map config map - * @param oi OI to read throttle from - */ - public TalonClusterDrive(maps.org.usfirst.frc.team449.robot.drive.talonCluster.TalonClusterDriveMap - .TalonClusterDrive map, OI2017ArcadeGamepad oi, gear startingGear) { - super(map.getDrive()); - //Initialize stuff directly from the map. - this.map = map; - this.oi = oi; - PID_SCALE = map.getPIDScale(); - turnPID = map.getTurnPID(); - straightPID = map.getStraightPID(); - upshiftBufferTimer = new BufferTimer(map.getDelayAfterUpshiftConditionsMet()); - downshiftBufferTimer = new BufferTimer(map.getDelayAfterDownshiftConditionsMet()); - cooldownAfterDownshift = (long) (map.getCooldownAfterDownshift() * 1000.); - cooldownAfterUpshift = (long) (map.getCooldownAfterUpshift() * 1000.); - upshiftFwdThresh = map.getUpshiftFwdThreshold(); - upshiftSpeed = map.getUpshiftSpeed(); - downshiftSpeed = map.getDownshiftSpeed(); - currentGear = startingGear; - rightMaster = new RotPerSecCANTalonSRX(map.getRightMaster()); - leftMaster = new RotPerSecCANTalonSRX(map.getLeftMaster()); - - //We only ever use a NavX on the SPI port because the other ports don't work. - navx = new AHRS(SPI.Port.kMXP); - - // Initialize shifting constants, assuming robot is stationary. - overrideAutoshift = false; - timeLastUpshifted = 0; - timeLastDownshifted = 0; - - // If the map has the shifting piston, instantiate it. - if (map.hasShifter()) { - this.shifter = new MappedDoubleSolenoid(map.getShifter()); + * @param leftMaster The master talon on the left side of the drive. + * @param rightMaster The master talon on the right side of the drive. + * @param MPHandler The motion profile handler that runs this drive's motion profiles. + * @param PIDScale The amount to scale the output to the PID loop by. Defaults to 1. + */ + @JsonCreator + public TalonClusterDrive(@NotNull @JsonProperty(required = true) RotPerSecCANTalonSRX leftMaster, + @NotNull @JsonProperty(required = true) RotPerSecCANTalonSRX rightMaster, + @NotNull @JsonProperty(required = true) MappedAHRS navX, + @NotNull @JsonProperty(required = true) CANTalonMPHandler MPHandler, + @Nullable Double PIDScale) { + super(); + //Initialize stuff + if (PIDScale == null) { + PIDScale = 1.; } + PID_SCALE = PIDScale; + this.rightMaster = rightMaster; + this.leftMaster = leftMaster; + this.mpHandler = MPHandler; - //Set up the MP handler. - mpHandler = new CANTalonMPHandler(new RotPerSecCANTalonSRX[]{leftMaster, rightMaster}, map.getMPUpdateRateSecs(), map.getMinPointsInBottomMPBuffer()); + //We only ever use a NavX on the SPI port because the other ports don't work. + this.navX = navX; } /** @@ -177,33 +97,13 @@ private static double clipToOne(double in) { return Math.min(Math.max(in, -1), 1); } - /** - * A getter for whether we're currently overriding autoshifting. - * - * @return true if overriding, false otherwise. - */ - @Override - public boolean getOverrideAutoshift() { - return overrideAutoshift; - } - - /** - * A setter for overriding the autoshifting. - * - * @param override Whether or not to override autoshifting. - */ - @Override - public void setOverrideAutoshift(boolean override) { - this.overrideAutoshift = override; - } - /** * Sets the left and right wheel speeds as a percent of max voltage, not nearly as precise as PID. * * @param left The left voltage throttle, [-1, 1] * @param right The right voltage throttle, [-1, 1] */ - private void setVBusThrottle(double left, double right) { + protected void setVBusThrottle(double left, double right) { //Set voltage mode throttles leftMaster.setPercentVbus(left); rightMaster.setPercentVbus(-right); //This is negative so PID doesn't have to be. Future people, if your robot goes in circles in voltage mode, this may be why. @@ -215,17 +115,15 @@ private void setVBusThrottle(double left, double right) { * @param left The left PID velocity setpoint as a percent [-1, 1] * @param right The right PID velocity setpoint as a percent [-1, 1] */ - private void setPIDThrottle(double left, double right) { - //If we're not shifting, scale by the max speed in the current gear - if (overrideAutoshift || oi.getFwd() == 0) { + protected void setPIDThrottle(double left, double right) { + //scale by the max speed + if (leftMaster.getMaxSpeed() == null || rightMaster.getMaxSpeed() == null) { + setVBusThrottle(left, right); + System.out.println("You're trying to set PID throttle, but the drive talons don't have PID constants defined. Using voltage control instead."); + } else { leftMaster.setSpeed(PID_SCALE * (left * leftMaster.getMaxSpeed())); rightMaster.setSpeed(PID_SCALE * (right * rightMaster.getMaxSpeed())); } - //If we are shifting, scale by the high gear max speed to make acceleration smoother and faster. - else { - leftMaster.setSpeed(PID_SCALE * (left * leftMaster.getMaxSpeedHG())); - rightMaster.setSpeed(PID_SCALE * (right * rightMaster.getMaxSpeedHG())); - } } /** @@ -241,6 +139,28 @@ public void setOutput(double left, double right) { // setVBusThrottle(left, right); } + /** + * Get the velocity of the left side of the drive. + * + * @return The signed velocity in rotations per second, or null if the drive doesn't have encoders. + */ + @Override + @Nullable + public Double getLeftVel() { + return leftMaster.getSpeed(); + } + + /** + * Get the velocity of the right side of the drive. + * + * @return The signed velocity in rotations per second, or null if the drive doesn't have encoders. + */ + @Override + @Nullable + public Double getRightVel() { + return rightMaster.getSpeed(); + } + /** * Completely stop the robot by setting the voltage to each side to be 0. */ @@ -254,8 +174,8 @@ public void fullStop() { */ @Override public void enableMotors() { - leftMaster.canTalon.enable(); - rightMaster.canTalon.enable(); + leftMaster.getCanTalon().enable(); + rightMaster.getCanTalon().enable(); } /** @@ -283,106 +203,7 @@ public void setDefaultCommandManual(Command defaultCommand) { */ @Override public double getGyroOutput() { - return navx.pidGet(); - } - - /** - * @return The gear this subsystem is currently in. - */ - @Override - public gear getGear() { - return currentGear; - } - - /** - * Shift to a specific gear. - * - * @param gear Which gear to shift to. - */ - @Override - public void setGear(gear gear) { - //If we have a shifter on the robot - if (shifter != null) { - //If we want to downshift - if (gear == ShiftingDrive.gear.LOW) { - //Physically shift gears - shifter.set(DoubleSolenoid.Value.kForward); - //Switch the PID constants - rightMaster.switchToLowGear(); - leftMaster.switchToLowGear(); - //Record the current time - timeLastDownshifted = Robot.currentTimeMillis(); - } else { - //Physically shift gears - shifter.set(DoubleSolenoid.Value.kReverse); - //Switch the PID constants - rightMaster.switchToHighGear(); - leftMaster.switchToHighGear(); - //Record the current time. - timeLastUpshifted = Robot.currentTimeMillis(); - } - //Set logging var - currentGear = gear; - } else { - //Warn the user if they try to shift but didn't define a shifting piston. - Logger.addEvent("You're trying to shift gears, but your drive doesn't have a shifter.", this.getClass()); - } - } - - /** - * @return whether the robot should downshift - */ - private boolean shouldDownshift() { - //We should shift if we're going slower than the downshift speed - boolean okToShift = Math.max(Math.abs(leftMaster.getSpeed()), Math.abs(rightMaster.getSpeed())) < downshiftSpeed; - //Or if we're just turning in place. - okToShift = okToShift || (oi.getFwd() == 0 && oi.getRot() != 0); - //Or commanding a low speed. - okToShift = okToShift || (Math.abs(oi.getFwd()) < upshiftFwdThresh); - //But we can only shift if we're out of the cooldown period. - okToShift = okToShift && Robot.currentTimeMillis() - timeLastUpshifted > cooldownAfterUpshift; - //And there's no need to downshift if we're already in low gear. - okToShift = okToShift && currentGear == gear.HIGH; - //And we don't want to shift if autoshifting is turned off. - okToShift = okToShift && !overrideAutoshift; - - //We use a BufferTimer so we only shift if the conditions are met for a specific continuous interval. - // This avoids brief blips causing shifting. - return downshiftBufferTimer.get(okToShift); - } - - /** - * @return whether the robot should upshift - */ - private boolean shouldUpshift() { - //We should shift if we're going faster than the upshift speed... - boolean okToShift = Math.min(Math.abs(leftMaster.getSpeed()), Math.abs(rightMaster.getSpeed())) > upshiftSpeed; - //AND the driver's trying to go forward fast. - okToShift = okToShift && Math.abs(oi.getFwd()) > upshiftFwdThresh; - //But we can only shift if we're out of the cooldown period. - okToShift = okToShift && Robot.currentTimeMillis() - timeLastDownshifted > cooldownAfterDownshift; - //And there's no need to upshift if we're already in high gear. - okToShift = okToShift && currentGear == gear.LOW; - //And we don't want to shift if autoshifting is turned off. - okToShift = okToShift && !overrideAutoshift; - - //We use a BufferTimer so we only shift if the conditions are met for a specific continuous interval. - // This avoids brief blips causing shifting. - return upshiftBufferTimer.get(okToShift); - } - - /** - * Check if we should autoshift, then, if so, shift. - */ - @Override - public void autoshift() { - if (shouldUpshift()) { - //Upshift if we should - setGear(gear.HIGH); - } else if (shouldDownshift()) { - //Downshift if we should - setGear(gear.LOW); - } + return navX.pidGet(); } /** @@ -411,8 +232,9 @@ public void setOverrideNavX(boolean override) { * @return An AHRS object representing this subsystem's NavX. */ @Override + @NotNull public AHRS getNavX() { - return navx; + return navX; } /** @@ -421,14 +243,16 @@ public AHRS getNavX() { * @return An N-length array of String labels for data, where N is the length of the Object[] returned by getData(). */ @Override + @NotNull + @Contract(pure = true) public String[] getHeader() { - return new String[]{"left_vel,", - "right_vel,", - "left_setpoint,", - "right_setpoint,", - "left_current,", - "right_current,", - "left_voltage,", + return new String[]{"left_vel", + "right_vel", + "left_setpoint", + "right_setpoint", + "left_current", + "right_current", + "left_voltage", "right_voltage"}; } @@ -438,15 +262,16 @@ public String[] getHeader() { * @return An N-length array of Objects, where N is the number of labels given by getHeader. */ @Override + @NotNull public Object[] getData() { return new Object[]{leftMaster.getSpeed(), rightMaster.getSpeed(), leftMaster.getSetpoint(), rightMaster.getSetpoint(), - leftMaster.canTalon.getOutputCurrent(), - rightMaster.canTalon.getOutputCurrent(), - leftMaster.canTalon.getOutputVoltage(), - rightMaster.canTalon.getOutputVoltage()}; + leftMaster.getCanTalon().getOutputCurrent(), + rightMaster.getCanTalon().getOutputCurrent(), + leftMaster.getCanTalon().getOutputVoltage(), + rightMaster.getCanTalon().getOutputVoltage()}; } /** @@ -455,6 +280,8 @@ public Object[] getData() { * @return A string that will identify this object in the log file. */ @Override + @NotNull + @Contract(pure = true) public String getName() { return "Drive"; } @@ -465,7 +292,7 @@ public String getName() { * @param profile The profile to be loaded. */ @Override - public void loadMotionProfile(MotionProfileData profile) { + public void loadMotionProfile(@NotNull MotionProfileData profile) { mpHandler.loadTopLevel(profile); } @@ -528,7 +355,7 @@ public void stopMPProcesses() { * @param right The profile to load into the right side. */ @Override - public void loadMotionProfile(MotionProfileData left, MotionProfileData right) { + public void loadMotionProfile(@NotNull MotionProfileData left, @NotNull MotionProfileData right) { mpHandler.loadIndividualProfiles(new MotionProfileData[]{left, right}); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/JiggleRobot.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/JiggleRobot.java index 24689a84..8f84987e 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/JiggleRobot.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/JiggleRobot.java @@ -1,20 +1,50 @@ package org.usfirst.frc.team449.robot.drive.talonCluster.commands; -import edu.wpi.first.wpilibj.command.CommandGroup; -import maps.org.usfirst.frc.team449.robot.util.ToleranceBufferAnglePIDMap; +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.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; +import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.NavxSubsystem; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Rotates the robot back and forth in order to dislodge any stuck balls. */ -public class JiggleRobot extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class JiggleRobot extends YamlCommandGroupWrapper { + /** * Instantiate the CommandGroup * - * @param subsystem The unidirectionalDrive to execute this command on. + * @param toleranceBuffer How many consecutive loops have to be run while within tolerance to be considered on + * target. Multiply by loop period of ~20 milliseconds for time. Defaults to 0. + * @param absoluteTolerance The maximum number of degrees off from the target at which we can be considered within + * tolerance. + * @param minimumOutput The minimum output of the loop. Defaults to zero. + * @param maximumOutput The maximum output of the loop. Can be null, and if it is, no maximum output is used. + * @param deadband The deadband around the setpoint, in degrees, within which no output is given to the + * motors. Defaults to zero. + * @param inverted Whether the loop is inverted. Defaults to false. + * @param kP Proportional gain. Defaults to zero. + * @param kI Integral gain. Defaults to zero. + * @param kD Derivative gain. Defaults to zero. + * @param subsystem The drive to execute this command on. */ - public JiggleRobot(UnidirectionalDrive subsystem, ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID turnPID) { - addSequential(new NavXRelativeTTA(turnPID, 10, subsystem, 3)); - addSequential(new NavXRelativeTTA(turnPID, -10, subsystem, 3)); + @JsonCreator + public JiggleRobot(@JsonProperty(required = true) double absoluteTolerance, + int toleranceBuffer, + double minimumOutput, @Nullable Double maximumOutput, + double deadband, + boolean inverted, + int kP, + int kI, + int kD, + @NotNull @JsonProperty(required = true) T subsystem) { + addSequential(new NavXRelativeTTA<>(absoluteTolerance, toleranceBuffer, minimumOutput, maximumOutput, deadband, inverted, kP, kI, kD, 10, subsystem, 3)); + addSequential(new NavXRelativeTTA<>(absoluteTolerance, toleranceBuffer, minimumOutput, maximumOutput, deadband, inverted, kP, kI, kD, -10, subsystem, 3)); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXDriveStraight.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXDriveStraight.java index b207e12a..942ad974 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXDriveStraight.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXDriveStraight.java @@ -1,51 +1,79 @@ package org.usfirst.frc.team449.robot.drive.talonCluster.commands; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import maps.org.usfirst.frc.team449.robot.util.ToleranceBufferAnglePIDMap; +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.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; import org.usfirst.frc.team449.robot.interfaces.oi.TankOI; import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.NavxSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.commands.PIDAngleCommand; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Drives straight using the NavX gyro to keep a constant alignment. */ -public class NavXDriveStraight extends PIDAngleCommand { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class NavXDriveStraight extends PIDAngleCommand { /** - * The tank OI to get input from. + * The drive subsystem to give output to. */ - private TankOI oi; + @NotNull + protected final T subsystem; /** - * The drive subsystem to give output to. + * The tank OI to get input from. */ - private UnidirectionalDrive drive; + @NotNull + private final TankOI oi; /** * Whether to use the left joystick to drive straight. */ - private boolean useLeft; + private final boolean useLeft; /** * Default constructor. * - * @param map A map with the PID constants for controlling the angular PID loop. - * @param drive The unidirectional drive to execute this command on. - * @param oi The tank OI to take input from. - * @param useLeft Which joystick to use to get the forward component to drive straight. True for left, false for - * right. + * @param toleranceBuffer How many consecutive loops have to be run while within tolerance to be considered on + * target. Multiply by loop period of ~20 milliseconds for time. Defaults to 0. + * @param absoluteTolerance The maximum number of degrees off from the target at which we can be considered within + * tolerance. + * @param minimumOutput The minimum output of the loop. Defaults to zero. + * @param maximumOutput The maximum output of the loop. Can be null, and if it is, no maximum output is used. + * @param deadband The deadband around the setpoint, in degrees, within which no output is given to the + * motors. Defaults to zero. + * @param inverted Whether the loop is inverted. Defaults to false. + * @param kP Proportional gain. Defaults to zero. + * @param kI Integral gain. Defaults to zero. + * @param kD Derivative gain. Defaults to zero. + * @param subsystem The drive to execute this command on. + * @param oi The tank OI to take input from. + * @param useLeft Which joystick to use to get the forward component to drive straight. True for left, + * false for right. */ - public NavXDriveStraight(ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID map, UnidirectionalDrive drive, - TankOI oi, boolean useLeft) { - super(map, (NavxSubsystem) drive); + @JsonCreator + public NavXDriveStraight(@JsonProperty(required = true) double absoluteTolerance, + int toleranceBuffer, + double minimumOutput, @Nullable Double maximumOutput, + double deadband, + boolean inverted, + int kP, + int kI, + int kD, + @NotNull @JsonProperty(required = true) T subsystem, + @NotNull @JsonProperty(required = true) TankOI oi, + @JsonProperty(required = true) boolean useLeft) { + super(absoluteTolerance, toleranceBuffer, minimumOutput, maximumOutput, deadband, inverted, subsystem, kP, kI, kD); this.oi = oi; - this.drive = drive; + this.subsystem = subsystem; this.useLeft = useLeft; //This is likely to need to interrupt the DefaultCommand and therefore should require its subsystem. - requires((Subsystem) drive); + requires(subsystem); } /** @@ -58,9 +86,6 @@ protected void usePIDOutput(double output) { //Process the PID output with deadband, minimum output, etc. output = processPIDOutput(output); - //Log processed output. - SmartDashboard.putNumber("NavXDriveStraight PID output", output); - //Set throttle to the specified stick. double throttle; if (useLeft) { @@ -70,7 +95,7 @@ protected void usePIDOutput(double output) { } //Set the output to the throttle velocity adjusted by the PID output. - drive.setOutput(throttle - output, throttle + output); + subsystem.setOutput(throttle - output, throttle + output); } /** @@ -83,17 +108,9 @@ protected void initialize() { } /** - * Does nothing, the actual work is done in usePIDOutput. - */ - @Override - protected void execute() { - //nada. - } - - /** - * Finishes instantaneously. + * Never finishes. * - * @return true + * @return false */ @Override protected boolean isFinished() { diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXRelativeTTA.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXRelativeTTA.java index d3e92a63..6773e3d8 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXRelativeTTA.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXRelativeTTA.java @@ -1,28 +1,56 @@ package org.usfirst.frc.team449.robot.drive.talonCluster.commands; -import maps.org.usfirst.frc.team449.robot.util.ToleranceBufferAnglePIDMap; +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.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.Robot; import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.NavxSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Turn a certain number of degrees from the current heading. */ -public class NavXRelativeTTA extends NavXTurnToAngle { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class NavXRelativeTTA extends NavXTurnToAngle { /** * Default constructor. * - * @param map An turnPID map with PID values, an absolute tolerance, and minimum output. - * @param setpoint The setpoint, in degrees from 180 to -180. - * @param drive The drive subsystem to execute this command on. - * @param timeout How long this command is allowed to run for, in seconds. Needed because sometimes floating-point - * errors prevent termination. + * @param toleranceBuffer How many consecutive loops have to be run while within tolerance to be considered + * on target. Multiply by loop period of ~20 milliseconds for time. Defaults to 0. + * @param absoluteTolerance The maximum number of degrees off from the target at which we can be considered within + * tolerance. + * @param minimumOutput The minimum output of the loop. Defaults to zero. + * @param maximumOutput The maximum output of the loop. Can be null, and if it is, no maximum output is used. + * @param deadband The deadband around the setpoint, in degrees, within which no output is given to the + * motors. Defaults to zero. + * @param inverted Whether the loop is inverted. Defaults to false. + * @param kP Proportional gain. Defaults to zero. + * @param kI Integral gain. Defaults to zero. + * @param kD Derivative gain. Defaults to zero. + * @param setpoint The setpoint, in degrees from 180 to -180. + * @param drive The drive subsystem to execute this command on. + * @param timeout How long this command is allowed to run for, in seconds. Needed because sometimes + * floating-point errors prevent termination. */ - public NavXRelativeTTA(ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID map, double setpoint, UnidirectionalDrive drive, - double timeout) { - super(map, setpoint, drive, timeout); + @JsonCreator + public NavXRelativeTTA(@JsonProperty(required = true) double absoluteTolerance, + int toleranceBuffer, + double minimumOutput, @Nullable Double maximumOutput, + double deadband, + boolean inverted, + int kP, + int kI, + int kD, + @JsonProperty(required = true) double setpoint, + @NotNull @JsonProperty(required = true) T drive, + @JsonProperty(required = true) double timeout) { + super(absoluteTolerance, toleranceBuffer, minimumOutput, maximumOutput, deadband, inverted, kP, kI, kD, setpoint, drive, timeout); } /** @@ -34,7 +62,7 @@ protected void initialize() { this.startTime = Robot.currentTimeMillis(); Logger.addEvent("NavXRelativeTurnToAngle init.", this.getClass()); //Do math to setup the setpoint. - this.setSetpoint(clipTo180(((NavxSubsystem) drive).getGyroOutput() + setpoint)); + this.setSetpoint(clipTo180(((NavxSubsystem) subsystem).getGyroOutput() + setpoint)); //Make sure to enable the controller! this.getPIDController().enable(); } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXTurnToAngle.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXTurnToAngle.java index 03335ce8..23d295ea 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXTurnToAngle.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/NavXTurnToAngle.java @@ -1,56 +1,84 @@ package org.usfirst.frc.team449.robot.drive.talonCluster.commands; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import maps.org.usfirst.frc.team449.robot.util.ToleranceBufferAnglePIDMap; +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.Contract; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.Robot; import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.NavxSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.commands.PIDAngleCommand; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Turns to a specified angle, relative to the angle the NavX was at when the robot was turned on. */ -public class NavXTurnToAngle extends PIDAngleCommand { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class NavXTurnToAngle extends PIDAngleCommand { /** * The drive subsystem to execute this command on and to get the gyro reading from. */ - protected UnidirectionalDrive drive; + @NotNull + protected final T subsystem; /** * The angle to turn to. */ - protected double setpoint; + protected final double setpoint; /** - * The time this command was initiated + * How long this command is allowed to run for (in milliseconds) */ - protected long startTime; + private final long timeout; /** - * How long this command is allowed to run for (in milliseconds) + * The time this command was initiated */ - private long timeout; + protected long startTime; /** * Default constructor. * - * @param map A map with PID values, an absolute tolerance, and minimum output. - * @param setpoint The setpoint, in degrees from 180 to -180. - * @param drive The drive subsystem to execute this command on. Must also be a NavX subsystem. - * @param timeout How long this command is allowed to run for, in seconds. Needed because sometimes floating-point - * errors prevent termination. + * @param toleranceBuffer How many consecutive loops have to be run while within tolerance to be considered on + * target. Multiply by loop period of ~20 milliseconds for time. Defaults to 0. + * @param absoluteTolerance The maximum number of degrees off from the target at which we can be considered within + * tolerance. + * @param minimumOutput The minimum output of the loop. Defaults to zero. + * @param maximumOutput The maximum output of the loop. Can be null, and if it is, no maximum output is used. + * @param deadband The deadband around the setpoint, in degrees, within which no output is given to the + * motors. Defaults to zero. + * @param inverted Whether the loop is inverted. Defaults to false. + * @param kP Proportional gain. Defaults to zero. + * @param kI Integral gain. Defaults to zero. + * @param kD Derivative gain. Defaults to zero. + * @param setpoint The setpoint, in degrees from 180 to -180. + * @param subsystem The drive subsystem to execute this command on. + * @param timeout How long this command is allowed to run for, in seconds. Needed because sometimes + * floating-point errors prevent termination. */ - public NavXTurnToAngle(ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID map, double setpoint, UnidirectionalDrive drive, - double timeout) { - super(map, (NavxSubsystem) drive); - this.drive = drive; + @JsonCreator + public NavXTurnToAngle(@JsonProperty(required = true) double absoluteTolerance, + int toleranceBuffer, + double minimumOutput, @Nullable Double maximumOutput, + double deadband, + boolean inverted, + int kP, + int kI, + int kD, + @JsonProperty(required = true) double setpoint, + @NotNull @JsonProperty(required = true) T subsystem, + @JsonProperty(required = true) double timeout) { + super(absoluteTolerance, toleranceBuffer, minimumOutput, maximumOutput, deadband, inverted, subsystem, kP, kI, kD); + this.subsystem = subsystem; this.setpoint = setpoint; //Convert from seconds to milliseconds this.timeout = (long) (timeout * 1000); - requires((Subsystem) drive); + requires(subsystem); } /** @@ -59,6 +87,7 @@ public NavXTurnToAngle(ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID map, d * @param theta The angle to clip, in degrees. * @return The equivalent of that number, clipped to be between -180 and 180. */ + @Contract(pure = true) protected static double clipTo180(double theta) { return (theta + 180) % 360 - 180; } @@ -70,17 +99,11 @@ protected static double clipTo180(double theta) { */ @Override protected void usePIDOutput(double output) { - //Logging - SmartDashboard.putNumber("Preprocessed output", output); - SmartDashboard.putNumber("NavX Turn To Angle Setpoint", getSetpoint()); - //Process the output with deadband, minimum output, etc. output = processPIDOutput(output); - //More logging - SmartDashboard.putNumber("NavXTurnToAngle PID loop output", output); - - drive.setOutput(-output, output); //spin to the right angle + //spin to the right angle + subsystem.setOutput(-output, output); } /** @@ -95,15 +118,6 @@ protected void initialize() { this.getPIDController().enable(); } - /** - * Log data to SmartDashboard. - */ - @Override - protected void execute() { - SmartDashboard.putBoolean("onTarget", this.getPIDController().onTarget()); - SmartDashboard.putNumber("Avg Navx Error", this.getPIDController().getAvgError()); - } - /** * Exit when the robot reaches the setpoint or enough time has passed. * @@ -111,7 +125,7 @@ protected void execute() { */ @Override protected boolean isFinished() { - //The PIDController onTarget() is crap and sometimes never terminates because of floating point errors, so we need a timeout + //The PIDController onTarget() is crap and sometimes never returns true because of floating point errors, so we need a timeout return this.getPIDController().onTarget() || Robot.currentTimeMillis() - startTime > timeout; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ShiftingUnidirectionalNavXArcadeDrive.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ShiftingUnidirectionalNavXArcadeDrive.java deleted file mode 100644 index e59965fd..00000000 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ShiftingUnidirectionalNavXArcadeDrive.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.usfirst.frc.team449.robot.drive.talonCluster.commands; - -import maps.org.usfirst.frc.team449.robot.util.ToleranceBufferAnglePIDMap; -import org.usfirst.frc.team449.robot.interfaces.drive.shifting.ShiftingDrive; -import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; -import org.usfirst.frc.team449.robot.interfaces.oi.ArcadeOI; - -/** - * Drive with arcade drive setup, autoshift, and when the driver isn't turning, use a NavX to stabilize the robot's - * alignment. - */ -public class ShiftingUnidirectionalNavXArcadeDrive extends UnidirectionalNavXArcadeDrive { - /** - * Default constructor - * - * @param map The angle PID map containing PID and other tuning constants. - * @param drive The drive to execute this command on. Must also be a NavXSubsystem and a ShiftingDrive. - * @param oi The OI controlling the robot. - */ - public ShiftingUnidirectionalNavXArcadeDrive(ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID map, UnidirectionalDrive drive, ArcadeOI oi) { - super(map, drive, oi); - } - - /** - * Autoshift, decide whether or not we should be in free drive or straight drive, and log data. - */ - @Override - public void execute() { - //Auto-shifting - ((ShiftingDrive) driveSubsystem).autoshift(); - super.execute(); - } -} 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 new file mode 100644 index 00000000..99c728ed --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/ShiftingUnidirectionalNavXDefaultDrive.java @@ -0,0 +1,106 @@ +package org.usfirst.frc.team449.robot.drive.talonCluster.commands; + +import com.fasterxml.jackson.annotation.*; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; +import org.usfirst.frc.team449.robot.interfaces.drive.shifting.ShiftingDrive; +import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; +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.Logger; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; + +/** + * Drive with arcade drive setup, autoshift, and when the driver isn't turning, use a NavX to stabilize the robot's + * alignment. + */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ShiftingUnidirectionalNavXDefaultDrive extends UnidirectionalNavXDefaultDrive { + + /** + * The drive to execute this command on. + */ + @NotNull + protected final T subsystem; + + /** + * The helper object for autoshifting. + */ + @NotNull + protected final AutoshiftProcessor autoshiftProcessor; + + /** + * Default constructor + * + * @param toleranceBuffer How many consecutive loops have to be run while within tolerance to be + * considered + * on target. Multiply by loop period of ~20 milliseconds for time. Defaults to 0. + * @param absoluteTolerance The maximum number of degrees off from the target at which we can be considered + * within tolerance. + * @param minimumOutput The minimum output of the loop. Defaults to zero. + * @param maximumOutput The maximum output of the loop. Can be null, and if it is, no maximum output is + * used. + * @param deadband The deadband around the setpoint, in degrees, within which no output is given to + * the motors. Defaults to zero. + * @param maxAngularVelToEnterLoop The maximum angular velocity, in degrees/sec, at which the loop will be entered. + * Defaults to 180. + * @param inverted Whether the loop is inverted. Defaults to false. + * @param kP Proportional gain. Defaults to zero. + * @param kI Integral gain. Defaults to zero. + * @param kD Derivative gain. Defaults to zero. + * @param loopEntryDelay The delay to enter the loop after conditions for entry are met. Defaults to + * zero. + * @param subsystem The drive to execute this command on. + * @param oi The OI controlling the robot. + * @param autoshiftProcessor The helper object for autoshifting. + */ + @JsonCreator + public ShiftingUnidirectionalNavXDefaultDrive(@JsonProperty(required = true) double absoluteTolerance, + int toleranceBuffer, + double minimumOutput, @Nullable Double maximumOutput, + double deadband, + @Nullable Double maxAngularVelToEnterLoop, + boolean inverted, + int kP, + int kI, + int kD, + double loopEntryDelay, + @NotNull @JsonProperty(required = true) T subsystem, + @NotNull @JsonProperty(required = true) UnidirectionalOI oi, + @NotNull @JsonProperty(required = true) AutoshiftProcessor autoshiftProcessor) { + super(absoluteTolerance, toleranceBuffer, minimumOutput, maximumOutput, deadband, maxAngularVelToEnterLoop, + inverted, kP, kI, kD, loopEntryDelay, subsystem, oi); + this.autoshiftProcessor = autoshiftProcessor; + this.subsystem = subsystem; + } + + /** + * Autoshift, decide whether or not we should be in free drive or straight drive, and log data. + */ + @Override + public void execute() { + //Auto-shifting + autoshiftProcessor.autoshift(oi.getLeftOutput(), oi.getRightOutput(), subsystem.getLeftVel(), + subsystem.getRightVel(), gear -> subsystem.setGear(gear)); + super.execute(); + } + + /** + * Log when this command ends + */ + @Override + protected void end() { + Logger.addEvent("ShiftingUnidirectionalNavXArcadeDrive End.", this.getClass()); + } + + /** + * Stop the motors and log when this command is interrupted. + */ + @Override + protected void interrupted() { + Logger.addEvent("ShiftingUnidirectionalNavXArcadeDrive Interrupted! Stopping the robot.", this.getClass()); + subsystem.fullStop(); + } +} diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/UnidirectionalNavXArcadeDrive.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/UnidirectionalNavXArcadeDrive.java deleted file mode 100644 index 54624bb9..00000000 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/UnidirectionalNavXArcadeDrive.java +++ /dev/null @@ -1,176 +0,0 @@ -package org.usfirst.frc.team449.robot.drive.talonCluster.commands; - -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import maps.org.usfirst.frc.team449.robot.util.ToleranceBufferAnglePIDMap; -import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; -import org.usfirst.frc.team449.robot.interfaces.oi.ArcadeOI; -import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.NavxSubsystem; -import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.commands.PIDAngleCommand; -import org.usfirst.frc.team449.robot.util.BufferTimer; -import org.usfirst.frc.team449.robot.util.Logger; - -/** - * Drive with arcade drive setup, and when the driver isn't turning, use a NavX to stabilize the robot's alignment. - */ -public class UnidirectionalNavXArcadeDrive extends PIDAngleCommand { - /** - * The UnidirectionalDrive this command is controlling. - */ - protected UnidirectionalDrive driveSubsystem; - - /** - * The OI giving the vel and turn stick values. - */ - private ArcadeOI oi; - - /** - * Whether or not we should be using the NavX to drive straight stably. - */ - private boolean drivingStraight; - - /** - * The velocity input from OI. Should be between -1 and 1. - */ - private double vel; - - /** - * The rotation input from OI. Should be between -1 and 1. - */ - private double rot; - - /** - * The maximum velocity for the robot to be at in order to switch to driveStraight, in degrees/sec - */ - private double maxAngularVel; - - /** - * A bufferTimer so we only switch to driving straight when the conditions are met for a certain period of time. - */ - private BufferTimer driveStraightTimer; - - /** - * Default constructor - * - * @param map The angle PID map containing PID and other tuning constants. - * @param drive The drive to execute this command on. Must also be a NavXSubsystem. - * @param oi The OI controlling the robot. - */ - public UnidirectionalNavXArcadeDrive(ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID map, UnidirectionalDrive drive, - ArcadeOI oi) { - //Assign stuff - super(map, (NavxSubsystem) drive); - maxAngularVel = map.getMaxAngularVel(); - this.oi = oi; - driveSubsystem = drive; - - driveStraightTimer = new BufferTimer(map.getDriveStraightDelay()); - - //Needs a requires because it's a default command. - requires((Subsystem) drive); - - //Logging, but in Spanish. - Logger.addEvent("Drive Robot bueno", this.getClass()); - } - - /** - * Initialize PIDController and variables. - */ - @Override - protected void initialize() { - //Reset all values of the PIDController and enable it. - this.getPIDController().reset(); - this.getPIDController().enable(); - Logger.addEvent("UnidirectionalNavXArcadeDrive init.", this.getClass()); - - //Initial assignment - drivingStraight = false; - vel = oi.getFwd(); - rot = oi.getRot(); - } - - /** - * Decide whether or not we should be in free drive or straight drive, and log data. - */ - @Override - protected void execute() { - //Set vel and rot to what they should be. - vel = oi.getFwd(); - rot = oi.getRot(); - - //If we're driving straight but the driver tries to turn or overrides the NavX: - if (drivingStraight && (rot != 0 || ((NavxSubsystem) driveSubsystem).getOverrideNavX())) { - //Switch to free drive - drivingStraight = false; - Logger.addEvent("Switching to free drive.", this.getClass()); - } - //If we're free driving and the driver lets go of the turn stick: - else if (driveStraightTimer.get(!(((NavxSubsystem) driveSubsystem).getOverrideNavX()) && !(drivingStraight) && rot == 0 && Math.abs(((NavxSubsystem) driveSubsystem).getNavX().getRate()) <= maxAngularVel)) { - //Switch to driving straight - drivingStraight = true; - //Set the setpoint to the current heading and reset the NavX - this.getPIDController().reset(); - this.getPIDController().setSetpoint(subsystem.getGyroOutput()); - this.getPIDController().enable(); - Logger.addEvent("Switching to DriveStraight.", this.getClass()); - } - - //Log data and stuff - SmartDashboard.putBoolean("driving straight?", drivingStraight); - SmartDashboard.putBoolean("Override Navx", ((NavxSubsystem) driveSubsystem).getOverrideNavX()); - SmartDashboard.putNumber("Vel Axis", vel); - SmartDashboard.putNumber("Rot axis", rot); - } - - /** - * Run constantly because this is a defaultDrive - * - * @return false - */ - @Override - protected boolean isFinished() { - return false; - } - - /** - * Log when this command ends - */ - @Override - protected void end() { - Logger.addEvent("UnidirectionalNavXArcadeDrive End.", this.getClass()); - } - - /** - * Stop the motors and log when this command is interrupted. - */ - @Override - protected void interrupted() { - Logger.addEvent("UnidirectionalNavXArcadeDrive Interrupted! Stopping the robot.", this.getClass()); - driveSubsystem.setOutput(0.0, 0.0); - } - - /** - * Give the correct output to the motors based on whether we're in free drive or drive straight. - * - * @param output The output of the angular PID loop. - */ - @Override - protected void usePIDOutput(double output) { - //If we're driving straight.. - if (drivingStraight) { - //Process the output (minimumOutput, deadband, etc.) - output = processPIDOutput(output); - - //Log stuff - SmartDashboard.putNumber("PID output", output); - - //Adjust the heading according to the PID output, it'll be positive if we want to go right. - driveSubsystem.setOutput(vel - output, vel + output); - } - //If we're free driving... - else { - //Set the throttle to normal arcade throttle. - driveSubsystem.setOutput(oi.getLeftOutput(), oi.getRightOutput()); - } - } -} \ No newline at end of file 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 new file mode 100644 index 00000000..e6216519 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/drive/talonCluster/commands/UnidirectionalNavXDefaultDrive.java @@ -0,0 +1,197 @@ +package org.usfirst.frc.team449.robot.drive.talonCluster.commands; + +import com.fasterxml.jackson.annotation.*; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; +import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; +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.interfaces.subsystem.NavX.commands.PIDAngleCommand; +import org.usfirst.frc.team449.robot.util.BufferTimer; +import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; + +/** + * Drive with arcade drive setup, and when the driver isn't turning, use a NavX to stabilize the robot's alignment. + */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class UnidirectionalNavXDefaultDrive extends PIDAngleCommand { + + /** + * The drive this command is controlling. + */ + @NotNull + protected final T subsystem; + + /** + * The OI giving the input stick values. + */ + @NotNull + protected final UnidirectionalOI oi; + + /** + * The maximum velocity for the robot to be at in order to switch to driveStraight, in degrees/sec + */ + private final double maxAngularVelToEnterLoop; + + /** + * A bufferTimer so we only switch to driving straight when the conditions are met for a certain period of time. + */ + @NotNull + private final BufferTimer driveStraightLoopEntryTimer; + + /** + * Whether or not we should be using the NavX to drive straight stably. + */ + private boolean drivingStraight; + + /** + * Default constructor + * + * @param toleranceBuffer How many consecutive loops have to be run while within tolerance to be + * considered on target. Multiply by loop period of ~20 milliseconds for time. + * Defaults to 0. + * @param absoluteTolerance The maximum number of degrees off from the target at which we can be + * considered within tolerance. + * @param minimumOutput The minimum output of the loop. Defaults to zero. + * @param maximumOutput The maximum output of the loop. Can be null, and if it is, no maximum output + * is used. + * @param deadband The deadband around the setpoint, in degrees, within which no output is given + * to the motors. Defaults to zero. + * @param maxAngularVelToEnterLoop The maximum angular velocity, in degrees/sec, at which the loop will be + * entered. Defaults to 180. + * @param inverted Whether the loop is inverted. Defaults to false. + * @param kP Proportional gain. Defaults to zero. + * @param kI Integral gain. Defaults to zero. + * @param kD Derivative gain. Defaults to zero. + * @param loopEntryDelay The delay to enter the loop after conditions for entry are met. Defaults to + * zero. + * @param subsystem The drive to execute this command on. + * @param oi The OI controlling the robot. + */ + @JsonCreator + public UnidirectionalNavXDefaultDrive(@JsonProperty(required = true) double absoluteTolerance, + int toleranceBuffer, + double minimumOutput, @Nullable Double maximumOutput, + double deadband, + @Nullable Double maxAngularVelToEnterLoop, + boolean inverted, + int kP, + int kI, + int kD, + double loopEntryDelay, + @NotNull @JsonProperty(required = true) T subsystem, + @NotNull @JsonProperty(required = true) UnidirectionalOI oi) { + //Assign stuff + super(absoluteTolerance, toleranceBuffer, minimumOutput, maximumOutput, deadband, inverted, subsystem, kP, kI, kD); + this.oi = oi; + this.subsystem = subsystem; + + driveStraightLoopEntryTimer = new BufferTimer(loopEntryDelay); + if (maxAngularVelToEnterLoop == null) { + maxAngularVelToEnterLoop = 180.; + } + this.maxAngularVelToEnterLoop = maxAngularVelToEnterLoop; + + //Needs a requires because it's a default command. + requires(this.subsystem); + + //Logging, but in Spanish. + Logger.addEvent("Drive Robot bueno", this.getClass()); + } + + /** + * Initialize PIDController and variables. + */ + @Override + protected void initialize() { + //Reset all values of the PIDController and enable it. + this.getPIDController().reset(); + this.getPIDController().enable(); + Logger.addEvent("UnidirectionalNavXArcadeDrive init.", this.getClass()); + + //Initial assignment + drivingStraight = false; + } + + /** + * Decide whether or not we should be in free drive or straight drive, and log data. + */ + @Override + protected void execute() { + //Check whether we're commanding to drive straight or turn. + boolean commandingStraight = oi.commandingStraight(); + + //If we're driving straight but the driver tries to turn or overrides the NavX: + if (drivingStraight && (!commandingStraight || subsystem.getOverrideNavX())) { + //Switch to free drive + drivingStraight = false; + Logger.addEvent("Switching to free drive.", this.getClass()); + } + //If we're free driving and the driver tries to turn: + else if (driveStraightLoopEntryTimer.get(!(subsystem.getOverrideNavX()) && !(drivingStraight) && + commandingStraight && Math.abs(subsystem.getNavX().getRate()) <= maxAngularVelToEnterLoop)) { + //Switch to driving straight + drivingStraight = true; + //Set the setpoint to the current heading and reset the NavX + this.getPIDController().reset(); + this.getPIDController().setSetpoint(subsystem.getGyroOutput()); + this.getPIDController().enable(); + Logger.addEvent("Switching to DriveStraight.", this.getClass()); + } + + //Log data and stuff + SmartDashboard.putBoolean("driving straight?", drivingStraight); + } + + /** + * Run constantly because this is a defaultDrive + * + * @return false + */ + @Override + protected boolean isFinished() { + return false; + } + + /** + * Log when this command ends + */ + @Override + protected void end() { + Logger.addEvent("UnidirectionalNavXArcadeDrive End.", this.getClass()); + } + + /** + * Stop the motors and log when this command is interrupted. + */ + @Override + protected void interrupted() { + Logger.addEvent("UnidirectionalNavXArcadeDrive Interrupted! Stopping the robot.", this.getClass()); + subsystem.fullStop(); + } + + /** + * Give the correct output to the motors based on whether we're in free drive or drive straight. + * + * @param output The output of the angular PID loop. + */ + @Override + protected void usePIDOutput(double output) { + //If we're driving straight.. + if (drivingStraight) { + //Process the output (minimumOutput, deadband, etc.) + output = processPIDOutput(output); + + //Adjust the heading according to the PID output, it'll be positive if we want to go right. + subsystem.setOutput(oi.getLeftOutput() - output, oi.getRightOutput() + output); + } + //If we're free driving... + else { + //Set the throttle to normal arcade throttle. + subsystem.setOutput(oi.getLeftOutput(), oi.getRightOutput()); + } + } +} \ No newline at end of file diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/DriveSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/DriveSubsystem.java index a419903c..72512b89 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/DriveSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/DriveSubsystem.java @@ -1,9 +1,13 @@ package org.usfirst.frc.team449.robot.interfaces.drive; +import com.fasterxml.jackson.annotation.JsonTypeInfo; + /** * Any locomotion device for the robot. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") public interface DriveSubsystem { + /** * Completely stop the robot by setting the voltage to each side to be 0. */ diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/ShiftingDrive.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/ShiftingDrive.java index bb4a2e68..a0b73268 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/ShiftingDrive.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/ShiftingDrive.java @@ -1,18 +1,18 @@ package org.usfirst.frc.team449.robot.interfaces.drive.shifting; +import com.fasterxml.jackson.annotation.JsonTypeInfo; +import org.jetbrains.annotations.NotNull; + /** * A drive that has a high gear and a low gear and can switch between them. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") public interface ShiftingDrive { - /** - * Check if we should autoshift, then, if so, shift. - */ - void autoshift(); - /** * @return The gear this subsystem is currently in. */ + @NotNull gear getGear(); /** @@ -20,7 +20,7 @@ public interface ShiftingDrive { * * @param gear Which gear to shift to. */ - void setGear(gear gear); + void setGear(@NotNull gear gear); /** * A getter for whether we're currently overriding autoshifting. diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/OverrideAutoShift.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/OverrideAutoShift.java index 8983efb7..bdebb31a 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/OverrideAutoShift.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/OverrideAutoShift.java @@ -1,23 +1,30 @@ package org.usfirst.frc.team449.robot.interfaces.drive.shifting.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.drive.shifting.ShiftingDrive; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Override or unoverride whether we're autoshifting. Used to stay in low gear for pushing matches and more! */ -public class OverrideAutoShift extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class OverrideAutoShift extends YamlCommandWrapper { /** * Whether or not to override. */ - private boolean override; + private final boolean override; /** * The drive subsystem to execute this command on. */ - private ShiftingDrive subsystem; + @NotNull + private final ShiftingDrive subsystem; /** * Default constructor @@ -25,7 +32,9 @@ public class OverrideAutoShift extends Command { * @param drive The drive subsystem to execute this command on. * @param override Whether or not to override autoshifting. */ - public OverrideAutoShift(ShiftingDrive drive, boolean override) { + @JsonCreator + public OverrideAutoShift(@NotNull @JsonProperty(required = true) ShiftingDrive drive, + @JsonProperty(required = true) boolean override) { subsystem = drive; this.override = override; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/ShiftGears.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/ShiftGears.java index 2d64f359..36900397 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/ShiftGears.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/ShiftGears.java @@ -1,25 +1,33 @@ package org.usfirst.frc.team449.robot.interfaces.drive.shifting.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.drive.shifting.ShiftingDrive; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Shifts gears. Basically a "ToggleGear" command. */ -public class ShiftGears extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ShiftGears extends YamlCommandWrapper { /** * The drive to execute this command on */ - private ShiftingDrive subsystem; + @NotNull + private final ShiftingDrive subsystem; /** * Default constructor * * @param subsystem The drive to execute this command on */ - public ShiftGears(ShiftingDrive subsystem) { + @JsonCreator + public ShiftGears(@NotNull @JsonProperty(required = true) ShiftingDrive subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToGear.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToGear.java index b4c74d47..75dd3a6f 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToGear.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToGear.java @@ -1,23 +1,31 @@ package org.usfirst.frc.team449.robot.interfaces.drive.shifting.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.drive.shifting.ShiftingDrive; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Switches to a specified gear. */ -public class SwitchToGear extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class SwitchToGear extends YamlCommandWrapper { /** * The drive to execute this command on. */ - private ShiftingDrive subsystem; + @NotNull + private final ShiftingDrive subsystem; /** * The gear to switch to. */ - private ShiftingDrive.gear switchTo; + @NotNull + private final ShiftingDrive.gear switchTo; /** * Default constructor @@ -25,7 +33,9 @@ public class SwitchToGear extends Command { * @param subsystem The drive to execute this command on * @param switchTo The gear to switch to. */ - public SwitchToGear(ShiftingDrive subsystem, ShiftingDrive.gear switchTo) { + @JsonCreator + public SwitchToGear(@NotNull @JsonProperty(required = true) ShiftingDrive subsystem, + @NotNull @JsonProperty(required = true) ShiftingDrive.gear switchTo) { this.subsystem = subsystem; this.switchTo = switchTo; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToHighGear.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToHighGear.java index 7863b3cc..064142a0 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToHighGear.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToHighGear.java @@ -1,25 +1,33 @@ package org.usfirst.frc.team449.robot.interfaces.drive.shifting.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.drive.shifting.ShiftingDrive; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * A command that switches to high gear. */ -public class SwitchToHighGear extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class SwitchToHighGear extends YamlCommandWrapper { /** * The drive subsystem to execute this command on. */ - private ShiftingDrive subsystem; + @NotNull + private final ShiftingDrive subsystem; /** * Default constructor * * @param subsystem The subsystem to execute this command on */ - public SwitchToHighGear(ShiftingDrive subsystem) { + @JsonCreator + public SwitchToHighGear(@NotNull @JsonProperty(required = true) ShiftingDrive subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToLowGear.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToLowGear.java index b908cc94..b1e15b2a 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToLowGear.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/SwitchToLowGear.java @@ -1,25 +1,33 @@ package org.usfirst.frc.team449.robot.interfaces.drive.shifting.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.drive.shifting.ShiftingDrive; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * A command that switches to low gear. */ -public class SwitchToLowGear extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class SwitchToLowGear extends YamlCommandWrapper { /** * The drive subsystem to execute this command on. */ - private ShiftingDrive subsystem; + @NotNull + private final ShiftingDrive subsystem; /** * Default constructor * * @param subsystem The drive subsystem to execute this command on */ - public SwitchToLowGear(ShiftingDrive subsystem) { + @JsonCreator + public SwitchToLowGear(@NotNull @JsonProperty(required = true) ShiftingDrive subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/ToggleOverrideAutoShift.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/ToggleOverrideAutoShift.java index e59d83fc..8173f803 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/ToggleOverrideAutoShift.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/shifting/commands/ToggleOverrideAutoShift.java @@ -1,25 +1,33 @@ package org.usfirst.frc.team449.robot.interfaces.drive.shifting.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.drive.shifting.ShiftingDrive; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Override or unoverride whether we're autoshifting. Used to stay in low gear for pushing matches and more! */ -public class ToggleOverrideAutoShift extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ToggleOverrideAutoShift extends YamlCommandWrapper { /** * The drive subsystem to execute this command on. */ - private ShiftingDrive subsystem; + @NotNull + private final ShiftingDrive subsystem; /** * Default constructor * * @param drive The drive subsystem to execute this command on. */ - public ToggleOverrideAutoShift(ShiftingDrive drive) { + @JsonCreator + public ToggleOverrideAutoShift(@NotNull @JsonProperty(required = true) ShiftingDrive drive) { subsystem = drive; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/UnidirectionalDrive.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/UnidirectionalDrive.java index 294ce568..01c3326b 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/UnidirectionalDrive.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/UnidirectionalDrive.java @@ -1,11 +1,14 @@ package org.usfirst.frc.team449.robot.interfaces.drive.unidirectional; +import com.fasterxml.jackson.annotation.JsonTypeInfo; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.interfaces.drive.DriveSubsystem; /** * A drive with a left side and a right side. "Unidirectional" because it can only move forwards or backwards, not * sideways. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") public interface UnidirectionalDrive extends DriveSubsystem { /** @@ -15,4 +18,20 @@ public interface UnidirectionalDrive extends DriveSubsystem { * @param right the output for the right side of the drive, from [-1, 1] */ void setOutput(double left, double right); + + /** + * Get the velocity of the left side of the drive. + * + * @return The signed velocity in rotations per second, or null if the drive doesn't have encoders. + */ + @Nullable + Double getLeftVel(); + + /** + * Get the velocity of the right side of the drive. + * + * @return The signed velocity in rotations per second, or null if the drive doesn't have encoders. + */ + @Nullable + Double getRightVel(); } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/DetermineNominalVoltage.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/DetermineNominalVoltage.java new file mode 100644 index 00000000..9f2497a8 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/DetermineNominalVoltage.java @@ -0,0 +1,101 @@ +package org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.commands; + +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.interfaces.drive.unidirectional.UnidirectionalDrive; +import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; + +/** + * Run the motors until they move, slowly increasing the voltage up from 0. + */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class DetermineNominalVoltage extends YamlCommandWrapper { + + /** + * The drive subsystem to execute this command on. + */ + @NotNull + private final T subsystem; + + /** + * The current percent of max output commanded, on [0, 1]. + */ + private double percentCommanded; + + /** + * The minimum speed, in RPS, at which the drive is considered to be moving. + */ + private double minSpeed; + + /** + * Default constructor + * + * @param subsystem The subsystem to execute this command on + * @param minSpeed The minimum speed, in RPS, at which the drive is considered to be moving. + */ + @JsonCreator + public DetermineNominalVoltage(@NotNull @JsonProperty(required = true) T subsystem, + @JsonProperty(required = true) double minSpeed) { + //Initialize stuff + this.subsystem = subsystem; + this.minSpeed = minSpeed; + requires(subsystem); + } + + /** + * Log initialization + */ + @Override + protected void initialize() { + percentCommanded = 0; + //Reset drive velocity (for safety reasons) + subsystem.fullStop(); + Logger.addEvent("DetermineNominalVoltage init", this.getClass()); + } + + /** + * Send output to motors + */ + @Override + protected void execute() { + //Adjust it by 0.01 per second, so 0.01 * 20/1000, which is 0.0002 + percentCommanded += 0.0002; + //Set the velocity + subsystem.setOutput(percentCommanded, percentCommanded); + } + + /** + * Exit if the motors are moving. + * + * @return True if the motors are moving, false otherwise. + */ + @Override + protected boolean isFinished() { + return Math.max(Math.abs(subsystem.getLeftVel()), Math.abs(subsystem.getRightVel())) >= minSpeed; + } + + /** + * Stop the drive and log when the command ends. + */ + @Override + protected void end() { + //Brake on exit. Yes this should be setOutput because often we'll be testing how well the PID loop handles a full stop. + subsystem.fullStop(); + Logger.addEvent("DetermineNominalVoltage end.", this.getClass()); + } + + /** + * Log and stop the drive when the command is interrupted. + */ + @Override + protected void interrupted() { + Logger.addEvent("DetermineNominalVoltage Interrupted! Stopping the robot.", this.getClass()); + //Brake if we're interrupted + subsystem.fullStop(); + } +} diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/DriveAtSpeed.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/DriveAtSpeed.java index db10ce1b..4a60b74b 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/DriveAtSpeed.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/DriveAtSpeed.java @@ -1,47 +1,59 @@ package org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.commands; -import edu.wpi.first.wpilibj.command.Command; +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.Robot; import org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.UnidirectionalDrive; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** - * Go at a certain speed for a set number of seconds + * Go at a certain velocity for a set number of seconds */ -public class DriveAtSpeed extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class DriveAtSpeed extends YamlCommandWrapper { /** * Speed to go at */ - private double speed; + private final double velocity; /** * How long to run for */ - private double seconds; + private final double seconds; /** - * When this command was initialized. + * The drive subsystem to execute this command on. */ - private long startTime; + @NotNull + private final T subsystem; /** - * The drive subsystem to execute this command on. + * When this command was initialized. */ - private UnidirectionalDrive subsystem; + private long startTime; /** * Default constructor * - * @param drive The drive to execute this command on - * @param speed How fast to go, in RPS - * @param seconds How long to drive for. + * @param subsystem The drive to execute this command on + * @param velocity How fast to go, in RPS + * @param seconds How long to drive for. */ - public DriveAtSpeed(UnidirectionalDrive drive, double speed, double seconds) { + @JsonCreator + public DriveAtSpeed(@NotNull @JsonProperty(required = true) T subsystem, + @JsonProperty(required = true) double velocity, + @JsonProperty(required = true) double seconds) { //Initialize stuff - this.subsystem = drive; - this.speed = speed; + this.subsystem = subsystem; + this.velocity = velocity; this.seconds = seconds; + requires(subsystem); Logger.addEvent("Drive Robot bueno", this.getClass()); } @@ -52,7 +64,7 @@ public DriveAtSpeed(UnidirectionalDrive drive, double speed, double seconds) { protected void initialize() { //Set up start time startTime = Robot.currentTimeMillis(); - //Reset drive speed (for safety reasons) + //Reset drive velocity (for safety reasons) subsystem.fullStop(); Logger.addEvent("DriveAtSpeed init", this.getClass()); } @@ -62,8 +74,8 @@ protected void initialize() { */ @Override protected void execute() { - //Set the speed - subsystem.setOutput(speed, speed); + //Set the velocity + subsystem.setOutput(velocity, velocity); } /** diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/DriveStraight.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/DriveStraight.java index cd2507ca..2ad7a075 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/DriveStraight.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/DriveStraight.java @@ -1,48 +1,59 @@ package org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.commands; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Subsystem; +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.interfaces.drive.unidirectional.UnidirectionalDrive; import org.usfirst.frc.team449.robot.interfaces.oi.TankOI; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Drives straight when using a tank drive. */ -public class DriveStraight extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class DriveStraight extends YamlCommandWrapper { /** * The oi that this command gets input from. */ - private TankOI oi; + @NotNull + private final TankOI oi; /** * Whether to use the left or right joystick for the forward velocity. */ - private boolean useLeft; + private final boolean useLeft; /** - * The throttle gotten from the joystick. This is a field instead of a local variable to avoid garbage collection. + * The drive subsystem to execute this command on. */ - private double throttle; + @NotNull + private final T subsystem; /** - * The drive subsystem to execute this command on. + * The throttle gotten from the joystick. This is a field instead of a local variable to avoid garbage collection. */ - private UnidirectionalDrive subsystem; + private double throttle; /** * Drive straight without NavX stabilization. * - * @param drive The drive subsystem to execute this command on. - * @param oi The oi to get input from. - * @param useLeft true to use the left stick to drive straight, false to use the right. + * @param subsystem The drive subsystem to execute this command on. + * @param oi The oi to get input from. + * @param useLeft true to use the left stick to drive straight, false to use the right. */ - public DriveStraight(UnidirectionalDrive drive, TankOI oi, boolean useLeft) { - subsystem = drive; + @JsonCreator + public DriveStraight(@NotNull @JsonProperty(required = true) T subsystem, + @NotNull @JsonProperty(required = true) TankOI oi, + @JsonProperty(required = true) boolean useLeft) { + this.subsystem = subsystem; this.oi = oi; this.useLeft = useLeft; - requires((Subsystem) subsystem); + requires(subsystem); Logger.addEvent("Drive Robot bueno", this.getClass()); } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/PIDBackAndForth.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/PIDBackAndForth.java index 13f36103..23b17086 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/PIDBackAndForth.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/PIDBackAndForth.java @@ -1,23 +1,34 @@ package org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.commands; -import edu.wpi.first.wpilibj.command.CommandGroup; +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.interfaces.drive.unidirectional.UnidirectionalDrive; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Drive back and forth to tune PID. */ -public class PIDBackAndForth extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class PIDBackAndForth extends YamlCommandGroupWrapper { + /** * Instantiate the CommandGroup * - * @param subsystem the unidirectional drive to execute this command on. + * @param subsystem the drive to execute this command on. + * @param speed The speed to drive forwards and backwards at, from [0, 1]. + * @param timeInSecs How long to drive in each direction for, in seconds. */ - public PIDBackAndForth(UnidirectionalDrive subsystem) { - double time = 1.5; - + @JsonCreator + public PIDBackAndForth(@NotNull @JsonProperty(required = true) T subsystem, + @JsonProperty(required = true) double speed, + @JsonProperty(required = true) double timeInSecs) { //Drive forwards - addSequential(new DriveAtSpeed(subsystem, 0.7, time)); + addSequential(new DriveAtSpeed<>(subsystem, speed, timeInSecs)); //Drive backwards - addSequential(new DriveAtSpeed(subsystem, -0.7, time)); + addSequential(new DriveAtSpeed<>(subsystem, -speed, timeInSecs)); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/PIDTest.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/PIDTest.java index 8160182c..f4783f34 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/PIDTest.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/PIDTest.java @@ -1,24 +1,34 @@ package org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.commands; -import edu.wpi.first.wpilibj.command.CommandGroup; +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.interfaces.drive.unidirectional.UnidirectionalDrive; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Drive forward at constant speed then stop to tune PID. */ -public class PIDTest extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class PIDTest extends YamlCommandGroupWrapper { /** * Default constructor * - * @param subsystem the UnidirectionalDrive to execute this command on + * @param subsystem the subsystem to execute this command on * @param driveTime How long to drive forwards for, in seconds. + * @param speed The speed to drive at, from [0, 1]. */ - public PIDTest(UnidirectionalDrive subsystem, double driveTime) { - + @JsonCreator + public PIDTest(@NotNull @JsonProperty(required = true) T subsystem, + @JsonProperty(required = true) double driveTime, + @JsonProperty(required = true) double speed) { //Drive forward for a bit - addSequential(new DriveAtSpeed(subsystem, 0.7, driveTime)); - //Stop - addSequential(new DriveAtSpeed(subsystem, 0, 100)); + addSequential(new DriveAtSpeed<>(subsystem, speed, driveTime)); + //Stop actively to see how the PID responds. + addSequential(new DriveAtSpeed<>(subsystem, 0, 100)); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/SimpleUnidirectionalDrive.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/SimpleUnidirectionalDrive.java index a17286fd..e3bf5edd 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/SimpleUnidirectionalDrive.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/drive/unidirectional/commands/SimpleUnidirectionalDrive.java @@ -1,37 +1,47 @@ package org.usfirst.frc.team449.robot.interfaces.drive.unidirectional.commands; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Subsystem; +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.interfaces.drive.unidirectional.UnidirectionalDrive; import org.usfirst.frc.team449.robot.interfaces.oi.UnidirectionalOI; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Very simple unidirectional drive control. */ -public class SimpleUnidirectionalDrive extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class SimpleUnidirectionalDrive extends YamlCommandWrapper { /** * The OI used for input. */ - public UnidirectionalOI oi; + @NotNull + public final UnidirectionalOI oi; /** - * The drive subsystem to execute this command on. + * The subsystem to execute this command on. */ - private UnidirectionalDrive subsystem; + @NotNull + private final T subsystem; /** * Default constructor * - * @param drive The drive to execute this command on - * @param oi The OI that gives the input to this command. + * @param subsystem The subsystem to execute this command on + * @param oi The OI that gives the input to this command. */ - public SimpleUnidirectionalDrive(UnidirectionalDrive drive, UnidirectionalOI oi) { + @JsonCreator + public SimpleUnidirectionalDrive(@NotNull @JsonProperty(required = true) T subsystem, + @NotNull @JsonProperty(required = true) UnidirectionalOI oi) { this.oi = oi; - this.subsystem = drive; + this.subsystem = subsystem; //Default commands need to require their subsystems. - requires((Subsystem) drive); + requires(subsystem); } /** diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/ArcadeOI.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/ArcadeOI.java index de51e4fd..3444664a 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/ArcadeOI.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/ArcadeOI.java @@ -4,21 +4,21 @@ * An arcade-style dual joystick OI. */ public abstract class ArcadeOI implements UnidirectionalOI { + /** + * Get the rotational input. + * * @return rotational velocity component from [-1, 1], where 1 is right and -1 is left. */ public abstract double getRot(); /** + * Get the velocity input. + * * @return forward velocity component from [-1, 1], where 1 is forwards and -1 is backwards */ public abstract double getFwd(); - /** - * Map all buttons to commands. Should only be run after all subsystems have been instantiated. - */ - public abstract void mapButtons(); - /** * The output to be given to the left side of the drive. * @@ -36,4 +36,13 @@ public double getLeftOutput() { public double getRightOutput() { return getFwd() - getRot(); } + + /** + * Whether the driver is trying to drive straight. + * @return True if the driver is trying to drive straight, false otherwise. + */ + @Override + public boolean commandingStraight(){ + return getRot() == 0; + } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/BaseOI.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/BaseOI.java deleted file mode 100644 index c8a7efa4..00000000 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/BaseOI.java +++ /dev/null @@ -1,11 +0,0 @@ -package org.usfirst.frc.team449.robot.interfaces.oi; - -/** - * A basic OI interface that all OI classes should implement. - */ -public interface BaseOI { - /** - * Map all buttons to commands. Should only be run after all subsystems have been instantiated. - */ - void mapButtons(); -} diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/TankOI.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/TankOI.java index 64a7c089..97c6a814 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/TankOI.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/TankOI.java @@ -4,21 +4,21 @@ * A tank-style dual joystick OI. */ public abstract class TankOI implements UnidirectionalOI { + /** + * Get the throttle for the left side of the drive. + * * @return percent of max speed for left motor cluster from [-1.0, 1.0] */ public abstract double getLeftThrottle(); /** + * Get the throttle for the right side of the drive. + * * @return percent of max speed for right motor cluster from [-1.0, 1.0] */ public abstract double getRightThrottle(); - /** - * Map all buttons to commands. Should only be run after all subsystems have been instantiated. - */ - public abstract void mapButtons(); - /** * The output to be given to the left side of the drive. * diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/UnidirectionalOI.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/UnidirectionalOI.java index 0a99eae9..d614b760 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/UnidirectionalOI.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/oi/UnidirectionalOI.java @@ -1,10 +1,13 @@ package org.usfirst.frc.team449.robot.interfaces.oi; +import com.fasterxml.jackson.annotation.JsonTypeInfo; + /** * An OI to control a robot with a unidirectional drive that has a left and right side (e.g. not meccanum, swerve, or * holonomic) */ -public interface UnidirectionalOI extends BaseOI { +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") +public interface UnidirectionalOI { /** * The output to be given to the left side of the drive. @@ -21,7 +24,8 @@ public interface UnidirectionalOI extends BaseOI { double getRightOutput(); /** - * Map all buttons to commands. Should only be run after all subsystems have been instantiated. + * Whether the driver is trying to drive straight. + * @return True if the driver is trying to drive straight, false otherwise. */ - void mapButtons(); + boolean commandingStraight(); } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/IntakeSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/IntakeSubsystem.java index 066e0d5a..1e2ea584 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/IntakeSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/IntakeSubsystem.java @@ -1,14 +1,20 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.Intake; +import com.fasterxml.jackson.annotation.JsonTypeInfo; +import org.jetbrains.annotations.NotNull; + /** * A subsystem used for intaking and possibly ejecting game pieces. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") public interface IntakeSubsystem { + /** * Get the mode of the intake * * @return off, in slow, in fast, out slow, out fast. */ + @NotNull IntakeMode getMode(); /** @@ -16,7 +22,7 @@ public interface IntakeSubsystem { * * @param mode off, in slow, in fast, out slow, out fast. */ - void setMode(IntakeMode mode); + void setMode(@NotNull IntakeMode mode); enum IntakeMode { OFF, IN_SLOW, IN_FAST, OUT_SLOW, OUT_FAST diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/commands/SetIntakeMode.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/commands/SetIntakeMode.java index 779d40c5..cc1db03e 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/commands/SetIntakeMode.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/commands/SetIntakeMode.java @@ -1,23 +1,31 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.Intake.IntakeSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Sets the mode of the intake. */ -public class SetIntakeMode extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class SetIntakeMode extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private IntakeSubsystem subsystem; + @NotNull + private final IntakeSubsystem subsystem; /** * The mode to set this subsystem to. */ - private IntakeSubsystem.IntakeMode mode; + @NotNull + private final IntakeSubsystem.IntakeMode mode; /** * Default constructor @@ -25,7 +33,9 @@ public class SetIntakeMode extends Command { * @param subsystem The subsystem to execute this command on. * @param mode The mode to set the intake to. */ - public SetIntakeMode(IntakeSubsystem subsystem, IntakeSubsystem.IntakeMode mode) { + @JsonCreator + public SetIntakeMode(@NotNull @JsonProperty(required = true) IntakeSubsystem subsystem, + @NotNull @JsonProperty(required = true) IntakeSubsystem.IntakeMode mode) { this.subsystem = subsystem; this.mode = mode; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/commands/ToggleIntaking.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/commands/ToggleIntaking.java index 4131e52f..edb1522a 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/commands/ToggleIntaking.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Intake/commands/ToggleIntaking.java @@ -1,23 +1,31 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.Intake.IntakeSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Toggles whether the subsystem is off or set to a given mode. */ -public class ToggleIntaking extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ToggleIntaking extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private IntakeSubsystem subsystem; + @NotNull + private final IntakeSubsystem subsystem; /** * The mode to set this subsystem to if it's currently off. */ - private IntakeSubsystem.IntakeMode mode; + @NotNull + private final IntakeSubsystem.IntakeMode mode; /** * Default constructor @@ -25,7 +33,9 @@ public class ToggleIntaking extends Command { * @param subsystem The subsystem to execute this command on. * @param mode The mode to set this subsystem to if it's currently off. */ - public ToggleIntaking(IntakeSubsystem subsystem, IntakeSubsystem.IntakeMode mode) { + @JsonCreator + public ToggleIntaking(@NotNull @JsonProperty(required = true) IntakeSubsystem subsystem, + @NotNull @JsonProperty(required = true) IntakeSubsystem.IntakeMode mode) { this.subsystem = subsystem; this.mode = mode; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/MPSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/MPSubsystem.java index 71267a88..a1df69cd 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/MPSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/MPSubsystem.java @@ -1,17 +1,21 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile; +import com.fasterxml.jackson.annotation.JsonTypeInfo; +import org.jetbrains.annotations.NotNull; import org.usfirst.frc.team449.robot.util.MotionProfileData; /** * A subsystem that can have motion profiles run on it. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") public interface MPSubsystem { + /** * Loads a profile into the MP buffer. * * @param profile The profile to be loaded. */ - void loadMotionProfile(MotionProfileData profile); + void loadMotionProfile(@NotNull MotionProfileData profile); /** * Start running the profile that's currently loaded into the MP buffer. diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/TwoSideMPSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/TwoSideMPSubsystem.java index 9d8cdf90..2781ef8d 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/TwoSideMPSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/TwoSideMPSubsystem.java @@ -1,17 +1,21 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem; +import com.fasterxml.jackson.annotation.JsonTypeInfo; +import org.jetbrains.annotations.NotNull; import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.MPSubsystem; import org.usfirst.frc.team449.robot.util.MotionProfileData; /** * An MP subsystem with two sides that therefore needs two profiles at a time. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") public interface TwoSideMPSubsystem extends MPSubsystem { + /** * Loads given profiles into the left and right sides of the drive. * * @param left The profile to load into the left side. * @param right The profile to load into the right side. */ - void loadMotionProfile(MotionProfileData left, MotionProfileData right); + void loadMotionProfile(@NotNull MotionProfileData left, @NotNull MotionProfileData right); } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/commands/LoadProfileTwoSides.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/commands/LoadProfileTwoSides.java index a5c31b41..ee13dcfb 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/commands/LoadProfileTwoSides.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/commands/LoadProfileTwoSides.java @@ -1,24 +1,32 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem.TwoSideMPSubsystem; import org.usfirst.frc.team449.robot.util.Logger; import org.usfirst.frc.team449.robot.util.MotionProfileData; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Loads the given profiles into the subsystem, but doesn't run it. */ -public class LoadProfileTwoSides extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class LoadProfileTwoSides extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private TwoSideMPSubsystem subsystem; + @NotNull + private final TwoSideMPSubsystem subsystem; /** * The motion profiles for the left and right sides to execute, respectively. */ - private MotionProfileData left, right; + @NotNull + private final MotionProfileData left, right; /** * Default constructor @@ -27,7 +35,10 @@ public class LoadProfileTwoSides extends Command { * @param left The profile for the left side to run. * @param right The profile for the right side to run. */ - public LoadProfileTwoSides(TwoSideMPSubsystem subsystem, MotionProfileData left, MotionProfileData right) { + @JsonCreator + public LoadProfileTwoSides(@NotNull @JsonProperty(required = true) TwoSideMPSubsystem subsystem, + @NotNull @JsonProperty(required = true) MotionProfileData left, + @NotNull @JsonProperty(required = true) MotionProfileData right) { this.subsystem = subsystem; this.left = left; this.right = right; diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/commands/RunProfileTwoSides.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/commands/RunProfileTwoSides.java index 34d411b4..b14a970b 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/commands/RunProfileTwoSides.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/TwoSideMPSubsystem/commands/RunProfileTwoSides.java @@ -1,14 +1,21 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem.commands; -import edu.wpi.first.wpilibj.command.CommandGroup; +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.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem.TwoSideMPSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.commands.RunLoadedProfile; import org.usfirst.frc.team449.robot.util.MotionProfileData; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Loads and runs the given profiles into the given subsystem. */ -public class RunProfileTwoSides extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class RunProfileTwoSides extends YamlCommandGroupWrapper { /** * Default constructor. @@ -16,10 +23,14 @@ public class RunProfileTwoSides extends CommandGroup { * @param subsystem The subsystem to execute this command on. * @param left The motion profile for the left side to load and execute. * @param right The motion profile for the right side to load and execute. - * @param timeout The maximum amount of time this command is allowed to take, in seconds.. + * @param timeout The maximum amount of time this command is allowed to take, in seconds. */ - public RunProfileTwoSides(TwoSideMPSubsystem subsystem, MotionProfileData left, MotionProfileData right, double timeout) { + @JsonCreator + public RunProfileTwoSides(@NotNull @JsonProperty(required = true) T subsystem, + @NotNull @JsonProperty(required = true) MotionProfileData left, + @NotNull @JsonProperty(required = true) MotionProfileData right, + @JsonProperty(required = true) double timeout) { addSequential(new LoadProfileTwoSides(subsystem, left, right)); - addSequential(new RunLoadedProfile(subsystem, timeout, true)); + addSequential(new RunLoadedProfile<>(subsystem, timeout, true)); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/LoadProfile.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/LoadProfile.java index 1962e983..9b8dc0ed 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/LoadProfile.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/LoadProfile.java @@ -1,24 +1,32 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.MotionProfile.MPSubsystem; import org.usfirst.frc.team449.robot.util.Logger; import org.usfirst.frc.team449.robot.util.MotionProfileData; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Loads the given profile into the subsystem, but doesn't run it. */ -public class LoadProfile extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class LoadProfile extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private MPSubsystem subsystem; + @NotNull + private final MPSubsystem subsystem; /** * The profile to execute. */ - private MotionProfileData profile; + @NotNull + private final MotionProfileData profile; /** * Default constructor @@ -26,7 +34,9 @@ public class LoadProfile extends Command { * @param subsystem The subsystem to execute this command on. * @param profile The profile to run. */ - public LoadProfile(MPSubsystem subsystem, MotionProfileData profile) { + @JsonCreator + public LoadProfile(@NotNull @JsonProperty(required = true) MPSubsystem subsystem, + @NotNull @JsonProperty(required = true) MotionProfileData profile) { this.subsystem = subsystem; this.profile = profile; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/RunLoadedProfile.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/RunLoadedProfile.java index 3abd4b4f..1cba35ef 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/RunLoadedProfile.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/RunLoadedProfile.java @@ -1,33 +1,40 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.commands; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Subsystem; +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.Robot; import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.MPSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Runs the command that is currently loaded in the given subsystem. */ -public class RunLoadedProfile extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class RunLoadedProfile extends YamlCommandWrapper { /** * The amount of time this command is allowed to run for, in milliseconds. */ - private long timeout; + private final long timeout; /** - * The time this command started running at. + * The subsystem to execute this command on. */ - private long startTime; + @NotNull + private final MPSubsystem subsystem; /** - * The subsystem to execute this command on. + * The time this command started running at. */ - private MPSubsystem subsystem; + private long startTime; /** - * Whether or not we're currently running the profile. + * Whether we're running a profile or waiting for the bottom-level buffer to fill. */ private boolean runningProfile; @@ -39,11 +46,14 @@ public class RunLoadedProfile extends Command { * @param timeout The max amount of time this subsystem is allowed to run for, in seconds. * @param require Whether or not to require the subsystem this command is running on. */ - public RunLoadedProfile(MPSubsystem subsystem, double timeout, boolean require) { + @JsonCreator + public RunLoadedProfile(@NotNull @JsonProperty(required = true) T subsystem, + @JsonProperty(required = true) double timeout, + @JsonProperty(required = true) boolean require) { this.subsystem = subsystem; //Require if specified. if (require) { - requires((Subsystem) subsystem); + requires(subsystem); } //Convert to milliseconds. @@ -59,7 +69,7 @@ public RunLoadedProfile(MPSubsystem subsystem, double timeout, boolean require) protected void initialize() { //Record the start time. startTime = Robot.currentTimeMillis(); - + Logger.addEvent("RunLoadedProfile init", this.getClass()); runningProfile = false; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/RunProfile.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/RunProfile.java index 1650a687..3c2d0cab 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/RunProfile.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/MotionProfile/commands/RunProfile.java @@ -1,13 +1,20 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.commands; -import edu.wpi.first.wpilibj.command.CommandGroup; -import org.usfirst.frc.team449.robot.interfaces.subsystem.MotionProfile.MPSubsystem; +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.interfaces.subsystem.MotionProfile.TwoSideMPSubsystem.TwoSideMPSubsystem; import org.usfirst.frc.team449.robot.util.MotionProfileData; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Loads and runs the given profile into the given subsystem. */ -public class RunProfile extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class RunProfile extends YamlCommandGroupWrapper { /** * Default constructor. @@ -16,8 +23,11 @@ public class RunProfile extends CommandGroup { * @param profile The motion profile to load and execute. * @param timeout The maximum amount of time this command is allowed to take, in seconds. */ - public RunProfile(MPSubsystem subsystem, MotionProfileData profile, double timeout) { + @JsonCreator + public RunProfile(@NotNull @JsonProperty(required = true) T subsystem, + @NotNull @JsonProperty(required = true) MotionProfileData profile, + @JsonProperty(required = true) double timeout) { addSequential(new LoadProfile(subsystem, profile)); - addSequential(new RunLoadedProfile(subsystem, timeout, true)); + addSequential(new RunLoadedProfile<>(subsystem, timeout, true)); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/NavxSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/NavxSubsystem.java index dd5c2d03..7c02a605 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/NavxSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/NavxSubsystem.java @@ -1,12 +1,16 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.NavX; +import com.fasterxml.jackson.annotation.JsonTypeInfo; import com.kauailabs.navx.frc.AHRS; +import org.jetbrains.annotations.NotNull; /** * A subsystem that has a NavX on it. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT) public interface NavxSubsystem { + /** * Get the output of the NavX * @@ -33,5 +37,6 @@ public interface NavxSubsystem { * * @return An AHRS object representing this subsystem's NavX. */ + @NotNull AHRS getNavX(); } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/OverrideNavX.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/OverrideNavX.java index d638d07b..180b6567 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/OverrideNavX.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/OverrideNavX.java @@ -1,23 +1,30 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.NavX.NavxSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Set whether or not to override the NavX. */ -public class OverrideNavX extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class OverrideNavX extends YamlCommandWrapper { /** * Whether or not to override the NavX. */ - private boolean override; + private final boolean override; /** * The subsystem to execute this command on. */ - private NavxSubsystem subsystem; + @NotNull + private final NavxSubsystem subsystem; /** * Default constructor. @@ -25,7 +32,9 @@ public class OverrideNavX extends Command { * @param subsystem The subsystem to execute this command on * @param override Whether or not to override the NavX. */ - public OverrideNavX(NavxSubsystem subsystem, boolean override) { + @JsonCreator + public OverrideNavX(@NotNull @JsonProperty(required = true) NavxSubsystem subsystem, + @JsonProperty(required = true) boolean override) { this.override = override; this.subsystem = 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 5d77b925..c9f72037 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 @@ -1,49 +1,73 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.commands; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.annotation.JsonTypeInfo; +import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.PIDCommand; import edu.wpi.first.wpilibj.command.Scheduler; -import maps.org.usfirst.frc.team449.robot.util.ToleranceBufferAnglePIDMap; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.NavxSubsystem; +import org.usfirst.frc.team449.robot.util.YamlCommand; /** * A command that uses a NavX to turn to a certain angle. */ -public abstract class PIDAngleCommand extends PIDCommand { +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") +public abstract class PIDAngleCommand extends PIDCommand implements YamlCommand { /** - * The minimum the robot should be able to output, to overcome friction. - */ - protected double minimumOutput; - - /** - * Whether or not to use minimumOutput. + * The subsystem to execute this command on. */ - protected boolean minimumOutputEnabled; + @NotNull + protected final NavxSubsystem subsystem; /** - * The subsystem to execute this command on. + * The minimum the robot should be able to output, to overcome friction. */ - protected NavxSubsystem subsystem; + private final double minimumOutput; /** * The range in which output is turned off to prevent "dancing" around the setpoint. */ - protected double deadband; + private final double deadband; /** * Whether or not the loop is inverted. */ - protected boolean inverted; + private final boolean inverted; /** * Default constructor. * - * @param map The map with this command's constants. - * @param subsystem The NavX subsystem. + * @param absoluteTolerance The maximum number of degrees off from the target at which we can be considered + * within tolerance. + * @param toleranceBuffer How many consecutive loops have to be run while within tolerance to be considered + * on target. Multiply by loop period of ~20 milliseconds for time. Defaults to 0. + * @param minimumOutput The minimum output of the loop. Defaults to zero. + * @param maximumOutput The maximum output of the loop. Can be null, and if it is, no maximum output is + * used. + * @param deadband The deadband around the setpoint, in degrees, within which no output is given to + * the motors. Defaults to zero. + * @param inverted Whether the loop is inverted. Defaults to false. + * @param kP Proportional gain. Defaults to zero. + * @param kI Integral gain. Defaults to zero. + * @param kD Derivative gain. Defaults to zero. + * @param subsystem The subsystem to execute this command on. */ - public PIDAngleCommand(ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID map, NavxSubsystem subsystem) { + @JsonCreator + public PIDAngleCommand(@JsonProperty(required = true) double absoluteTolerance, + int toleranceBuffer, + double minimumOutput, @Nullable Double maximumOutput, + double deadband, + boolean inverted, + @NotNull @JsonProperty(required = true) NavxSubsystem subsystem, + int kP, + int kI, + int kD) { //Set P, I and D. I and D will normally be 0 if you're using cascading control, like you should be. - super(map.getPID().getP(), map.getPID().getI(), map.getPID().getD()); + super(kP, kI, kD); this.subsystem = subsystem; //Navx reads from -180 to 180. @@ -53,33 +77,26 @@ public PIDAngleCommand(ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID map, N this.getPIDController().setContinuous(true); //Set the absolute tolerance to be considered on target within. - this.getPIDController().setAbsoluteTolerance(map.getAbsoluteTolerance()); + this.getPIDController().setAbsoluteTolerance(absoluteTolerance); //This is how long we have to be within the tolerance band. Multiply by loop period for time in ms. - this.getPIDController().setToleranceBuffer(map.getToleranceBuffer()); + this.getPIDController().setToleranceBuffer(toleranceBuffer); //Minimum output, the smallest output it's possible to give. One-tenth of your drive's top speed is about // right. - //TODO test and implement that Talon nominalOutputVoltage and then get rid of this. - minimumOutput = map.getMinimumOutput(); - minimumOutputEnabled = map.getMinimumOutputEnabled(); + this.minimumOutput = minimumOutput; //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 (map.getMaximumOutputEnabled()) { - this.getPIDController().setOutputRange(-map.getMaximumOutput(), map.getMaximumOutput()); + if (maximumOutput != null) { + this.getPIDController().setOutputRange(-minimumOutput, maximumOutput); } //Set a deadband around the setpoint, in degrees, within which don't move, to avoid "dancing" - if (map.getDeadbandEnabled()) { - deadband = map.getDeadband(); - } else { - //Deadband of zero is equivalent to no deadband at all. - deadband = 0; - } + this.deadband = deadband; //Set whether or not to invert the loop. - inverted = map.getInverted(); + this.inverted = inverted; } /** @@ -90,14 +107,11 @@ public PIDAngleCommand(ToleranceBufferAnglePIDMap.ToleranceBufferAnglePID map, N * right side. */ protected double processPIDOutput(double output) { - //If we're using minimumOutput.. - if (minimumOutputEnabled) { - //Set the output to the minimum if it's too small. - if (output > 0 && output < minimumOutput) { - output = minimumOutput; - } else if (output < 0 && output > -minimumOutput) { - output = -minimumOutput; - } + //Set the output to the minimum if it's too small. + if (output > 0 && output < minimumOutput) { + output = minimumOutput; + } else if (output < 0 && output > -minimumOutput) { + output = -minimumOutput; } //Set the output to 0 if we're within the deadband. if (Math.abs(this.getPIDController().getError()) < deadband) { @@ -110,13 +124,6 @@ protected double processPIDOutput(double output) { return output; } - /* - NOTE: usePIDOutput() is an abstract method in PIDCommand. Any subclass of PIDAngleCommand must implement it. - It is called from the PIDController in PIDCommand, which will give it the output (i.e. u(t)) of the PID loop. - It's up to the programmer to decide how to use this. For any subclass of PIDAngleCommand, you can generally just - use it as a throttle value, or add it the throttle. Remember that one side is positive and one side is negative! - */ - /** * Returns the input for the pid loop. *

@@ -137,4 +144,15 @@ It is called from the PIDController in PIDCommand, which will give it the output protected double returnPIDInput() { return subsystem.getGyroOutput(); } + + /** + * Get the command object this object is. + * + * @return this. + */ + @Override + @NotNull + public Command getCommand() { + return this; + } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/ToggleOverrideNavX.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/ToggleOverrideNavX.java index 317f1789..c86a2fe4 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/ToggleOverrideNavX.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/NavX/commands/ToggleOverrideNavX.java @@ -1,25 +1,33 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.NavX.NavxSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Toggle whether or not to override the NavX. */ -public class ToggleOverrideNavX extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ToggleOverrideNavX extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private NavxSubsystem subsystem; + @NotNull + private final NavxSubsystem subsystem; /** * Default constructor. * * @param subsystem The subsystem to execute this command on */ - public ToggleOverrideNavX(NavxSubsystem subsystem) { + @JsonCreator + public ToggleOverrideNavX(@NotNull @JsonProperty(required = true) NavxSubsystem subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/ShooterSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/ShooterSubsystem.java index e52d33f3..9ef66fed 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/ShooterSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/ShooterSubsystem.java @@ -1,9 +1,14 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter; +import com.fasterxml.jackson.annotation.JsonTypeInfo; +import org.jetbrains.annotations.NotNull; + /** * A subsystem with a shooter and feeder. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") public interface ShooterSubsystem { + /** * Turn the shooter on to a map-specified speed. */ @@ -29,6 +34,7 @@ public interface ShooterSubsystem { * * @return Off, spinning up, or shooting. */ + @NotNull ShooterState getShooterState(); /** @@ -36,7 +42,7 @@ public interface ShooterSubsystem { * * @param state Off, spinning up, or shooting */ - void setShooterState(ShooterState state); + void setShooterState(@NotNull ShooterState state); /** * How long it takes for the shooter to get up to launch speed. Should be measured experimentally. diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/SpinUpShooter.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/SpinUpShooter.java index 133f363a..20c48fad 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/SpinUpShooter.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/SpinUpShooter.java @@ -1,25 +1,33 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.Shooter.ShooterSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Turn on the shooter but not the feeder in order to give the shooter time to get up to speed. */ -public class SpinUpShooter extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class SpinUpShooter extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private ShooterSubsystem subsystem; + @NotNull + private final ShooterSubsystem subsystem; /** * Default constructor * * @param subsystem The subsystem to execute this command on. */ - public SpinUpShooter(ShooterSubsystem subsystem) { + @JsonCreator + public SpinUpShooter(@NotNull @JsonProperty(required = true) ShooterSubsystem subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/SpinUpThenShoot.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/SpinUpThenShoot.java index d36b74a0..0ac3a527 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/SpinUpThenShoot.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/SpinUpThenShoot.java @@ -1,20 +1,27 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands; -import edu.wpi.first.wpilibj.command.CommandGroup; +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.interfaces.subsystem.Shooter.ShooterSubsystem; import org.usfirst.frc.team449.robot.util.WaitForMillis; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; /** * Spin up the shooter until it's at the target speed, then start feeding in balls. */ -public class SpinUpThenShoot extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class SpinUpThenShoot extends YamlCommandGroupWrapper { /** * Default constructor. * * @param subsystem The subsystem to execute this command on. */ - public SpinUpThenShoot(ShooterSubsystem subsystem) { + @JsonCreator + public SpinUpThenShoot(@NotNull @JsonProperty(required = true) ShooterSubsystem subsystem) { addSequential(new SpinUpShooter(subsystem)); //Use a wait command here because SpinUpShooter is instantaneous. addSequential(new WaitForMillis(subsystem.getSpinUpTimeMillis())); diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/ToggleShooting.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/ToggleShooting.java index 43e3aef1..c7c61ea8 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/ToggleShooting.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/ToggleShooting.java @@ -1,12 +1,18 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands; -import edu.wpi.first.wpilibj.command.CommandGroup; +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.interfaces.subsystem.Shooter.ShooterSubsystem; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; /** * Toggle whether or not the subsystem is firing. */ -public class ToggleShooting extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ToggleShooting extends YamlCommandGroupWrapper { /** @@ -14,7 +20,8 @@ public class ToggleShooting extends CommandGroup { * * @param subsystem The subsystem to execute this command on. */ - public ToggleShooting(ShooterSubsystem subsystem) { + @JsonCreator + public ToggleShooting(@NotNull @JsonProperty(required = true) ShooterSubsystem subsystem) { switch (subsystem.getShooterState()) { case OFF: addSequential(new SpinUpThenShoot(subsystem)); diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/TurnAllOff.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/TurnAllOff.java index 3fd45dd4..8e96426f 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/TurnAllOff.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/TurnAllOff.java @@ -1,25 +1,33 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.Shooter.ShooterSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Turn off the shooter and feeder. */ -public class TurnAllOff extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class TurnAllOff extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private ShooterSubsystem subsystem; + @NotNull + private final ShooterSubsystem subsystem; /** * Default constructor * * @param subsystem The subsystem to execute this command on. */ - public TurnAllOff(ShooterSubsystem subsystem) { + @JsonCreator + public TurnAllOff(@NotNull @JsonProperty(required = true) ShooterSubsystem subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/TurnAllOffWithRequires.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/TurnAllOffWithRequires.java new file mode 100644 index 00000000..3f4046ae --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/TurnAllOffWithRequires.java @@ -0,0 +1,28 @@ +package org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands; + +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.interfaces.subsystem.Shooter.ShooterSubsystem; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; + +/** + * Turn off the shooter and feeder, using requires() to interrupt any other commands that may be telling them to + * continue running. + */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class TurnAllOffWithRequires extends TurnAllOff { + + /** + * Default constructor + * + * @param subsystem The subsystem to execute this command on. + */ + @JsonCreator + public TurnAllOffWithRequires(@NotNull @JsonProperty(required = true) T subsystem) { + super(subsystem); + requires(subsystem); + } +} \ No newline at end of file diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/TurnAllOn.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/TurnAllOn.java index db02c257..0d3e9535 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/TurnAllOn.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/Shooter/commands/TurnAllOn.java @@ -1,25 +1,33 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.Shooter.ShooterSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Turn on the shooter and the feeder. */ -public class TurnAllOn extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class TurnAllOn extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private ShooterSubsystem subsystem; + @NotNull + private final ShooterSubsystem subsystem; /** * Default constructor * * @param subsystem The subsystem to execute this command on. */ - public TurnAllOn(ShooterSubsystem subsystem) { + @JsonCreator + public TurnAllOn(@NotNull @JsonProperty(required = true) ShooterSubsystem subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/BinaryMotorSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/BinaryMotorSubsystem.java index a53d5f25..0443ea31 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/BinaryMotorSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/BinaryMotorSubsystem.java @@ -1,8 +1,11 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor; +import com.fasterxml.jackson.annotation.JsonTypeInfo; + /** * A subsystem with a motor that only needs to be run at one speed, e.g. a flywheel shooter or simple intake. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") public interface BinaryMotorSubsystem { /** diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/ToggleMotor.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/ToggleMotor.java index e1f14c07..11ef1f6b 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/ToggleMotor.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/ToggleMotor.java @@ -1,25 +1,33 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.binaryMotor.BinaryMotorSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * A command that toggles the state of the motor between off and on. */ -public class ToggleMotor extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ToggleMotor extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private BinaryMotorSubsystem subsystem; + @NotNull + private final BinaryMotorSubsystem subsystem; /** * Default constructor * * @param subsystem The subsystem to execute this command on. */ - public ToggleMotor(BinaryMotorSubsystem subsystem) { + @JsonCreator + public ToggleMotor(@NotNull @JsonProperty(required = true) BinaryMotorSubsystem subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOff.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOff.java index 499d199c..021b98ec 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOff.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOff.java @@ -1,25 +1,33 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.binaryMotor.BinaryMotorSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Turns off the motor of the specified subsystem. */ -public class TurnMotorOff extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class TurnMotorOff extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private BinaryMotorSubsystem subsystem; + @NotNull + protected final BinaryMotorSubsystem subsystem; /** * Default constructor * * @param subsystem The subsystem to execute this command on. */ - public TurnMotorOff(BinaryMotorSubsystem subsystem) { + @JsonCreator + public TurnMotorOff(@NotNull @JsonProperty(required = true) BinaryMotorSubsystem subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOffWithRequires.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOffWithRequires.java index 72f9ddf3..d4ef7488 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOffWithRequires.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOffWithRequires.java @@ -1,29 +1,30 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Subsystem; +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.interfaces.subsystem.binaryMotor.BinaryMotorSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Turns off the motor of the subsystem, but does so while using requires() to interrupt any other commands currently * controlling the subsystem. */ -public class TurnMotorOffWithRequires extends Command { - - /** - * The subsystem to execute this command on. - */ - private BinaryMotorSubsystem subsystem; +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class TurnMotorOffWithRequires extends TurnMotorOff { /** * Default constructor * * @param subsystem The subsystem to execute this command on. */ - public TurnMotorOffWithRequires(BinaryMotorSubsystem subsystem) { - this.subsystem = subsystem; - requires((Subsystem) subsystem); + @JsonCreator + public TurnMotorOffWithRequires(@NotNull @JsonProperty(required = true) T subsystem) { + super(subsystem); + requires(subsystem); } /** @@ -34,24 +35,6 @@ protected void initialize() { Logger.addEvent("TurnMotorOffWithRequires init.", this.getClass()); } - /** - * Turn the motor off. - */ - @Override - protected void execute() { - subsystem.turnMotorOff(); - } - - /** - * Finish immediately because this is a state-change command. - * - * @return true - */ - @Override - protected boolean isFinished() { - return true; - } - /** * Log when this command ends */ diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOn.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOn.java index 5ee08aea..6742380a 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOn.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/binaryMotor/commands/TurnMotorOn.java @@ -1,25 +1,33 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands; -import edu.wpi.first.wpilibj.command.Command; +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.interfaces.subsystem.binaryMotor.BinaryMotorSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Turns on the motor of the specified subsystem. */ -public class TurnMotorOn extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class TurnMotorOn extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private BinaryMotorSubsystem subsystem; + @NotNull + private final BinaryMotorSubsystem subsystem; /** * Default constructor * * @param subsystem The subsystem to execute this command on. */ - public TurnMotorOn(BinaryMotorSubsystem subsystem) { + @JsonCreator + public TurnMotorOn(@NotNull @JsonProperty(required = true) BinaryMotorSubsystem subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/conditional/ConditionalSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/conditional/ConditionalSubsystem.java index ce0298e5..665ad169 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/conditional/ConditionalSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/conditional/ConditionalSubsystem.java @@ -1,8 +1,11 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.conditional; +import com.fasterxml.jackson.annotation.JsonTypeInfo; + /** * A subsystem with a condition that's sometimes met, e.g. a limit switch, a current/power limit, an IR sensor. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") public interface ConditionalSubsystem { /** diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/SolenoidSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/SolenoidSubsystem.java index e5485a95..7290059f 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/SolenoidSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/SolenoidSubsystem.java @@ -1,22 +1,27 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid; +import com.fasterxml.jackson.annotation.JsonTypeInfo; import edu.wpi.first.wpilibj.DoubleSolenoid; +import org.jetbrains.annotations.NotNull; /** * A subsystem with a single DoubleSolenoid piston. */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") public interface SolenoidSubsystem { + /** * Set the solenoid to a certain position. * * @param value Forward to extend the Solenoid, Reverse to contract it. */ - void setSolenoid(DoubleSolenoid.Value value); + void setSolenoid(@NotNull DoubleSolenoid.Value value); /** * Get the position of the solenoid. * * @return Forward if extended, Reverse if contracted. */ + @NotNull DoubleSolenoid.Value getSolenoidPosition(); } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/SolenoidForward.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/SolenoidForward.java index 26ce5a7f..bf0a6ac0 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/SolenoidForward.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/SolenoidForward.java @@ -1,26 +1,34 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands; +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.DoubleSolenoid; -import edu.wpi.first.wpilibj.command.Command; +import org.jetbrains.annotations.NotNull; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.SolenoidSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * A command that extends a piston. */ -public class SolenoidForward extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class SolenoidForward extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private SolenoidSubsystem subsystem; + @NotNull + private final SolenoidSubsystem subsystem; /** * Default constructor * * @param subsystem The solenoid subsystem to execute this command on. */ - public SolenoidForward(SolenoidSubsystem subsystem) { + @JsonCreator + public SolenoidForward(@NotNull @JsonProperty(required = true) SolenoidSubsystem subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/SolenoidReverse.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/SolenoidReverse.java index 053fb111..62607e3c 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/SolenoidReverse.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/SolenoidReverse.java @@ -1,26 +1,34 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands; +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.DoubleSolenoid; -import edu.wpi.first.wpilibj.command.Command; +import org.jetbrains.annotations.NotNull; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.SolenoidSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * A command that contracts a piston. */ -public class SolenoidReverse extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class SolenoidReverse extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private SolenoidSubsystem subsystem; + @NotNull + private final SolenoidSubsystem subsystem; /** * Default constructor * * @param subsystem The solenoid subsystem to execute this command on. */ - public SolenoidReverse(SolenoidSubsystem subsystem) { + @JsonCreator + public SolenoidReverse(@NotNull @JsonProperty(required = true) SolenoidSubsystem subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/ToggleSolenoid.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/ToggleSolenoid.java index 0d12f958..2330b0b5 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/ToggleSolenoid.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/interfaces/subsystem/solenoid/commands/ToggleSolenoid.java @@ -1,26 +1,34 @@ package org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands; +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.DoubleSolenoid; -import edu.wpi.first.wpilibj.command.Command; +import org.jetbrains.annotations.NotNull; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.SolenoidSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * A command that toggles the position of a piston. */ -public class ToggleSolenoid extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ToggleSolenoid extends YamlCommandWrapper { /** * The subsystem to execute this command on. */ - private SolenoidSubsystem subsystem; + @NotNull + private final SolenoidSubsystem subsystem; /** * Default constructor * * @param subsystem The solenoid subsystem to execute this command on. */ - public ToggleSolenoid(SolenoidSubsystem subsystem) { + @JsonCreator + public ToggleSolenoid(@NotNull @JsonProperty(required = true) SolenoidSubsystem subsystem) { this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/activegear/ActiveGearSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/activegear/ActiveGearSubsystem.java index 58f430d1..5f3e39b5 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/activegear/ActiveGearSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/activegear/ActiveGearSubsystem.java @@ -1,33 +1,40 @@ package org.usfirst.frc.team449.robot.mechanism.activegear; +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.DoubleSolenoid; -import org.usfirst.frc.team449.robot.MappedSubsystem; +import org.jetbrains.annotations.NotNull; import org.usfirst.frc.team449.robot.components.MappedDoubleSolenoid; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.SolenoidSubsystem; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * The subsystem that carries and pushes gears. */ -public class ActiveGearSubsystem extends MappedSubsystem implements SolenoidSubsystem { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ActiveGearSubsystem extends YamlSubsystem implements SolenoidSubsystem { + /** - * Whether piston is currently contracted + * Piston for pushing gears */ - private boolean contracted; + @NotNull + private final DoubleSolenoid piston; /** - * Piston for pushing gears + * Whether piston is currently contracted */ - private DoubleSolenoid piston; + private boolean contracted; /** - * Creates a mapped subsystem and sets its map + * Default constructor * - * @param map the map of constants relevant to this subsystem + * @param piston The piston that comprises this subsystem. */ - public ActiveGearSubsystem(maps.org.usfirst.frc.team449.robot.mechanism.activegear.ActiveGearMap.ActiveGear map) { - super(map.getMechanism()); - this.map = map; - this.piston = new MappedDoubleSolenoid(map.getPiston()); + @JsonCreator + public ActiveGearSubsystem(@NotNull @JsonProperty(required = true) MappedDoubleSolenoid piston) { + this.piston = piston; } /** @@ -35,7 +42,7 @@ public ActiveGearSubsystem(maps.org.usfirst.frc.team449.robot.mechanism.activege * * @param value Forward to extend the Solenoid, Reverse to contract it. */ - public void setSolenoid(DoubleSolenoid.Value value) { + public void setSolenoid(@NotNull DoubleSolenoid.Value value) { piston.set(value); contracted = (value == DoubleSolenoid.Value.kReverse); } @@ -45,6 +52,7 @@ public void setSolenoid(DoubleSolenoid.Value value) { * * @return Forward if extended, Reverse if contracted. */ + @NotNull @Override public DoubleSolenoid.Value getSolenoidPosition() { return contracted ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward; diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/ClimberSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/ClimberSubsystem.java index d52c8798..3d22bbf6 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/ClimberSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/climber/ClimberSubsystem.java @@ -1,61 +1,75 @@ package org.usfirst.frc.team449.robot.mechanism.climber; +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.VictorSP; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.components.MappedVictor; import org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRX; import org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.BinaryMotorSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.conditional.ConditionalSubsystem; -import org.usfirst.frc.team449.robot.mechanism.MechanismSubsystem; import org.usfirst.frc.team449.robot.util.BufferTimer; import org.usfirst.frc.team449.robot.util.Loggable; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * A climber subsystem that uses power monitoring to stop climbing. */ -public class ClimberSubsystem extends MechanismSubsystem implements Loggable, BinaryMotorSubsystem, ConditionalSubsystem { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ClimberSubsystem extends YamlSubsystem implements Loggable, BinaryMotorSubsystem, ConditionalSubsystem { + /** * The CANTalon controlling one of the climber motors. */ - private RotPerSecCANTalonSRX canTalonSRX; + @NotNull + private final RotPerSecCANTalonSRX canTalonSRX; /** * The Victor controlling the other climber motor. */ - private VictorSP victor; + @Nullable + private final VictorSP victor; /** * The maximum allowable power before we stop the motor. */ - private double max_power; + private final double maxPower; /** * The bufferTimer controlling how long we can be above the current limit before we stop climbing. */ - private BufferTimer currentLimitTimer; + @NotNull + private final BufferTimer currentLimitTimer; /** * Whether or not the motor is currently spinning. */ private boolean motorSpinning; + /** - * Construct a ClimberSubsystem + * Default constructor * - * @param map the config map + * @param talonSRX The CANTalon controlling one of the climber motors. + * @param maxPower The maximum power at which the motor won't shut off. + * @param victor The VictorSP controlling the other climber motor. Can be null. + * @param millisAboveMaxPower The number of milliseconds it takes to shut off the climber after being above the + * current limit. Defaults to 0. */ - public ClimberSubsystem(maps.org.usfirst.frc.team449.robot.mechanism.climber.ClimberMap.Climber map) { - super(map.getMechanism()); + @JsonCreator + public ClimberSubsystem(@NotNull @JsonProperty(required = true) RotPerSecCANTalonSRX talonSRX, + @JsonProperty(required = true) double maxPower, + @Nullable MappedVictor victor, + int millisAboveMaxPower) { //Instantiate things - this.map = map; - canTalonSRX = new RotPerSecCANTalonSRX(map.getWinch()); - this.max_power = map.getMaxPower(); - currentLimitTimer = new BufferTimer(map.getMillisAboveMaxPower()); + canTalonSRX = talonSRX; + this.maxPower = maxPower; + currentLimitTimer = new BufferTimer(millisAboveMaxPower); motorSpinning = false; - - //Victor is optional - if (map.hasVictor()) { - this.victor = new MappedVictor(map.getVictor()); - } + this.victor = victor; } /** @@ -86,10 +100,11 @@ private void setPercentVbus(double percentVbus) { * * @return An N-length array of String labels for data, where N is the length of the Object[] returned by getData(). */ + @NotNull @Override public String[] getHeader() { - return new String[]{"current,", - "voltage,", + return new String[]{"current", + "voltage", "power"}; } @@ -98,10 +113,11 @@ public String[] getHeader() { * * @return An N-length array of Objects, where N is the number of labels given by getHeader. */ + @NotNull @Override public Object[] getData() { - return new Object[]{canTalonSRX.canTalon.getOutputCurrent(), - canTalonSRX.canTalon.getOutputVoltage(), + return new Object[]{canTalonSRX.getCanTalon().getOutputCurrent(), + canTalonSRX.getCanTalon().getOutputVoltage(), canTalonSRX.getPower()}; } @@ -110,6 +126,7 @@ public Object[] getData() { * * @return A string that will identify this object in the log file. */ + @NotNull @Override public String getName() { return "climber"; @@ -120,7 +137,7 @@ public String getName() { */ @Override public void turnMotorOn() { - canTalonSRX.canTalon.enable(); + canTalonSRX.getCanTalon().enable(); setPercentVbus(1); motorSpinning = true; } @@ -131,7 +148,7 @@ public void turnMotorOn() { @Override public void turnMotorOff() { setPercentVbus(0); - canTalonSRX.canTalon.disable(); + canTalonSRX.getCanTalon().disable(); motorSpinning = false; } @@ -152,6 +169,6 @@ public boolean isMotorOn() { */ @Override public boolean isConditionTrue() { - return currentLimitTimer.get(Math.abs(canTalonSRX.getPower()) > max_power); + return currentLimitTimer.get(Math.abs(canTalonSRX.getPower()) > maxPower); } } \ No newline at end of file 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 37d70ce0..2e69b08a 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 @@ -1,29 +1,36 @@ package org.usfirst.frc.team449.robot.mechanism.climber.commands; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Subsystem; +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.interfaces.subsystem.binaryMotor.BinaryMotorSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.conditional.ConditionalSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * Run a BinaryMotor while a condition is true. */ -public class RunMotorWhileConditonMet extends Command { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class RunMotorWhileConditonMet extends YamlCommandWrapper { /** * The subsystem to execute this command on */ - private BinaryMotorSubsystem subsystem; + @NotNull + private final T subsystem; /** * Default constructor * - * @param subsystem The BinaryMotor subsystem to execute this command on. Must also be a {@link - * ConditionalSubsystem}. + * @param subsystem The subsystem to execute this command on. */ - public RunMotorWhileConditonMet(BinaryMotorSubsystem subsystem) { - requires((Subsystem) subsystem); + @JsonCreator + public RunMotorWhileConditonMet(@NotNull @JsonProperty(required = true) T subsystem) { + requires(subsystem); this.subsystem = subsystem; Logger.addEvent("RunMotorWhileConditonMet constructed", this.getClass()); } @@ -51,7 +58,7 @@ protected void execute() { */ @Override protected boolean isFinished() { - return ((ConditionalSubsystem) subsystem).isConditionTrue(); + return subsystem.isConditionTrue(); } /** diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/intake/Intake2017/Intake2017.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/intake/Intake2017/Intake2017.java index 66230333..91895650 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/intake/Intake2017/Intake2017.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/intake/Intake2017/Intake2017.java @@ -1,80 +1,95 @@ package org.usfirst.frc.team449.robot.mechanism.intake.Intake2017; +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.DoubleSolenoid; import edu.wpi.first.wpilibj.VictorSP; -import org.usfirst.frc.team449.robot.MappedSubsystem; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.components.MappedDoubleSolenoid; import org.usfirst.frc.team449.robot.components.MappedVictor; import org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.IntakeSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.SolenoidSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * A subsystem that picks up balls from the ground. */ -public class Intake2017 extends MappedSubsystem implements SolenoidSubsystem, IntakeSubsystem { - /** - * Whether intake is currently up - */ - private boolean intakeUp; +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class Intake2017 extends YamlSubsystem implements SolenoidSubsystem, IntakeSubsystem { /** * VictorSP for the fixed intake */ - private VictorSP fixedVictor; + @NotNull + private final VictorSP fixedVictor; /** * VictorSP for the actuated intake */ - private VictorSP actuatedVictor; + @Nullable + private final VictorSP actuatedVictor; /** * Piston for raising and lowering the intake */ - private DoubleSolenoid piston; + @Nullable + private final DoubleSolenoid piston; /** * How fast the fixed victor should go to pick up balls, on [-1, 1] */ - private double fixedIntakeSpeed; + private final double fixedIntakeSpeed; /** * How fast the fixed victor should go to agitate balls while they're being fed into the shooter, on [-1, 1] */ - private double fixedAgitateSpeed; + private final double fixedAgitateSpeed; /** * How fast the actuated victor should go to pick up balls, on [-1, 1] */ - private double actuatedSpeed; + private final double actuatedSpeed; + + /** + * Whether intake is currently up + */ + private boolean intakeUp; /** * The mode the intake's currently in. */ + @NotNull private IntakeMode mode; /** - * Creates a mapped subsystem and sets its map + * Default constructor. * - * @param map the map of constants relevant to this subsystem - */ - public Intake2017(maps.org.usfirst.frc.team449.robot.mechanism.intake.Intake2017Map.Intake2017 map) { - super(map.getMechanism()); + * @param fixedVictor The VictorSP powering the fixed intake. + * @param fixedAgitateSpeed The speed to run the fixed victor at to agitate balls, on [-1, 1] + * @param fixedIntakeSpeed The speed to run the fixed victor to intake balls, on [-1, 1] + * @param actuatedVictor The VictorSP powering the actuated intake. Can be null. + * @param actuatedSpeed The speed to run the actuated victor to intake balls, on [-1, 1]. Defaults to 0. + * @param piston The piston for raising and lowering the actuated intake. Can be null. + */ + @JsonCreator + public Intake2017(@NotNull @JsonProperty(required = true) MappedVictor fixedVictor, + @JsonProperty(required = true) double fixedAgitateSpeed, + @JsonProperty(required = true) double fixedIntakeSpeed, + @Nullable MappedVictor actuatedVictor, + double actuatedSpeed, + @Nullable MappedDoubleSolenoid piston) { //Instantiate stuff. - this.map = map; - fixedVictor = new MappedVictor(map.getFixedVictor()); - fixedAgitateSpeed = map.getFixedAgitateSpeed(); - fixedIntakeSpeed = map.getFixedIntakeSpeed(); - actuatedSpeed = map.getActuatedSpeed(); + this.fixedVictor = fixedVictor; + this.fixedIntakeSpeed = fixedIntakeSpeed; + this.fixedAgitateSpeed = fixedAgitateSpeed; + this.actuatedVictor = actuatedVictor; + this.actuatedSpeed = actuatedSpeed; + this.piston = piston; mode = IntakeMode.OFF; - - //Instantiate optional stuff - if (map.hasActuatedVictor()) { - actuatedVictor = new MappedVictor(map.getActuatedVictor()); - } - if (map.hasPiston()) { - piston = new MappedDoubleSolenoid(map.getPiston()); - } } /** @@ -102,7 +117,7 @@ private void setFixedVictor(double sp) { * * @param value Forward to extend the Solenoid, Reverse to contract it. */ - public void setSolenoid(DoubleSolenoid.Value value) { + public void setSolenoid(@NotNull DoubleSolenoid.Value value) { if (piston != null) { piston.set(value); intakeUp = (value == DoubleSolenoid.Value.kReverse); @@ -114,6 +129,7 @@ public void setSolenoid(DoubleSolenoid.Value value) { * * @return Forward if extended, Reverse if contracted. */ + @NotNull public DoubleSolenoid.Value getSolenoidPosition() { return intakeUp ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward; } @@ -134,6 +150,7 @@ protected void initDefaultCommand() { * * @return off, in slow, in fast, out slow, out fast. */ + @NotNull @Override public IntakeMode getMode() { return mode; @@ -145,7 +162,7 @@ public IntakeMode getMode() { * @param mode off, in slow, in fast, out slow, out fast. */ @Override - public void setMode(IntakeMode mode) { + public void setMode(@NotNull IntakeMode mode) { this.mode = mode; switch (mode) { case OFF: diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/pneumatics/PneumaticsSubsystem.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/pneumatics/PneumaticsSubsystem.java index 6a1173c5..714c03a2 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/pneumatics/PneumaticsSubsystem.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/pneumatics/PneumaticsSubsystem.java @@ -1,31 +1,46 @@ package org.usfirst.frc.team449.robot.mechanism.pneumatics; +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.Compressor; -import org.usfirst.frc.team449.robot.MappedSubsystem; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.components.PressureSensor; import org.usfirst.frc.team449.robot.util.Loggable; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** * A subsystem representing the pneumatics control system (e.g. the compressor and maybe a pressure sensor) */ -public class PneumaticsSubsystem extends MappedSubsystem implements Loggable { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class PneumaticsSubsystem extends YamlSubsystem implements Loggable { + /** * The compressor that provides pressure to the robot's pneumatics. */ - private Compressor compressor; + @NotNull + private final Compressor compressor; /** * The pressure sensor that reads the pneumatic pressure. */ - private PressureSensor pressureSensor; + @Nullable + private final PressureSensor pressureSensor; - public PneumaticsSubsystem(maps.org.usfirst.frc.team449.robot.mechanism.pneumatics.PneumaticSystemMap - .PneumaticSystem map) { - super(map); - compressor = new Compressor(map.getNodeID()); - if (map.hasPressureSensor()) { - pressureSensor = new PressureSensor(map.getPressureSensor()); - } + /** + * Default constructor + * + * @param nodeID The node ID of the compressor. + * @param pressureSensor The pressure sensor attached to this pneumatics system. Can be null. + */ + @JsonCreator + public PneumaticsSubsystem(@JsonProperty(required = true) int nodeID, + @Nullable PressureSensor pressureSensor) { + super(); + compressor = new Compressor(nodeID); + this.pressureSensor = pressureSensor; } /** @@ -49,6 +64,7 @@ public void startCompressor() { * * @return An N-length array of String labels for data, where N is the length of the Object[] returned by getData(). */ + @NotNull @Override public String[] getHeader() { return new String[]{"pressure"}; @@ -59,6 +75,7 @@ public String[] getHeader() { * * @return An N-length array of Objects, where N is the number of labels given by getHeader. */ + @NotNull @Override public Object[] getData() { if (pressureSensor == null) { @@ -73,6 +90,7 @@ public Object[] getData() { * * @return A string that will identify this object in the log file. */ + @NotNull @Override public String getName() { return "pneumatics"; diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/pneumatics/commands/StartCompressor.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/pneumatics/commands/StartCompressor.java index 080462e7..e6497a22 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/pneumatics/commands/StartCompressor.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/pneumatics/commands/StartCompressor.java @@ -1,23 +1,34 @@ package org.usfirst.frc.team449.robot.mechanism.pneumatics.commands; -import org.usfirst.frc.team449.robot.ReferencingCommand; +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.mechanism.pneumatics.PneumaticsSubsystem; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlCommandWrapper; /** * Start up the pneumatic compressor. */ -public class StartCompressor extends ReferencingCommand { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class StartCompressor extends YamlCommandWrapper { - private PneumaticsSubsystem subsystem; + /** + * The subsystem to execute this command on. + */ + @NotNull + private final PneumaticsSubsystem subsystem; /** * Default constructor * * @param subsystem The subsystem to execute this command on. */ - public StartCompressor(PneumaticsSubsystem subsystem) { - super(subsystem); + @JsonCreator + public StartCompressor(@NotNull @JsonProperty(required = true) PneumaticsSubsystem subsystem) { + super(); this.subsystem = subsystem; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/SingleFlywheelShooter.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/SingleFlywheelShooter.java index b615cb44..e1c0d2b9 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/SingleFlywheelShooter.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/singleflywheelshooter/SingleFlywheelShooter.java @@ -1,64 +1,81 @@ package org.usfirst.frc.team449.robot.mechanism.singleflywheelshooter; +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.VictorSP; -import org.usfirst.frc.team449.robot.MappedSubsystem; +import org.jetbrains.annotations.NotNull; import org.usfirst.frc.team449.robot.components.MappedVictor; import org.usfirst.frc.team449.robot.components.RotPerSecCANTalonSRX; import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.ShooterSubsystem; import org.usfirst.frc.team449.robot.util.Loggable; import org.usfirst.frc.team449.robot.util.Logger; +import org.usfirst.frc.team449.robot.util.YamlSubsystem; /** - * Class for the flywheel + * A flywheel shooter with a single flywheel and a single-motor feeder system. */ -public class SingleFlywheelShooter extends MappedSubsystem implements Loggable, ShooterSubsystem { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class SingleFlywheelShooter extends YamlSubsystem implements Loggable, ShooterSubsystem { + /** * The flywheel's Talon */ - private RotPerSecCANTalonSRX shooterTalon; + @NotNull + private final RotPerSecCANTalonSRX shooterTalon; /** * The feeder's Victor */ - private VictorSP feederVictor; + @NotNull + private final VictorSP feederVictor; /** * How fast to run the feeder, from [-1, 1] */ - private double feederThrottle; - - /** - * Whether the flywheel is currently commanded to spin - */ - private ShooterState state; + private final double feederThrottle; /** * Throttle at which to run the shooter, from [-1, 1] */ - private double shooterThrottle; + private final double shooterThrottle; /** * How long it takes the shooter to get up to speed, in milliseconds. */ - private long spinUpTime; + private final long spinUpTime; /** - * Construct a SingleFlywheelShooter - * - * @param map config map + * Whether the flywheel is currently commanded to spin */ - public SingleFlywheelShooter(maps.org.usfirst.frc.team449.robot.mechanism.singleflywheelshooter - .SingleFlywheelShooterMap.SingleFlywheelShooter map) { - super(map.getMechanism()); - this.map = map; - shooterTalon = new RotPerSecCANTalonSRX(map.getShooter()); + @NotNull + private ShooterState state; - shooterThrottle = map.getShooterThrottle(); + /** + * Default constructor + * + * @param shooterTalon The TalonSRX controlling the flywheel. + * @param shooterThrottle The throttle, from [-1, 1], at which to run the shooter. + * @param feederVictor The VictorSP controlling the feeder. + * @param feederThrottle The throttle, from [-1, 1], at which to run the feeder. + * @param spinUpTimeSecs The amount of time, in seconds, it takes for the shooter to get up to speed. Defaults to + * 0. + */ + @JsonCreator + public SingleFlywheelShooter(@NotNull @JsonProperty(required = true) RotPerSecCANTalonSRX shooterTalon, + @JsonProperty(required = true) double shooterThrottle, + @NotNull @JsonProperty(required = true) MappedVictor feederVictor, + @JsonProperty(required = true) double feederThrottle, + double spinUpTimeSecs) { + super(); + this.shooterTalon = shooterTalon; + this.shooterThrottle = shooterThrottle; + this.feederVictor = feederVictor; + this.feederThrottle = feederThrottle; state = ShooterState.OFF; - spinUpTime = (long) (map.getSpinUpTimeSecs() * 1000.); - feederVictor = new MappedVictor(map.getFeeder()); - feederThrottle = map.getFeederThrottle(); - Logger.addEvent("Shooter F: " + shooterTalon.canTalon.getF(), this.getClass()); + spinUpTime = (long) (spinUpTimeSecs * 1000.); + Logger.addEvent("Shooter F: " + shooterTalon.getCanTalon().getF(), this.getClass()); } /** @@ -76,7 +93,12 @@ private void setFlywheelVBusSpeed(double sp) { * @param sp percent PID velocity setpoint [-1, 1] */ private void setFlywheelPIDSpeed(double sp) { - shooterTalon.setSpeed(shooterTalon.getMaxSpeed() * sp); + if (shooterTalon.getMaxSpeed() == null) { + setFlywheelVBusSpeed(sp); + System.out.println("You're trying to set PID throttle, but the shooter talon doesn't have PID constants defined. Using voltage control instead."); + } else { + shooterTalon.setSpeed(shooterTalon.getMaxSpeed() * sp); + } } /** @@ -110,12 +132,13 @@ protected void initDefaultCommand() { * * @return An N-length array of String labels for data, where N is the length of the Object[] returned by getData(). */ + @NotNull @Override public String[] getHeader() { - return new String[]{"speed,", - "setpoint,", - "error,", - "voltage,", + return new String[]{"speed", + "setpoint", + "error", + "voltage", "current"}; } @@ -124,13 +147,14 @@ public String[] getHeader() { * * @return An N-length array of Objects, where N is the number of labels given by getHeader. */ + @NotNull @Override public Object[] getData() { return new Object[]{shooterTalon.getSpeed(), shooterTalon.getSetpoint(), shooterTalon.getError(), - shooterTalon.canTalon.getOutputVoltage(), - shooterTalon.canTalon.getOutputCurrent()}; + shooterTalon.getCanTalon().getOutputVoltage(), + shooterTalon.getCanTalon().getOutputCurrent()}; } /** @@ -138,6 +162,7 @@ public Object[] getData() { * * @return A string that will identify this object in the log file. */ + @NotNull @Override public String getName() { return "shooter"; @@ -148,7 +173,7 @@ public String getName() { */ @Override public void turnShooterOn() { - shooterTalon.canTalon.enable(); + shooterTalon.getCanTalon().enable(); setFlywheelDefaultSpeed(shooterThrottle); } @@ -158,7 +183,7 @@ public void turnShooterOn() { @Override public void turnShooterOff() { setFlywheelVBusSpeed(0); - shooterTalon.canTalon.disable(); + shooterTalon.getCanTalon().disable(); } /** @@ -182,6 +207,7 @@ public void turnFeederOff() { * * @return Off, spinning up, or shooting. */ + @NotNull @Override public ShooterState getShooterState() { return state; @@ -193,7 +219,7 @@ public ShooterState getShooterState() { * @param state Off, spinning up, or shooting */ @Override - public void setShooterState(ShooterState state) { + public void setShooterState(@NotNull ShooterState state) { this.state = state; } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/FireShooter.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/FireShooter.java index 714cac94..89192aa3 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/FireShooter.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/FireShooter.java @@ -1,23 +1,31 @@ package org.usfirst.frc.team449.robot.mechanism.topcommands.shooter; -import edu.wpi.first.wpilibj.command.CommandGroup; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIdentityInfo; +import com.fasterxml.jackson.annotation.ObjectIdGenerators; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.IntakeSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.commands.SetIntakeMode; import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.ShooterSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands.TurnAllOn; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; /** * Command group for firing the shooter. * Runs flywheel, runs static intake, stops dynamic intake, raises intake, and runs feeder. */ -public class FireShooter extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class FireShooter extends YamlCommandGroupWrapper { + /** * Constructs a FireShooter command group * - * @param shooterSubsystem shooter subsystem - * @param intakeSubsystem intake subsystem + * @param shooterSubsystem shooter subsystem. Can be null. + * @param intakeSubsystem intake subsystem. Can be null. */ - public FireShooter(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) { + @JsonCreator + public FireShooter(@Nullable ShooterSubsystem shooterSubsystem, + @Nullable IntakeSubsystem intakeSubsystem) { if (shooterSubsystem != null) { addParallel(new TurnAllOn(shooterSubsystem)); } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/LoadShooter.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/LoadShooter.java index 924b0552..aaf05fd9 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/LoadShooter.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/LoadShooter.java @@ -1,30 +1,38 @@ package org.usfirst.frc.team449.robot.mechanism.topcommands.shooter; -import edu.wpi.first.wpilibj.command.CommandGroup; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIdentityInfo; +import com.fasterxml.jackson.annotation.ObjectIdGenerators; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.IntakeSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.commands.SetIntakeMode; import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.ShooterSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands.TurnAllOff; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.SolenoidSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidReverse; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; /** * Command group for intaking balls from the ground. * Stops flywheel, runs static intake, runs dynamic intake, lowers intake, and stops feeder. */ -public class LoadShooter extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class LoadShooter extends YamlCommandGroupWrapper { + /** * Constructs a LoadShooter command group * - * @param shooterSubsystem shooter subsystem - * @param intakeSubsystem intake subsystem. Must also be a {@link SolenoidSubsystem}. + * @param shooterSubsystem shooter subsystem. Can be null. + * @param intakeSubsystem intake subsystem. Can be null. */ - public LoadShooter(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) { + @JsonCreator + public LoadShooter(@Nullable ShooterSubsystem shooterSubsystem, + @Nullable T intakeSubsystem) { if (shooterSubsystem != null) { addParallel(new TurnAllOff(shooterSubsystem)); } if (intakeSubsystem != null) { - addParallel(new SolenoidReverse((SolenoidSubsystem) intakeSubsystem)); + addParallel(new SolenoidReverse(intakeSubsystem)); addParallel(new SetIntakeMode(intakeSubsystem, IntakeSubsystem.IntakeMode.IN_FAST)); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/RackShooter.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/RackShooter.java index 475acabb..d9addae5 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/RackShooter.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/RackShooter.java @@ -1,30 +1,38 @@ package org.usfirst.frc.team449.robot.mechanism.topcommands.shooter; -import edu.wpi.first.wpilibj.command.CommandGroup; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIdentityInfo; +import com.fasterxml.jackson.annotation.ObjectIdGenerators; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.IntakeSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.commands.SetIntakeMode; import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.ShooterSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands.SpinUpShooter; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.SolenoidSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidReverse; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; /** * Command group for preparing the shooter to fire. * Starts flywheel, runs static intake, stops dynamic intake, raises intake, and stops feeder. */ -public class RackShooter extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class RackShooter extends YamlCommandGroupWrapper { + /** * Constructs a RackShooter command group * - * @param shooterSubsystem shooter subsystem - * @param intakeSubsystem intake subsystem. Must also be a {@link SolenoidSubsystem}. + * @param shooterSubsystem shooter subsystem. Can be null. + * @param intakeSubsystem intake subsystem. Can be null. */ - public RackShooter(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) { + @JsonCreator + public RackShooter(@Nullable ShooterSubsystem shooterSubsystem, + @Nullable T intakeSubsystem) { if (shooterSubsystem != null) { addParallel(new SpinUpShooter(shooterSubsystem)); } if (intakeSubsystem != null) { - addParallel(new SolenoidReverse((SolenoidSubsystem) intakeSubsystem)); + addParallel(new SolenoidReverse(intakeSubsystem)); addParallel(new SetIntakeMode(intakeSubsystem, IntakeSubsystem.IntakeMode.IN_SLOW)); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/ResetShooter.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/ResetShooter.java index ea8c2b06..b8ce3807 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/ResetShooter.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/mechanism/topcommands/shooter/ResetShooter.java @@ -1,30 +1,38 @@ package org.usfirst.frc.team449.robot.mechanism.topcommands.shooter; -import edu.wpi.first.wpilibj.command.CommandGroup; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIdentityInfo; +import com.fasterxml.jackson.annotation.ObjectIdGenerators; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.IntakeSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.commands.SetIntakeMode; import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.ShooterSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands.TurnAllOff; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.SolenoidSubsystem; import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidReverse; +import org.usfirst.frc.team449.robot.util.YamlCommandGroupWrapper; /** * Command group to reset everything. * Turns everything off, raises intake */ -public class ResetShooter extends CommandGroup { +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ResetShooter extends YamlCommandGroupWrapper { + /** * Constructs a ResetShooter command group * - * @param shooterSubsystem shooter subsystem - * @param intakeSubsystem intake subsystem. Must also be a {@link SolenoidSubsystem}. + * @param shooterSubsystem shooter subsystem. Can be null. + * @param intakeSubsystem intake subsystem. Can be null. */ - public ResetShooter(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) { + @JsonCreator + public ResetShooter(@Nullable ShooterSubsystem shooterSubsystem, + @Nullable T intakeSubsystem) { if (shooterSubsystem != null) { addParallel(new TurnAllOff(shooterSubsystem)); } if (intakeSubsystem != null) { - addParallel(new SolenoidReverse((SolenoidSubsystem) intakeSubsystem)); + addParallel(new SolenoidReverse(intakeSubsystem)); addParallel(new SetIntakeMode(intakeSubsystem, IntakeSubsystem.IntakeMode.OFF)); } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/ArcadeOIWithDPad.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/ArcadeOIWithDPad.java new file mode 100644 index 00000000..766b0e98 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/ArcadeOIWithDPad.java @@ -0,0 +1,102 @@ +package org.usfirst.frc.team449.robot.oi; + +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.jetbrains.annotations.Nullable; +import org.usfirst.frc.team449.robot.components.MappedJoystick; +import org.usfirst.frc.team449.robot.components.MappedThrottle; +import org.usfirst.frc.team449.robot.interfaces.oi.ArcadeOI; +import org.usfirst.frc.team449.robot.util.Polynomial; + +/** + * An arcade OI with an option to use the D-pad for turning. + */ +@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) +public class ArcadeOIWithDPad extends ArcadeOI { + + /** + * How much the D-pad moves the robot rotationally on a 0 to 1 scale, equivalent to pushing the turning stick that + * much of the way + */ + private final double dPadShift; + + /** + * The throttle wrapper for the stick controlling turning velocity + */ + @NotNull + private final MappedThrottle rotThrottle; + + /** + * The throttle wrapper for the stick controlling linear velocity + */ + @NotNull + private final MappedThrottle fwdThrottle; + + /** + * The controller with the D-pad. Can be null if not using D-pad. + */ + @Nullable + private final Joystick gamepad; + + @Nullable + private final Polynomial scaleRotByFwdPoly; + + /** + * Default constructor + * + * @param gamepad The gamepad containing the joysticks and buttons. Can be null if not using the D-pad. + * @param rotThrottle The throttle for rotating the robot. + * @param fwdThrottle The throttle for driving the robot straight. + * @param invertDPad Whether or not to invert the D-pad. Defaults to false. + * @param dPadShift How fast the dPad should turn the robot, on [0, 1]. Defaults to 0. + */ + @JsonCreator + public ArcadeOIWithDPad( + @NotNull @JsonProperty(required = true) MappedThrottle rotThrottle, + @NotNull @JsonProperty(required = true) MappedThrottle fwdThrottle, + double dPadShift, + boolean invertDPad, + @Nullable MappedJoystick gamepad, + @Nullable Polynomial scaleRotByFwdPoly) { + this.dPadShift = (invertDPad ? -1 : 1) * dPadShift; + this.rotThrottle = rotThrottle; + this.fwdThrottle = fwdThrottle; + this.gamepad = gamepad; + this.scaleRotByFwdPoly = scaleRotByFwdPoly; + } + + /** + * The output of the throttle controlling linear velocity, smoothed and adjusted according to what type of + * joystick it is. + * + * @return The processed stick output, sign-adjusted so 1 is forward and -1 is backwards. + */ + public double getFwd() { + //Scale based on rotational throttle for more responsive turning at high speed + return fwdThrottle.getValue(); + } + + /** + * Get the output of the D-pad or turning joystick, whichever is in use. If both are in use, the D-pad takes + * preference. + * + * @return The processed stick or D-pad output, sign-adjusted so 1 is right and -1 is left. + */ + public double getRot() { + //If the gamepad is being pushed to the left or right + if (gamepad != null && !(gamepad.getPOV() == -1 || gamepad.getPOV() % 180 == 0)) { + //Output the shift value + return gamepad.getPOV() < 180 ? dPadShift : -dPadShift; + } else { + //Return the throttle value if it's outside of the deadband. + if (scaleRotByFwdPoly != null) { + return rotThrottle.getValue() * scaleRotByFwdPoly.get(fwdThrottle.getValue()); + } + return rotThrottle.getValue(); + } + } +} diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017Arcade.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017Arcade.java deleted file mode 100644 index 196eac53..00000000 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017Arcade.java +++ /dev/null @@ -1,64 +0,0 @@ -package org.usfirst.frc.team449.robot.oi; - -import edu.wpi.first.wpilibj.Joystick; -import maps.org.usfirst.frc.team449.robot.oi.OI2017ArcadeMap; -import org.usfirst.frc.team449.robot.interfaces.oi.ArcadeOI; -import org.usfirst.frc.team449.robot.oi.components.SmoothedThrottle; -import org.usfirst.frc.team449.robot.oi.components.Throttle; - -/** - * A simple, two-stick arcade drive OI that uses two distinct joysticks - */ -public class OI2017Arcade extends ArcadeOI { - /** - * Left (rotation control) stick's throttle - */ - private Throttle rotThrottle; - - /** - * Right (fwd/rev control) stick's throttle - */ - private Throttle velThrottle; - - /** - * The map for this object. - */ - private OI2017ArcadeMap.OI2017Arcade map; - - /** - * Construct an OI2017Arcade - * - * @param map config map - */ - public OI2017Arcade(maps.org.usfirst.frc.team449.robot.oi.OI2017ArcadeMap.OI2017Arcade map) { - this.map = map; - - //Instantiate the sticks - Joystick _leftStick = new Joystick(map.getLeftStick()); - Joystick _rightStick = new Joystick(map.getRightStick()); - this.rotThrottle = new SmoothedThrottle(_leftStick, 1); - this.velThrottle = new SmoothedThrottle(_rightStick, 1); - } - - /** - * Map the buttons (call this after all subsytems are constructed) - */ - @Override - public void mapButtons() { - // Do nothing - } - - /** - * @return rotational velocity component - */ - public double getRot() { - return rotThrottle.getValue(); - } - - /** - * @return forward velocity component - */ - public double getFwd() { - return velThrottle.getValue(); - } -} diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017ArcadeGamepad.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017ArcadeGamepad.java deleted file mode 100644 index e8b6b749..00000000 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/oi/OI2017ArcadeGamepad.java +++ /dev/null @@ -1,453 +0,0 @@ -package org.usfirst.frc.team449.robot.oi; - -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.buttons.Button; -import maps.org.usfirst.frc.team449.robot.oi.JoystickButtonMap; -import maps.org.usfirst.frc.team449.robot.oi.OI2017ArcadeGamepadMap; -import org.usfirst.frc.team449.robot.Robot; -import org.usfirst.frc.team449.robot.drive.talonCluster.commands.JiggleRobot; -import org.usfirst.frc.team449.robot.drive.talonCluster.commands.NavXRelativeTTA; -import org.usfirst.frc.team449.robot.drive.talonCluster.commands.NavXTurnToAngle; -import org.usfirst.frc.team449.robot.interfaces.drive.shifting.ShiftingDrive; -import org.usfirst.frc.team449.robot.interfaces.drive.shifting.commands.*; -import org.usfirst.frc.team449.robot.interfaces.oi.ArcadeOI; -import org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.IntakeSubsystem; -import org.usfirst.frc.team449.robot.interfaces.subsystem.Intake.commands.ToggleIntaking; -import org.usfirst.frc.team449.robot.interfaces.subsystem.NavX.commands.OverrideNavX; -import org.usfirst.frc.team449.robot.interfaces.subsystem.Shooter.commands.ToggleShooting; -import org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands.TurnMotorOff; -import org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands.TurnMotorOffWithRequires; -import org.usfirst.frc.team449.robot.interfaces.subsystem.binaryMotor.commands.TurnMotorOn; -import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidForward; -import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.SolenoidReverse; -import org.usfirst.frc.team449.robot.interfaces.subsystem.solenoid.commands.ToggleSolenoid; -import org.usfirst.frc.team449.robot.mechanism.climber.commands.RunMotorWhileConditonMet; -import org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.FireShooter; -import org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.LoadShooter; -import org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.RackShooter; -import org.usfirst.frc.team449.robot.mechanism.topcommands.shooter.ResetShooter; -import org.usfirst.frc.team449.robot.oi.buttons.MappedJoystickButton; -import org.usfirst.frc.team449.robot.oi.components.SmoothedThrottle; -import org.usfirst.frc.team449.robot.oi.components.Throttle; -import org.usfirst.frc.team449.robot.util.Logger; -import org.usfirst.frc.team449.robot.vision.commands.ChangeCam; - -import java.util.ArrayList; -import java.util.List; - -/** - * An OI for using an Xbox-style controller for an arcade drive, where one stick controls forward velocity and the other - * controls turning velocity. - */ -public class OI2017ArcadeGamepad extends ArcadeOI { - - /** - * How much the D-pad moves the robot rotationally on a 0 to 1 scale, equivalent to pushing the turning stick that - * much of the way - */ - private double shift; - - /** - * The throttle wrapper for the stick controlling turning velocity - */ - private Throttle rotThrottle; - - /** - * The throttle wrapper for the stick controlling linear velocity - */ - private Throttle fwdThrottle; - - /** - * The controller with the drive sticks - */ - private Joystick gamepad; - - /** - * The joystick output under which any input is considered noise - */ - private double deadband; - - /** - * Button for turning to 0 degrees absolute heading (where robot was powered on) - */ - private Button turnTo0; - - /** - * Button for turning to 30 degrees absolute heading (where robot was powered on) - */ - private Button turnTo30; - - /** - * Button for turning to 180 degrees absolute heading (where robot was powered on) - */ - private Button turnTo180; - - /** - * Button for turning to 330 degrees absolute heading (where robot was powered on) - */ - private Button turnTo330; - - /** - * Button for a 180 degree relative turn - */ - private Button turnaround; - - /** - * Button for switching to low gear - */ - private Button switchToLowGear; - - /** - * Button for switching to high gear - */ - private Button switchToHighGear; - - /** - * Button for climbing - */ - private Button climb; - - /** - * Button for toggling whether to use NavX DriveStraight - */ - private Button toggleOverrideNavX; - - /** - * Button for toggling cameras - */ - private Button switchCamera; - - /** - * Button for toggling intake on/off - */ - private Button toggleIntake; - - /** - * Button for toggling intake up/down - */ - private Button toggleIntakeUpDown; - - /** - * Button held to stay in low gear - */ - private Button tmpOverrideLow; - - /** - * Button held to stay in high gear - */ - private Button tmpOverrideHigh; - - /** - * Button for toggling autoshifting - */ - private Button toggleOverrideAutoshift; - - /** - * Button for toggling shooter - */ - private Button toggleShooter; - - /** - * Button for running the LoadShooter command group (intake balls) - */ - private Button loadShooter; - - /** - * Button for running the RackShooter command group (set up to shoot) - */ - private Button rackShooter; - - /** - * Button for running the FireShooter command group (fire the shooter) - */ - private Button fireShooter; - - /** - * Button for running the ResetShooter command group (turn everything off) - */ - private Button resetShooter; - - /** - * Button for toggling whether the gear handler is open or closed. - */ - private Button toggleGear; - - /** - * Buttons that open the gear handler when pressed and close it when released. - */ - private List