Test Bench Exercises

Test Bench

The Test Bench was created to allow students wanting to learn FRC robot programming a way to do example problems without needing access to a functioning robot. As you read these examples they will refer to terms and/or concepts that will not be familiar to you. Stop and lookup what these unfamiliar terms/concepts mean as you go along.

The Test Bench uses a roboRIO and a CTRE Power Distribution Panel (clink on the links and read about these things!). It has two motors attached to it, a brushed motor (NeveRest with 40:1 gearbox) and a brushless motor (a NEO 550) connected to two different motor controllers (a SPARK and a SparkMAX). The SPARK motor controller is controlled with PWM and the SparkMAX is controlled over the CAN bus of the roboRIO.

The NeveRest motor connected to the large disc is used in the position control exercises and refered to as the "Position Control" motor. It is connected to the SPARK motor controller which is wired into the roboRIO PWM channel 0. The position control motor has a SRX Mag Encoder attached to its output shaft. The SRX is wired into the roboRIO DIO channels 1, 2, and 0. Channels 1 & 2 are the Quadrature A and B inputs and channel 0 is the absolute PWM signal.

The NEO 550 motor that is mounted on the short bracket is used in the velocity control exercises and refered to as the "Velocity Control" motor. It is connected to the SparkMAX motor controller which is connected to the roboRIO CAN bus with ID 1. The NEO 550 has a built-in encoder.

The Test Bench does not have a WiFi radio so the coding computer must be connected to the roboRIO with a USB cable (USB A to USB B). The Test Bench must also be powered by a robot battery.

Figure 1. The Test Bench

Simple Motor Percent Output

Creating a Blank Project

In this example we will be creating a "Timed Robot" program to set the position motor’s percent output based on the position of a controller joystick.

Launch "WPILib VS Code" from the desktop. This runs VS Code with the WPILib libraries and tools enabled. Follow these directions to create a blank project from the "Timed Skeleton (Advanced)" template or see the animation below.

You should now have an empty project. The two files that we will be working with are src/main/include/Robot.h and src/main/cpp/Robot.cpp. Find those files in the Explorer on the left sidebar of VS Code and open Robot.h. It contains the Robot class which declares (See Forward Declarations and Definitions) the Robot-, Autonomous-, Teleop-, Disabled-, Test-, and Simulation- Init() and Periodic() functions.

Figure 2. Create a Blank Timed Robot Project

Contents of Generated Robot.h

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#pragma once

#include <frc/TimedRobot.h>

class Robot : public frc::TimedRobot {
 public:
  void RobotInit() override;
  void RobotPeriodic() override;

  void AutonomousInit() override;
  void AutonomousPeriodic() override;

  void TeleopInit() override;
  void TeleopPeriodic() override;

  void DisabledInit() override;
  void DisabledPeriodic() override;

  void TestInit() override;
  void TestPeriodic() override;

  void SimulationInit() override;
  void SimulationPeriodic() override;
};

Adding Controller and Motor Variables

For this simple example we won’t need most of the Init() and Periodic() functions so delete all the declared functions except RobotInit(), RobotPeriodic() and TeleopPeriodic(). RobotInit() runs once when the code first starts, RobotPeriodic() runs every 20 milliseconds at all times, and TeleopPeriodic() runs every 20 milliseconds while the Driver Station is in TeleOperated Mode.

We need to add member variables to the Robot class for our motor controller and for a joystick. The motor controller on the position motor is a SPARK controller so we will be using the `frc::Spark` class. Open the `frc::Spark` class API documentation which will show what methods are available with the `frc::Spark` class and what header needs to be included to use the class. In the API documentation for `frc::Spark` only the constructor that takes the PWM channel is shown along with the default constructor and the assignment operator. Expand the Public Member Functions inherited from frc::PWMMotorController section to see more member functions that are available that are inherited from the frc::PWMMotorController class which is a base class of `frc::Spark` (See Object Orientation). In Robot.h add a private section to the Robot class that declares a `frc::Spark` variable and initialized the channel to one. Also add a frc::XboxController variable initialized to port zero. You also need to add the two header files that are needed for the new classes using the #include directive. The headers are <frc/motorcontrol/Spark.h> and <frc/XboxController.h>.

