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.*;
 import frc.robot.sciSensorsActuators.SciJoystick;
 
@@ -20,7 +20,7 @@ public OI() {
         xboxController = new XboxController(PortMap.XBOX_CONTROLLER);
 
         circleControllerButton = new JoystickButton(leftStick, PortMap.JOYSTICK_TRIGGER);
-        circleControllerButton.whileActive(new CircleControllerCommand());
+        circleControllerButton.whileActiveContinuous(new CircleControllerCommand());
 
         pointOneButton = new JoystickButton(leftStick, PortMap.JOYSTICK_LEFT_BUTTON);
         pointOneButton.whenPressed(new PointOneCommand());

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

@@ -20,7 +20,7 @@
 import frc.robot.sciSensorsActuators.SciSpark;
 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.*;
 
 public class Robot extends TimedRobot {
@@ -166,7 +166,7 @@ public void robotPeriodic() {
         allPeriodicLogs();
         logDataPeriodic();
         DelayedPrinter.print("x: " + getPos().x + "\ty: " + getPos().y + "\nheading: " + getHeading() + "\npigeon angle: " + Robot.get(SD.PigeonAngle));
-        Scheduler.getInstance().run();
+        CommandScheduler.getInstance().run();
         DelayedPrinter.incTicks();
     }
 
@@ -191,7 +191,7 @@ public void teleopInit() {
     }
 
     public void teleopPeriodic() {
-        (new TankDriveCommand()).start();
+        CommandScheduler.getInstance().schedule(new TankDriveCommand());
     }
 
 

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

@@ -2,7 +2,7 @@
 
 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;
 
@@ -12,7 +12,7 @@ public class CircleControllerCommand extends InstantCommand {
     CircleController circleController = new CircleController();
 
     @Override
-    protected void execute() {
+    public void execute() {
         Robot.logger.logCommandStatus(FILENAME, CommandStatus.Executing);
         circleController.update(Robot.CURRENT_DESTINATION, Robot.CURRENT_DESTINATION_HEADING);
     }

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

@@ -1,6 +1,6 @@
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.Robot;
 import frc.robot.logging.Logger.CommandStatus;
 
@@ -9,11 +9,11 @@ public class IntakeReleaseCommand extends InstantCommand {
     private final String FILENAME = "IntakeReleaseCommand.java";
 
     public IntakeReleaseCommand() {
-        requires(Robot.intakeSubsystem);
+        addRequirements(Robot.intakeSubsystem);
     }
 
     @Override 
-    protected void execute() {
+    public void execute() {
 	    Robot.logger.logCommandStatus(FILENAME, CommandStatus.Executing);
         Robot.intakeSubsystem.stop();
     }

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

@@ -1,19 +1,19 @@
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.Robot;
 import frc.robot.logging.Logger.CommandStatus;
 
-public class IntakeSuckCommand extends InstantCommand {
+public class IntakeSuckCommand extends CommandBase {
 
     private final String FILENAME = "IntakeSuckCommand.java";
 
     public IntakeSuckCommand() {
-        requires(Robot.intakeSubsystem);
+        addRequirements(Robot.intakeSubsystem);
     }
 
     @Override 
-    protected void execute() {
+    public void execute() {
 	    Robot.logger.logCommandStatus(FILENAME, CommandStatus.Executing);
         Robot.intakeSubsystem.suck();
     }

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

@@ -1,11 +1,11 @@
 package frc.robot.commands;
 
 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/PointThreeCommand.java

@@ -1,11 +1,11 @@
 package frc.robot.commands;
 
 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/PointTwoCommand.java

@@ -1,11 +1,11 @@
 package frc.robot.commands;
 
 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/SwerveTankDriveCommand.java

@@ -3,15 +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 {
     private final String FILENAME = "SwerveTankDriveCommand.java";
-
-
-    public SwerveTankDriveCommand(){}
-
-    @Override protected void execute() {
+    @Override
+    public void execute() {
         Robot.logger.logCommandStatus(FILENAME, CommandStatus.Executing);
 
         // One controller controls turning percent, one controls velocity

src/main/java/frc/robot/commands/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;
 
 public class TankDriveCommand extends InstantCommand {
     private final String FILENAME = "TankDriveCommand.java";
     @Override 
-    protected void execute() {
+    public void execute() {
         Robot.driveSubsystem.manualDriveMode();
 	    Robot.logger.logCommandStatus(FILENAME, CommandStatus.Executing);
         Robot.driveSubsystem.setSpeed(Robot.oi.leftStick, Robot.oi.rightStick);

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

@@ -3,12 +3,13 @@
 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 {
     private final String FILENAME = "SwerveTankDriveCommand.java";
 
-    @Override protected void execute() {
+    @Override
+    public void execute() {
         Robot.logger.logCommandStatus(FILENAME, 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,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.SciSpark;
 
-public class SciSparkSpeedCommand extends Command {
+public class SciSparkSpeedCommand extends CommandBase {
 
     private SciSpark spark;
     private double goalSpeed;
@@ -18,8 +21,8 @@ public SciSparkSpeedCommand(SciSpark spark, int commandNumber){
     }
 
     @Override
-    protected void execute(){this.spark.instantSet();}
+    public void execute(){this.spark.instantSet();}
     @Override
-    protected boolean isFinished(){return this.spark.atGoal();}
+    public boolean isFinished(){return this.spark.atGoal();}
 
 }

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/sciSensorsActuators/SciSpark.java

@@ -2,6 +2,8 @@
 
 import com.revrobotics.CANSparkMax;
 
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
 import java.util.Optional;
 
 import frc.robot.Robot;
@@ -81,7 +83,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.helpers.DelayedPrinter;
@@ -69,7 +70,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

@@ -10,9 +10,9 @@
 import frc.robot.sciSensorsActuators.*;
 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 {
+public class DriveSubsystem implements Subsystem {
     // 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;
@@ -149,9 +149,4 @@ 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
-    }
 }

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

@@ -1,6 +1,6 @@
 package frc.robot.subsystems;
 
-import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 import frc.robot.PortMap;
@@ -9,7 +9,7 @@
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 
 
-public class IntakeSubsystem extends Subsystem {
+public class IntakeSubsystem implements Subsystem {
     DoubleSolenoid upDownSolenoid;
     public SciTalon intakeMotor;
 
@@ -33,9 +33,4 @@ public void setIntakeSpeed(double speed) {
     public void suck() {setIntakeSpeed(INTAKE_SPEED);}
     public void stop() {setIntakeSpeed(0);}
 
-    @Override 
-    protected void initDefaultCommand() {
-        // Useless
-    }
-
 }

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

@@ -1,11 +1,11 @@
 package frc.robot.subsystems;
 
-import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.Subsystem;
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 
-public class LimelightSubsystem extends Subsystem{
+public class LimelightSubsystem implements Subsystem{
 
     public final static double IMAGE_WIDTH = Math.toRadians(27.); // In degrees
     public final static double IMAGE_HEIGHT = Math.toRadians(20.5); // In degrees
@@ -30,9 +30,4 @@ public boolean contourExists(){
 
     public void periodicLog(){
     }
-
-    @Override
-    protected void initDefaultCommand() {
-        // LITTERALLY DIE
-    }
 }

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

@@ -5,10 +5,10 @@
 import frc.robot.robotState.RobotStateUpdater;
 import frc.robot.robotState.RobotState.SD;
 import frc.robot.sciSensorsActuators.SciPigeon;
-import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.Subsystem;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 
-public class PigeonSubsystem extends Subsystem {
+public class PigeonSubsystem implements Subsystem {
   // for the main pigeon on the robot
     public  SciPigeon pigeon;
     private TalonSRX pigeonTalon;
@@ -20,9 +20,4 @@ public PigeonSubsystem () {
     }
 
     public SciPigeon getPigeon() {return this.pigeon;}
-
-    @Override
-    protected void initDefaultCommand() {
-		//IGNORE THIS METHOD
-    }
 }