Now that we have commands to drive and turn let’s see how we can combine them to create more complex commands. Let’s create a new command that will drive the robot around a certain path. The first step is to create a new command called DriveCourseCommand. This type of command has a different structure so this time instead of copying the ExampleCommand we are going to make this new class inherit from SequentialCommandGroup instead of Command.
To create the new class, right click on the commands folder, choose New File, and enter the name DriveCourseCommand.java. Then replace the text in that file with:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 |
package frc.robot.commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.subsystems.DriveSubsystem; import robotCore.Logger; public class DriveCourseCommand extends SequentialCommandGroup { /** * Creates a new NewCommand. */ private final DriveSubsystem m_subsystem; public DriveCourseCommand(DriveSubsystem subsystem) { Logger.log("DriveCourseCommand", 3, "DriveCourseCommand()"); m_subsystem = subsystem; } } |
Key differences in what we’ve been doing is that 1) we inherit from SequentialCommandGroup and 2) we do not have the usual command functions (e.g. initialize, execute, etc.). This is because this command will be composed of a set of commands (a composite command).
To start, let’s say we want to drive forward 20 inches and then turn right by 90 degrees, we would add the following lines to the constructor:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
public DriveCourseCommand(DriveSubsystem subsystem) { Logger.log("DriveCourseCommand", 3, "DriveCourseCommand()"); m_subsystem = subsystem; double turn90 = 90; double speed = 0.3; double wait = 0.5; addCommands( new DriveForDistanceCommand(m_subsystem, speed, 20), new WaitCommand(wait), new TurnCommand(m_subsystem, speed, turn90)); } |
Notice how I placed the built in WaitCommand between the DriveForDistanceCommand and TurnCommand. The WaitCommand does nothing but wait for a specified time. Adding a small wait between the commands will increase the accuracy of the turns. Transitioning directly from driving to turning can cause errors like wheel slippage which will throw off our turn calculations. I am also using variables turn90, speed, and wait rather than specifying the numbers directly in the respective constructors. I am doing this since we will be adding multiple commands that use these numbers and defining them all in one place will make it easier for us to adjust the numbers if needed.
Now modify the RobotContainer class so that we run this new command when we press our test button 6.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 |
/*----------------------------------------------------------------------------*/ /* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ package frc.robot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import frc.robot.commands.DriveForTimeCommand; import frc.robot.commands.TestMotorSpeedCommand; import frc.robot.commands.TurnCommand; import frc.robot.commands.ArcadeDriveCommand; import frc.robot.commands.CalibrateSpeedCommand; import frc.robot.commands.DriveCourseCommand; import frc.robot.commands.DriveForDistanceCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ExampleSubsystem; /** * This class is where the bulk of the robot should be declared. Since * Command-based is a "declarative" paradigm, very little robot logic should * actually be handled in the {@link Robot} periodic methods (other than the * scheduler calls). Instead, the structure of the robot (including subsystems, * commands, and button mappings) should be declared here. */ public class RobotContainer { // The robot's subsystems and commands are defined here... @SuppressWarnings("unused") private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); private final CommandJoystick m_joystick = new CommandJoystick(0); private final ArcadeDriveCommand m_autoCommand = null; // new ExampleCommand(m_exampleSubsystem); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { m_driveSubsystem.setDefaultCommand(new ArcadeDriveCommand(m_driveSubsystem, m_joystick)); // Configure the button bindings configureButtonBindings(); } /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { m_joystick.button(1).onTrue(new DriveForTimeCommand(m_driveSubsystem, 0.50, 3.0)); m_joystick.button(2).onTrue(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 30)); m_joystick.button(3).onTrue(new TestMotorSpeedCommand(m_driveSubsystem)); m_joystick.button(4).whileTrue(new CalibrateSpeedCommand(m_driveSubsystem)); m_joystick.button(5).onTrue(new TurnCommand(m_driveSubsystem, 0.3, -180)); m_joystick.button(6).onTrue(new DriveCourseCommand(m_driveSubsystem)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous return m_autoCommand; } } |
Now deploy and run your program and verify that it drives forward 20 inches, turns right and then stops. Adjust the turn90 variable so that the turn is correct.
Now let’s change your program so that the robot will drive in a rectangular pattern 20 inches by 10 inches. In principle this should bring your robot back to the starting position. After you have completed and tested your code, compare it to my solution below.
.
.
.
.
.
.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 |
package frc.robot.commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.DriveSubsystem; import robotCore.Logger; public class DriveCourseCommand extends SequentialCommandGroup { /** * Creates a new NewCommand. */ private final DriveSubsystem m_subsystem; public DriveCourseCommand(DriveSubsystem subsystem) { Logger.log("DriveCourseCommand", 3, "DriveCourseCommand()"); m_subsystem = subsystem; double turn90 = 85; double speed = 0.3; double wait = 0.5; addCommands( new DriveForDistanceCommand(m_subsystem, speed, 20), new WaitCommand(wait), new TurnCommand(m_subsystem, speed, turn90), new WaitCommand(wait), new DriveForDistanceCommand(m_subsystem, speed, 10), new WaitCommand(wait), new TurnCommand(m_subsystem, speed, turn90), new WaitCommand(wait), new DriveForDistanceCommand(m_subsystem, speed, 20), new WaitCommand(wait), new TurnCommand(m_subsystem, speed, turn90), new WaitCommand(wait), new DriveForDistanceCommand(m_subsystem, speed, 10), new WaitCommand(wait), new TurnCommand(m_subsystem, speed, turn90)); } } |
Note that in my case I needed set the turn90 to 85 degrees instead of 90 in order to get the robot to drive the correct path. You will probably find that it was very hard to set the speed and turn parameters correctly to bring the robot back to the exact starting position. In fact, you probably saw that running the same program twice in a row actually produced a slightly different outcome. This is because there are a number of things that go on which introduce slight errors (such as wheel slippage), and these errors accumulate over time and are different every time. You might find that if you slow the robot down, you can get more reproducible results, but you can never reach the point where it is always spot on.
The important take-away from this: This kind of ‘dead reckoning’ navigation is fraught with problems. The only way to overcome these problems is to use sensors (e.g. gyro, distance sensor, camera, etc.) on the robot that give you a way to determine it’s absolute position and orientation by observing and using the environment around it. This is how the competition robots determine their position on the field.