Robot.h after changes

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#pragma once

#include <frc/TimedRobot.h>
#include <frc/motorcontrol/Spark.h>
#include <frc/XboxController.h>

class Robot : public frc::TimedRobot {
 public:
  void RobotInit() override;
  void RobotPeriodic() override;
  void TeleopPeriodic() override;
 private:
  frc::Spark m_motor{0};
  frc::XboxController m_controller{0};
};

Setting up Controller—Motor Connection

The src/main/cpp/Robot.cpp file has the definitions of the Init() and Periodic() functions for the Robot class. Remove all the Init() and Periodic() functions except RobotInit(), RobotPeriodic(), and TeleopPeriodic(). Now add the line shown below to TeleopPeriodic() which sets the motor percent output (-1 to 1 value) to the value of the X-axis on the controller. You won’t use RobotInit() and RobotPeriodic() just yet.

Robot.cpp after modifications

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "Robot.h"

void Robot::RobotInit() {}
void Robot::RobotPeriodic() {}

void Robot::TeleopPeriodic() {
    // Set the motor percent output to the controller left x-axis value
  m_motor.Set( m_controller.GetLeftX() );
}

#ifndef RUNNING_FRC_TESTS
int main() {
  return frc::StartRobot<Robot>();
}
#endif

Build and Deploy to RoboRIO

The roboRIO is an embedded computer running a real-time linux operation system. In order to run the robot program, it much be built and deployed to the roboRIO using VSCode. There must be a link between the coding computer and the roboRIO. This link can be made in three ways, first the coding computer can be connected via USB to the roboRIO. Second they can be connected wirelessly if the roboRIO is connected to an FRC Wifi Radio OpenMesh OM5P-AC by connecting the coding computer to the hotspot created by the robot. Third they can be connected via ethernet by running an ethernet cable between the computer and the OpenMesh OM5P-AC or a network switch connected to the OM5P-AC.

Since the Test Bench does not have a radio we must use the USB connection. After connecting to the roboRIO the robot program can be built and deployed.

ADD BUILD/DEPLOY GIF

FRC Driver Station

The Driver Station program manages the connection between the computer and the roboRIO. It gives that status of the connection and whether joysticks are recognized. It is how the robot is Enabled and Disabled among other things.

See the Driver Station Overview to learn about its features.

TASK 1

Build the project with the above changes added and make sure there are no errors. Then connect to the test bench with a USB cable and power the test bench. Connect an Xbox controller or a Logitech controller to the your laptop and run the Driver Station. Deploy the code, select TeleOperated on the driver station and press Enable. The left joystick’s x-axis motion should now control the speed of the motor.

QUESTIONS:

What other Xbox Controller controls could be used to move the motor?
What is the difference between a controller "button" and an "axis"?
How many axes are on an Xbox Controller?
How would you change the code to use the PS4 Controller?

Adding an Encoder

We are going to add the built-in encoder to our program so we can see how much the motor has turned and how fast it is turning. In Robot.h, use the frc::Encoder class to declare a variable for the encoder on channels 1 and 2.

Changes to Robot.h

  // Add the following header:
#include <frc/Encoder.h>

...

    // Add a private member variable such as:
  frc::Encoder m_enc{ 1, 2 };

Visualizing What is Happening

The Shuffleboard program is used to communicate with the roboRIO. The roboRIO can send information to Shuffleboard and Shuffleboard can be used to send information to the roboRIO. The frc::SmartDashboard class is one method to communicate with Shuffleboard. We will use the static member functions of the frc::SmartDashboard class to add information about the motor and the joystick position to Shuffleboard.

C++ classes with static member functions are used like regular functions. You do not create instances of the class. See Static Member Functions

Modify Robot.cpp to the following:

#include "Robot.h"
#include <frc/smartdashboard/SmartDashboard.h>

void Robot::RobotInit() {
  frc::SmartDashboard::PutData( "Velocity Motor", &m_motor );
  frc::SmartDashboard::PutNumber( "Encoder Distance", 0.0 );
  frc::SmartDashboard::PutNumber( "Joystick X-axis", 0.0 );
}

