Changes between Version 1 and Version 2 of ControlSystems/SoftwareTeam/Training/WPILib/CommandGroupProgramming

Timestamp:

Feb 27, 2020, 10:59:02 PM (5 years ago)

Author:

David Albert

Comment:

--

ControlSystems/SoftwareTeam/Training/WPILib/CommandGroupProgramming

@@ -1 +1 @@
 The last example showed two simple commands and how to connect them with the user interface, but the commands were simple: they set a servo position and immediately finished.  In practice, robot commands often need to run continuously until something happens; for example, a command to raise an arm might start a motor moving and continue until some time has passed or an encoder indicates that the arm has moved a certain distance or a limit switch is pressed.  WPILib commands make this easy.  Even better, you can string several such commands together in a !CommandGroup to effect complex behaviors.
+
+Commands can be grouped together to run in sequence for example, when the X button is pressed, the following code will run the servo open command, allow time for it to finish, and then run the servo close command:
+
+Constants.java
+{{{
+package frc.robot;
+
+public final class Constants {
+    public static int SERVO_PORT = 0;   // PWM port for servo motor
+    public static int OPEN_BUTTON = 1;  // A button opens mechanism
+    public static int CLOSE_BUTTON = 2; // B button closes mechanism   
+    public static int OPEN_THEN_CLOSE_BUTTON = 3; // X button opens then closes
+}
+}}}
+
+ServoSubSystem.java:
+{{{
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import edu.wpi.first.wpilibj.Servo;
+
+public class ServoSubsystem extends SubsystemBase {
+  private Servo servo;
+  private static ServoSubsystem instance = null;
+
+  private ServoSubsystem() {
+      servo = new Servo(Constants.SERVO_PORT);
+  }
+
+  // Singleton design pattern
+  public static ServoSubsystem getInstance() {
+      if (instance == null) {
+          instance = new ServoSubsystem();
+      }
+      return instance;
+  }
+
+  public void setAngle(double degrees) {
+    servo.setAngle(degrees);
+  }
+}
+}}}
+
+OpenCommand.java
+{{{
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.ServoSubsystem;
+import edu.wpi.first.wpilibj.Timer;
+
+public class OpenCommand extends CommandBase {
+  private Timer delayTimer;
+
+  public OpenCommand(SubsystemBase servoSubsystem) {
+      delayTimer = new Timer();
+      addRequirements(servoSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    ServoSubsystem.getInstance().setAngle(30); // set servo to open position
+    delayTimer.start();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return delayTimer.hasPeriodPassed(0.5); // allow 500ms for command to finish
+  }
+}
+}}}
+
+CloseCommand.java
+{{{
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.ServoSubsystem;
+
+public class CloseCommand extends CommandBase {
+
+    public CloseCommand(SubsystemBase servoSubsystem) {
+        addRequirements(servoSubsystem);
+    }
+
+    @Override
+    public boolean isFinished() {
+        ServoSubsystem.getInstance().setAngle(120); // set servo to closed position
+        return true;                 // and then finish
+    }
+}
+}}}
+
+OpenThenCloseCommand.java:
+{{{
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class OpenThenCloseCommand extends SequentialCommandGroup {
+
+    public OpenThenCloseCommand(SubsystemBase servoSubsystem) {
+        this.addCommands(new OpenCommand(servoSubsystem), new CloseCommand(servoSubsystem));
+    }
+}
+}}}
+
+RobotContainer.java:
+{{{
+package frc.robot;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.button.Button;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ServoSubsystem;
+import frc.robot.commands.OpenCommand;
+import frc.robot.commands.CloseCommand;
+import frc.robot.commands.OpenThenCloseCommand;
+
+public class RobotContainer {
+  private final ServoSubsystem m_servo = ServoSubsystem.getInstance();
+  private final XboxController xbox;
+  private final Button openButton, closeButton, openThenCloseButton;
+
+  public RobotContainer() {
+    xbox = new XboxController(0);
+    openButton = new JoystickButton(xbox, Constants.OPEN_BUTTON);
+    closeButton = new JoystickButton(xbox, Constants.CLOSE_BUTTON);   
+    openThenCloseButton = new JoystickButton(xbox, Constants.OPEN_THEN_CLOSE_BUTTON);
+
+    // Configure the button bindings
+    configureButtonBindings();
+  }
+
+  private void configureButtonBindings() {
+    openButton.whenPressed(new OpenCommand(m_servo));
+    closeButton.whenPressed(new CloseCommand(m_servo));
+    openThenCloseButton.whenPressed(new OpenThenCloseCommand(m_servo));
+  }
+
+  public Command getAutonomousCommand() {
+    return null;
+  }
+}
+}}}