Migrate to wpilibj2 and the new Command-based structure

src/main/java/frc/robot/OI.java

@@ -2,7 +2,7 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.commands.auto.*;
 import frc.robot.commands.intake.*;
 import frc.robot.commands.drive.*;
@@ -22,7 +22,7 @@ public OI() {
         xboxController = new XboxController(PortMap.XBOX_CONTROLLER);
 
         circleControllerButton = new JoystickButton(leftStick, PortMap.JOYSTICK_TRIGGER);
-        circleControllerButton.whileActive(new CircleControllerCommand());
+        circleControllerButton.whileActiveContinuous(new CircleControllerCommand());
 
         pointThreeButton = new JoystickButton(leftStick, PortMap.JOYSTICK_CENTER_BUTTON);
         pointThreeButton.whenPressed(new PointThreeCommand());       

src/main/java/frc/robot/Robot.java

@@ -20,7 +20,7 @@
 import frc.robot.robotState.RobotState.SD;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.controllers.*;
 import frc.robot.stateEstimation.interfaces.*;
 import frc.robot.stateEstimation.higherLevel.*;
@@ -174,7 +174,7 @@ public void robotPeriodic() {
         stateHistory.addState(getState().copy());
         allUpdateRobotStates();
         allModels();
-        Scheduler.getInstance().run();
+        CommandScheduler.getInstance().run();
         DelayedPrinter.incTicks();
     }
 
@@ -203,14 +203,14 @@ public void teleopInit() {
     }
 
     public void teleopPeriodic() {
-        (new TankDriveCommand()).start();    
+        CommandScheduler.getInstance().schedule(new TankDriveCommand());
         allPeriodicLogs();
         logDataPeriodic();
     }
 
 
     public void testPeriodic() {
-        (new TankDriveCommand()).start();
+        CommandScheduler.getInstance().schedule(new TankDriveCommand());
         Robot.driveSubsystem.l.diminishSnap();
         Robot.driveSubsystem.r.diminishSnap();
         DelayedPrinter.print("testing...");

src/main/java/frc/robot/commands/auto/CircleControllerCommand.java

@@ -2,15 +2,15 @@
 
 import frc.robot.Robot;
 import frc.robot.controllers.CircleController;
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.logging.Logger.CommandStatus;
 import frc.robot.helpers.DelayedPrinter;
 
 public class CircleControllerCommand extends InstantCommand {
     CircleController circleController = new CircleController();
 
     @Override
-    protected void execute() {
+    public void execute() {
         Robot.logger.logCommandStatus(CommandStatus.Executing);
         circleController.update(Robot.CURRENT_DESTINATION, Robot.CURRENT_DESTINATION_HEADING);
     }

src/main/java/frc/robot/commands/auto/PointOneCommand.java

@@ -1,11 +1,11 @@
 package frc.robot.commands.auto;
 
 import frc.robot.Robot;
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public class PointOneCommand extends InstantCommand {
     @Override
-    protected void execute() {
+    public void execute() {
         Robot.CURRENT_DESTINATION = Robot.newDestPoint;
         Robot.CURRENT_DESTINATION_HEADING = Robot.newDestHeading;
     }

src/main/java/frc/robot/commands/auto/PointThreeCommand.java

@@ -1,11 +1,11 @@
 package frc.robot.commands.auto;
 
 import frc.robot.Robot;
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public class PointThreeCommand extends InstantCommand {
     @Override
-    protected void execute() {
+    public void execute() {
         Robot.CURRENT_DESTINATION = Robot.TEST_POINT_3.point;
         Robot.CURRENT_DESTINATION_HEADING = Robot.TEST_POINT_3.heading;
     }

src/main/java/frc/robot/commands/auto/PointTwoCommand.java

@@ -1,11 +1,11 @@
 package frc.robot.commands.auto;
 
 import frc.robot.Robot;
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public class PointTwoCommand extends InstantCommand {
     @Override
-    protected void execute() {
+    public void execute() {
         Robot.CURRENT_DESTINATION = Robot.TEST_POINT_2.point;
         Robot.CURRENT_DESTINATION_HEADING = Robot.TEST_POINT_2.heading;
     }

src/main/java/frc/robot/commands/drive/SwerveTankDriveCommand.java

@@ -3,12 +3,12 @@
 import frc.robot.Robot;
 
 import frc.robot.logging.Logger.CommandStatus;
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public class SwerveTankDriveCommand extends InstantCommand {
     public SwerveTankDriveCommand(){}
 
-    @Override protected void execute() {
+    @Override public void execute() {
         Robot.logger.logCommandStatus(CommandStatus.Executing);
 
         // One controller controls turning percent, one controls velocity

src/main/java/frc/robot/commands/drive/TankDriveCommand.java

@@ -2,12 +2,12 @@
 
 import frc.robot.Robot;
 import frc.robot.logging.Logger.CommandStatus;
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.helpers.DelayedPrinter;
 
 public class TankDriveCommand extends InstantCommand {
     @Override 
-    protected void execute() {
+    public void execute() {
         Robot.driveSubsystem.manualDriveMode();
 	    Robot.logger.logCommandStatus(CommandStatus.Executing);
         Robot.driveSubsystem.setSpeed(Robot.oi.leftStick, Robot.oi.rightStick);

src/main/java/frc/robot/commands/drive/TorqueDriveCommand.java

@@ -3,10 +3,10 @@
 import frc.robot.Robot;
 
 import frc.robot.logging.Logger.CommandStatus;
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public class TorqueDriveCommand extends InstantCommand {
-    @Override protected void execute() {
+    @Override public void execute() {
         Robot.logger.logCommandStatus(CommandStatus.Executing);
         // One controller controls turning percent, one controls velocity
         double forward = Robot.oi.leftStick.getProcessedY();

src/main/java/frc/robot/commands/generalCommands/SciSparkSpeedCommand.java

@@ -1,9 +1,9 @@
 package frc.robot.commands.generalCommands;
 
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.sciSensorsActuators.SciSpark;
 
-public class SciSparkSpeedCommand extends Command {
+public class SciSparkSpeedCommand extends CommandBase {
 
     private SciSpark spark;
     private double goalSpeed;
@@ -18,9 +18,9 @@ public SciSparkSpeedCommand(SciSpark spark, int commandNumber){
     }
 
     @Override
-    protected void execute(){this.spark.instantSet();}
+    public void execute(){this.spark.instantSet();}
     @Override
     //protected boolean isFinished(){return true;}
-    protected boolean isFinished(){return this.spark.atGoal() || !this.spark.isCurrentCommandNumber(this.commandNunber);}
+    public boolean isFinished(){return this.spark.atGoal() || !this.spark.isCurrentCommandNumber(this.commandNunber);}
 
 }

src/main/java/frc/robot/commands/generalCommands/SciTalonSpeedCommand.java

@@ -1,9 +1,12 @@
 package frc.robot.commands.generalCommands;
 
-import edu.wpi.first.wpilibj.command.Command;
+import java.util.HashSet;
+import java.util.Set;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.sciSensorsActuators.SciTalon;
 
-public class SciTalonSpeedCommand extends Command {
+public class SciTalonSpeedCommand extends CommandBase {
 
     private SciTalon talon;
     private double goalSpeed;
@@ -18,12 +21,10 @@ public SciTalonSpeedCommand(SciTalon talon, int commandNumber){
     }
 
     @Override
-    protected void execute(){this.talon.instantSet();}
+    public void execute(){this.talon.instantSet();}
     @Override
-    protected boolean isFinished(){
+    public boolean isFinished(){
         return this.talon.atGoal() || !this.talon.isCurrentCommandNumber(this.commandNunber);
     }
-    @Override
-    protected void end(){ }
 
 }

src/main/java/frc/robot/commands/generalCommands/SparkDelayWarningCommand.java

@@ -1,10 +1,10 @@
 package frc.robot.commands.generalCommands;
 
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.sciSensorsActuators.SciSpark;
 import frc.robot.Utils;
 
-public class SparkDelayWarningCommand extends Command {
+public class SparkDelayWarningCommand extends CommandBase {
 
     private SciSpark sciSpark;
     private double input;
@@ -17,7 +17,7 @@ public SparkDelayWarningCommand (final SciSpark sciSpark, double input) {
     }
 
     @Override 
-    protected void execute () {
+    public void execute() {
         ticks++;
         if (!Utils.impreciseEquals(this.sciSpark.get(), this.input)) {
             System.out.println("WARNING: " + this.sciSpark.getDeviceName() + " was set to " + this.input
@@ -27,7 +27,7 @@ protected void execute () {
     }
 
     @Override
-    protected boolean isFinished () { 
+    public boolean isFinished() { 
         return !Utils.impreciseEquals(this.sciSpark.get(), this.input); 
     }
 

src/main/java/frc/robot/commands/generalCommands/TalonDelayWarningCommand.java

@@ -1,10 +1,10 @@
 package frc.robot.commands.generalCommands;
 
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.sciSensorsActuators.SciTalon;
 import frc.robot.Utils;
 
-public class TalonDelayWarningCommand extends Command {
+public class TalonDelayWarningCommand extends CommandBase {
 
     private SciTalon sciTalon;
     private double input;
@@ -17,7 +17,7 @@ public TalonDelayWarningCommand (final SciTalon sciTalon, double input) {
     }
 
     @Override 
-    protected void execute () {
+    public void execute () {
         ticks++;
         if (!Utils.impreciseEquals(this.sciTalon.getMotorOutputPercent(), this.input)) {
             System.out.println("WARNING: " + this.sciTalon.getDeviceID() + " was set to " + this.input
@@ -27,7 +27,7 @@ protected void execute () {
     }
 
     @Override
-    protected boolean isFinished () { 
+    public boolean isFinished () { 
         return !Utils.impreciseEquals(this.sciTalon.getMotorOutputPercent(), this.input); 
     }
 

src/main/java/frc/robot/commands/intake/IntakeReleaseCommand.java

@@ -1,16 +1,16 @@
 package frc.robot.commands.intake;
 
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.Robot;
 import frc.robot.logging.Logger.CommandStatus;
 
 public class IntakeReleaseCommand extends InstantCommand {
     public IntakeReleaseCommand() {
-        requires(Robot.intakeSubsystem);
+        addRequirements(Robot.intakeSubsystem);
     }
 
     @Override 
-    protected void execute() {
+    public void execute() {
 	    Robot.logger.logCommandStatus(CommandStatus.Executing);
         Robot.intakeSubsystem.stop();
     }

src/main/java/frc/robot/commands/intake/IntakeSuckCommand.java

@@ -1,16 +1,16 @@
 package frc.robot.commands.intake;
 
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.Robot;
 import frc.robot.logging.Logger.CommandStatus;
 
 public class IntakeSuckCommand extends InstantCommand {
     public IntakeSuckCommand() {
-        requires(Robot.intakeSubsystem);
+        addRequirements(Robot.intakeSubsystem);
     }
 
     @Override 
-    protected void execute() {
+    public void execute() {
 	    Robot.logger.logCommandStatus(CommandStatus.Executing);
         Robot.intakeSubsystem.suck();
     }

src/main/java/frc/robot/sciSensorsActuators/SciSpark.java

@@ -2,6 +2,8 @@
 
 import com.revrobotics.CANSparkMax;
 
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
 import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.Optional;
@@ -108,7 +110,7 @@ public void instantSet() {
 
     public void checkWarningStatus(double input, double realOutput){
         if (!Utils.inRange(input, super.get(), TOLERABLE_DIFFERENCE)) {
-            (new SparkDelayWarningCommand(this, input)).start();
+            CommandScheduler.getInstance().schedule(new SparkDelayWarningCommand(this, input));
         }   
     }
 
@@ -134,7 +136,7 @@ public void set(double speed, double maxJerk) {
         this.commandNumber++;
         // Set will call this command, which will continue to call instantSet
         // InstantSet will only set the value of the motor to the correct value if it is within maxJerk
-        (new SciSparkSpeedCommand(this, this.commandNumber)).start();
+        CommandScheduler.getInstance().schedule(new SciSparkSpeedCommand(this, this.commandNumber));
     }
     public void set(double speed) {set(speed, DEFAULT_MAX_JERK);}
 

src/main/java/frc/robot/sciSensorsActuators/SciTalon.java

@@ -3,6 +3,7 @@
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.Utils;
 import frc.robot.commands.generalCommands.SciTalonSpeedCommand;
 import frc.robot.commands.generalCommands.TalonDelayWarningCommand;
@@ -60,7 +61,7 @@ public void instantSet() {
     }
     public void checkWarningStatus(double input, double realOutput){
         if (!Utils.inRange(input, super.getMotorOutputPercent(), TOLERABLE_DIFFERENCE)) {
-            (new TalonDelayWarningCommand(this, input)).start();
+            CommandScheduler.getInstance().schedule(new TalonDelayWarningCommand(this, input));
         }   
     }
 
@@ -71,7 +72,7 @@ public void set(double speed, double maxJerk){
         this.commandNumber++;
         // Set will call this command, which will continue to call instantSet
         // InstantSet will only set the value of the motor to the correct value if it is within maxJerk
-        (new SciTalonSpeedCommand(this, this.commandNumber)).start();
+        CommandScheduler.getInstance().schedule(new SciTalonSpeedCommand(this, this.commandNumber));
     }
     public void set(double speed) {set(speed, DEFAULT_MAX_JERK);}
 

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -12,9 +12,9 @@
 import frc.robot.logging.LogUpdater;
 import frc.robot.logging.Logger.DefaultValue;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.Subsystem;
 
-public class DriveSubsystem extends Subsystem implements LogUpdater {
+public class DriveSubsystem implements Subsystem, LogUpdater {
     // Define tested error values here
     double TANK_ANGLE_P = .075, TANK_ANGLE_D = 0.0, TANK_ANGLE_I = 0;
     double TANK_SPEED_LEFT_P  = .1, TANK_SPEED_LEFT_D  = 0.0, TANK_SPEED_LEFT_I  = 0;
@@ -161,12 +161,7 @@ public void setSpeedTankForwardTurningMagnitude(double forward, double turnMagni
         // Note: this controls dtheta/dt rather than dtheta/dx
         setTank(forward - turnMagnitude, forward + turnMagnitude);
     }
-
-    @Override
-    protected void initDefaultCommand() {
-		//IGNORE THIS METHOD
-    }
-
+
     @Override 
     public void periodicLog(){}
 }