void Robot::RobotPeriodic() {
    // Get the current encoder distance and send it to the
    // SmartDashboard.
  double enc_dist = m_enc.GetDistance();
  frc::SmartDashboard::PutNumber( "Encoder Distance", enc_dist );
}

void Robot::TeleopPeriodic() {
    // Get the controller Left stick X-axis value
  double x_axis = m_controller.GetLeftX();

    // Send the value to the SmartDashboard
   frc::SmartDashboard::PutNumber( "Joystick X-axis", x_axis );

    // Set the motor percent output to the controller x-axis value
  m_motor.Set( x_axis );
}

#ifndef RUNNING_FRC_TESTS
int main() {
  return frc::StartRobot<Robot>();
}
#endif

TASK 2

Compile and deploy the code to the test bench. Run Shuffleboard and select the "SmartDashboard" tab. Move the motor disc by hand and observe the encoder value changing. Now select TeleOperated in the Driver Station and Enable the roboRIO. As you move the joystick, Shuffleboard will display the joystick output, the motor percent output (which should be the same), and the encoder distance.

QUESTIONS:

How much does the encoder distance value change for one rotation of the disc?

Encoder Values

The encoder values displayed on Shuffleboard are obtained with the frc::Encoder GetDistance() function. By default, encoders return distance in raw "counts" which can vary between a few counts per revolution up to 4096 or more depending on the resolution of the encoder. Approximate how many "counts" the encoder has per revolution by rotating the disc one full revolution (with the joystick) and determining the change in the distance measurement. For this motor the number of "counts" per revolution should come out to be 1024.

When programming the robot we want to work with more meaningful units than raw counts. If the mechanism is an arm that will move less than a full revolution then we probably want to use degrees. If the mechanism is a spinning flywheel then we probably want to use revolutions. The frc::Encoder class has a member function called SetDistancePerPulse() which allows you to change the units returned by the GetDistance() function.

Make the following modifications to the RobotInit() function to make the encoder return distance in rotations:

Changing Encoder Units

    // Add SetDistancePerPulse() function call to RobotInit()
    // Converts a 1024 count per revolution encoder to read rotations
  m_enc.SetDistancePerPulse( 1.0 / 1024 );

TASK 3

Deploy and run the robot code with the SetDistancePerPulse() function call added and note the units displayed in Shuffleboard which should now be rotations.

QUESTIONS:

How would you modify the code so the encoder returned distance in degrees?
What units would be most useful for a drive base motor?

Position Control Exercise

The Percent Output exercise (Simple Motor Percent Output) above is the most simplistic way of controlling a motor. Percent output control is only used for the simplest mechanisms. Many robotic mechanisms require either precise position control or velocity control which cannot be achieved with the percent output method. This project will move a motor to a specified position and try to hold it there. It will use P-control (position-control) to maintain the desired position which is a feedback control algorithm.

If you haven’t already, read the Motion Control section and watch the "PID Video, Part 1" in the first part of that section. This example builds on the project that was created in Simple Motor Percent Output so you will need the code from that project. If you have used the SetDistancePerPulse() function as outlined in Encoder Values then comment out the function call so that the encoder reads "counts".

P-Controller

The video in the first part of the Motion Control section does a good job of describing what a P-controller does but I will reiterate it here. The idea is to measure the current position of the robot mechanism (y) and then take the difference between the desired position (r) and the current position (y), this is the current position error (e). We then set the motor percent output to the error (e) multiplied by a constant (K_p) to scale things correctly.

Motor Output = K_p * e = K_p * ( r - y )

Add code to hold the position 0 while the A Button is held down and then move to the position 500 when the B button is held down. When neither button is held then it should just stop the motor. The pseudocode for this is expressed as:

Pseudocode for P-Controller

    If Button A is held::
      error = 0 - {encoder position}
      set motor output to (K_p * error)
    Else If Button B is held::
      error = 500 - {encoder position}
      set motor output to (K_p * error)
    Else
      set motor output to zero
    End If

This logic will go in the TeleopPeriodic() method. A good starting value for K_p is to take the total distance that the motor has to move from one setpoint to the other (500 in this case) and take the inverse of that number ( 1.0 / 500 ). So as a first guess, the value of K_p should be 0.002, it will give a motor percent output of 1.0 (500 * 0.002) when the mechanism first starts to move from one setpoint to the other. This motor output will decrease linearly as the target setpoint gets nearer.

TASK 1

Implement the above pseudocode for the P-controller. Change the value of the K_p constant and observe the difference in behavior of the mechanism. Note whether the actual position gets exactly to the desired position (setpoint).

QUESTIONS:

What happens when K_p is too small? too large?
What is the difference between the GetAButton() and GetAButtonPressed() methods of frc::XboxController?
How would you implement the P-controller logic with only one call to the Set() method of the `frc::Spark` class?

Sending Data with Shuffleboard

Not only is possible to send data from the robot program to Shuffleboard but also to send data back to the robot. The changing of the K_p constant in the above situation is a perfect example of when using Shuffleboard to send data to the robot is helpful. If we can send the K_p value to the robot while it is running then we do not need to change the code / compile / deploy just to change one constant. Above we used the PutNumber() method of the frc::SmartDashboard class to send a number to Shuffleboard. There is a corresponding GetNumber() method that will read a number from Shuffleboard. If we create a Shuffleboard entry in RobotInit() for the K_p value then we can read that value in TeleopPeriodic(). This way the value can be changed in Shuffleboard and it will use the new value in TeleopPeriodic().

TASK 2

Change the code so that the value of the K_p constant is read from Shuffleboard and therefore can be set while the robot code is running.

QUESTIONS:

When we find a good value for K_p what should we do to remember it?
It is dangerous to allow critical constants to be changed while in TeleOperated mode during a competition. Test mode is used to change parameters like this. How would Test mode be used so the K_p parameter cannot be changed during TeleOperated mode?

Shuffleboard Graphs

Read the Shuffleboard documentation and in particular Working with Graphs.

TASK 3

Graph the desired position (setpoint) and the actual encoder position in a Shuffleboard graph.

QUESTIONS:

What is the shape of the curve as you move from the 0 position to the 500 position and back?
Does the actual position get exactly to the desired position?

TASK 4

Modify the units returned by the encoder as demonstrated in Encoder Values to use rotations. Have Button A still go to 0 but change Button B to go to 2 rotations.

QUESTIONS:

How does the change in units affect the value of the K_p constant?

AdvantageScope

AdvantageScope is a NT viewer and a Log File viewer that is much more powerful than Shuffleboard. It’s graphs are far superior and much easier to use. Do the graphing exercises above using AdvantageScope rather than Shuffleboard. Explore the AdvantageScope Docs to see all of its available features.

TASK 5

Repeat the above two Shuffleboard Tasks using AdvantageScope.

QUESTIONS:

What happens if you pause the AdvantageScope output?
How do you zoom in on the graph?

Relative Encoder Limitations

Relative encoders consider the "0" position to be wherever the motor was when the encoder was powered on. Therefore it is not possible to know where the "0" position is and it changes each time the robot is powered on.

TASK 6

Note where the encoder considers the zero positon. Disable the robot and manually move the position motor to a new position then restart the robot code by going to Diagnostics → Restart Robot Code in the Driver Station. Re-enable the robot and notice that the 0 position is not in the same location as the last time the code was run.

Absolute Encoder

So far we have been using the relative quadrature output of the SRX Mag Encoder. The encoder also has a PWM signal for absolute positioning (See SRX Mag Encoder Hardware Guide). The frc::DutyCycleEncoder class is used to interface with the PWM absolute position signal which is wired to the DIO channel 0.

TASK 7

Modify the code to read the SRX Mag Encoder's absolute PWM signal using the GetAbsolutePosition() method of the frc::DutyCycleEncoder class. Move the motor disc by hand and note the values returned by the encoder. Use the absolute PWM signal to always move to the same 0 position when the A Button is pressed on the controller.

QUESTIONS:

What is the maximum value that the encoder reads and what happens when you rotate more than one rotation?
What is the difference between the GetAbsolutePosition() and GetDistance() methods of the frc::DutyCycleEncoder class?

Physical Units

The absolute encoder PWM signal is reading in "counts" just like the relative encoder signal did in Encoder Values. In this case the absolute signal has a larger number of "counts" per rotation that the relative signal. As before, it is much more useful to use some physical units with the encoder.

TASK 8

Modify the code to move to locations based on angles in degrees when the A and B Buttons are pressed. You will need to determine how to configure the frc::DutyCycleEncoder class to return angles in degrees. It is different than frc::Encoder.

Absolute Encoder Offsets

Usually an offset is needed for absolute encoders to make the zero point of the encoder a physically meaningful position. Put a piece of tape or a small bolt+nut in one of the holes on the position motor disc.

TASK 9

Use an offset to make the zero degrees position correspond to having the marked hole at the 3-o’clock position. Don’t do any motor control and just view the encoder position on AdvantageScope.

QUESTIONS:

Restart the robot code with the disc at several different starting positions. Does the signal roll over?
What happens if the disc is rotated more that one rotation?

Velocity Control Exercise

Velocity control is typically used on flywheels for shooting mechanisms or sometimes for intake wheels for game pieces. Using velocity control may seem like essentially the same as setting a motor’s percent output. However, velocity control reads the speed that the motor is actually spinning and makes corrections if the speed is wrong whereas percent output does not check for the correct speed. This becomes important, for example, when the battery voltage drops while the robot is running during a competition which will cause a motor set to a percent output value to slow down.

There is a very good description of controling a flywheel mechanism in the link below along with some interactive tools to see how changing the control system parameters affects the flywheel behavior.

Tuning a Flywheel Controller

REVLib

We will use the velocity motor (NEO 550) that is connected to the SparkMAX motor controller for this exercise. The SparkMAX uses the CAN bus of the roboRIO and requires an external (vendor) library to function. Vendor libraries can be added to a project following these instructions. You need to add the "REV Robotics REVLib" library to this project in order to use the SparkMAX controller.

The rev::CANSparkMax class is used to communicate with the SparkMAX controller. The rev::CANSparkMax is not part of the WPILib library and was added when the REVLib vendor library was added to the project in the steps above. Therefore the documention for the rev::CANSparkMax class and other classes that are provided by the REVLib library are located on the RevRobotics website. The Resources section of the Coding Guide gives links to the RevLib documentation, RevLib C++ API, and RevLib Examples. The Resources section also has several other useful links.

Velocity P-Control

Read the Motion Control section (again).

The SparkMAX is assigned CAN id #1 on the CAN bus. In Robot.h, create a variable for the motor using the rev::CANSparkMax class. You will need to determine what header file is needed to use the class. The rev::CANSparkMax Set() method will be used to control the motor percent output. Use the rev::CANSparkMax GetEncoder() method to access the built-in encoder on the NEO 550. The GetEncoder() method returns a rev::SparkMaxRelativeEncoder class object that can be used to retrieve the motor velocity (what method?). Some conversion may be necessary if a gearbox is attached to the NEO 550.

The equation for P-control will be the same as used in the position control exercise P-Controller except velocities will be used. The error e will be calculated as the difference between the setpoint velocity and the current velocity.

Motor Output = K_p * e = K_p * ( V_setpoint - V_actual )

TASK 1

Write a program to spin the NEO 550 to 2000 RPM when the A Button is held down. Use P-control on the motor velocity. Graph the setpoint and actual velocity in Shuffleboard.

QUESTIONS:

What value of K_p should you start with as a best guess?
Does the motor reach the setpoint velocity?
What is happening in this case (as opposed to position control)?

Feed Forward

See Introduction to DC Motor Feedforward and Feedforward Control in WPILib

Read the Feed Forward section and, if confused, read the Motion Control section a third time and the above WPILib information. This topic is confusing at first and you may need to re-read these sections several times.

Feed forward predictions are typically expressed in units of voltage. P-control on the otherhand is usually giving you a corrective error in percentage units. When using feed forward it is most common to use the motor controller class' SetVoltage() method if one exists.

The simplified motor feedforward equation that ignores static friction and acceleration effects can be written as:

Motor Voltage = K_v * V_setpoint

The K_v value used in the above equation is the inverse of the motor constant that is given in a motors documentation. Typically the manufacturer will give a K_v value in RPM/Volts but in the above equation we need Volts/RPM so that when we multiply by the setpoint velocity (in RPM) we will get a voltage to apply to the motors.

TASK 2

Modify your program to spin the NEO 550 to 2000 RPM when the A Button is held down using velocity based feed forward only. Don’t use the WPILib classes, just do the math yourself using the equation above like you did with the P-Control exercise.

QUESTIONS:

How do you determine the value of K_v you should start with? (HINT: See Motors Section and pay attention to units!)
Can you get the motor to reach the setpoint velocity? What about a different setpoint velocity?

Feed Forward with P-control

Typically a motorized mechanism would be controlled with both some form of feed forward combined with PID feedback to compensate for any error. The PID values (-1 to 1) are usually scaled to voltage (multiply by 12) and then the two terms (FF and PID) are added and sent to the SetVoltage() method.

TASK 3

Modify your program to use both feed forward and P-control.

QUESTIONS:

What would work if a class didn’t have a SetVoltage() method?

Built-in WPILib Classes for Control

TASK 4

Use the frc2::PIDController class to implement the P-control and the frc::SimpleMotorFeedforward<Distance> class for feed forward.

Trapezoidal Motion Exercise

In Position Control Exercise if the current motor position was very far from the desired position, then the motor would be immediately set to full power (100% output) toward the new desired position. This results in very abrupt and jerky movements of the mechanism. A better way to move the motor is to smoothly accelerate the motor towards the goal position until we reach a "cruise" velocity and then as we get close to the goal position we smoothly decelerate to a stop. This type of motion is called a trapezoidal motion profile because the plot of the velocity during the motion is a trapezoid.

The term goal is emphasized because it differs from the setpoint that we have been using up until this point. The way profiled motion works is that at each timestep the trapezoid profile computes a new setpoint. The setpoint has both position information and velocity information. The profiled motion is finished when the setpoint reaches the goal. Because the setpoint has position and velocity information, it is fairly straight forward to use feed forward and PID control while performing a profiled motion.

Figure 3. Trapezoid Profile (from CTRE Docs)

The WPILib provides the frc::TrapezoidProfile<Distance> class to generate a trapezoidal motion profile. It is a template class templated on either an angular unit or a distance unit. The WPI Documenation describes using trapezoidal motion profiles. Start with small values for the constraints (e.g. 100_deg_per_s, 200_deg_per_s_sq).

TASK 1

Write a program to use a trapezoidal profile to move the position motor disc to a 0 degree position when Button A is pressed and 180 degrees when Button B is pressed. Use feedforward and feedback control and use the steps in the Mechanism Tuning Procedure to determine the control constants.

QUESTIONS:

Double the constraint velocity and acceleration for the trapezoid profile. Does the mechanism need to be retuned? How was the speed and accuracy of the motion affected?

Arm Control Exercise

A vertical arm mechanism is one of the most complex control problems that is encountered on FRC robots. The links below give the basic information needed to understand the feed forward and PID aspects of the problem. Moving a vertical arm with motion profiles (i.e. trapezoidal profiles) gives good results and allows easy feed forward compensation.

See Arm Feedforward and Tuning a Vertical Arm Position Controller

Carefully mount the steel weight blocks to the position motor disc. Use the absolute encoder for control and make sure it is zeroed when the weights are at the 3-o’clock or 9-o’clock position (i.e. when the weight effect is worst on the mechanism). Also ensure that positive motor input raises the weight.

Start with all the control constants set to zero. Use feedforward and feedback control and use the steps in the Mechanism Tuning Procedure to determine the control constants like was done above. This time gravity effects will be present so kG must be set correctly.

TASK 1

Write a program to use a trapezoidal profile to move the position motor disc with the added weight blocks to a 0 degree position when Button A is pressed, 90 degrees when Button B is pressed, and 180 degrees when Button Y is pressed. Use both feed forward and PID control.

QUESTIONS:

How does the gravity feedforward value vary with angle?

Command Based Exercise

So far all the exercises have used the "Timed Robot" design pattern. When the robot code needs to respond to multiple button presses and joystick positions the logic in the TeleopPeriodic() function can get very busy with multiple if … else statements. The "Command Based" design pattern removes the need to worry about the button press and joystick logic. It also forces the programmer to break down their code into multiple files containing logically related information which results in better program organization.

In the Command Based design pattern, controller buttons and axes are "bound" to commands. When the button (or axis) is pressed the bound command is executed. The underlying command scheduler takes care of determining if the button is being pressed and calling your command if it is. One of the more difficult to understand aspects of Command Based programming is the use of Lambda Expressions and the idea of treating "functions as data" (i.e. passing functions as paramters to other functions). In Command Based programming lambda expressions occur frequently for simple commands that are bound to a controller button.

The other important difference with Command Based programs are the use of subsystems. A subsystem is a part of the robot hardware that operates together to perform a task, like an intake, climber, or drivetrain. Each subsystem has a Periodic() function that is called every 20ms where any processing for the subsystem is done. This is analogous to the RobotPeriodic() function in a Timed Robot program.

See Command-Based Programming and Functions as Data particularly Lambda Expressions in C++

Create a blank project using the "Command Robot" template. Browse the files that are automatically generated. The directory structure is a bit more complicated than the "Timed Robot" projects that you have been working with so far. There are still the Robot.h and Robot.cpp files but there are several more files created. The Command Based design pattern compartmentalizes the project into several files that each contain code for distinct parts of the project. The RobotContainer.cpp file has all the subsystems in it. The subsystems folder contains all the subsystems for the robot and the commands folder contains all the commands needed for the robot.

Command Triggers

Commands are started when an frc::Trigger object’s conditions are true. Triggers are usually the buttons on a joystick, but they may also be any arbitrary condition such as when a limit switch is activated. Look at Binding Commands to Triggers for the syntax used to create the command binding.

TASK 1

Write a command based program to control the velocity motor. When the A button is pressed, stop the motor. When the B button is pressed, set the motor speed to 1000 rpm. When the Y button is pressed, set the motor speed to 2000 rpm. Use frc2::InstantCommand with lambda expressions. You will need to create a subsystem for the velocity motor and provide some methods to set the speed of the motor. Use the feedforeward and feedback (PID) code from the Velocity Control Exercise in the Periodic() function of the subsystem.

QUESTIONS:

How many subsystems do you need?
What files in the project are not needed?

Default Commands

Commands that are bound to a joystick button only execute when the button is pressed (or held etc). Usually during TeleOp mode a robot program needs to respond to joystick input at all times (such as driving the robot around). This is accomplished with the default command of a subsystem. Each subsystem can have its own default command.

TASK 2

Setup a default command so that the right trigger varys the speed of the motor from 0 to 2000 rpm.

QUESTIONS:

Why can’t this be done with a button binding?

Trapezoid Profiled Position

TASK 3

Convert the Trapezoidal Motion Exercise to Command Based. Have the disc subsystem constantly track a goal position with a trapezoidal profile. Use commands bound to buttons to change the goal position of the subsystem.

QUESTIONS:

What happens when you disable the test bench and re-enable?

Team 4698 Test Bench Exercises

Test Bench

Simple Motor Percent Output

Creating a Blank Project

Adding Controller and Motor Variables

Setting up Controller—​Motor Connection

Build and Deploy to RoboRIO

FRC Driver Station

Adding an Encoder

Visualizing What is Happening

Encoder Values

Position Control Exercise

P-Controller

Sending Data with Shuffleboard

Shuffleboard Graphs

AdvantageScope

Relative Encoder Limitations

Absolute Encoder

Physical Units

Absolute Encoder Offsets

Velocity Control Exercise

REVLib

Velocity P-Control

Feed Forward

Feed Forward with P-control

Built-in WPILib Classes for Control

Trapezoidal Motion Exercise

Arm Control Exercise

Command Based Exercise

Command Triggers

Default Commands

Trapezoid Profiled Position

Setting up Controller—Motor Connection