- FRC Game Manual
- FRC Game Q&A

# Driving the Robot with Tank Drive and Joysticks

A common use case is to have a joystick that should drive some actuators that are part of a subsystem. The problem is that the joystick is created in the RobotContainer class and the motors to be controlled are in the subsystem. The idea is to create a command that, when scheduled, reads input from the joystick and calls a method that is created on the subsystem that drives the motors.

In this example a drive base subsystem is shown that is operated in tank drive using a pair of joysticks.

## Create a Drive Train Subsystem

Create a subsystem called Drive Train. Its responsibility will be to handle the driving for the robot base.

Inside the Drive Train, create a Differential Drive object. The Differential Drive object can contain a left motor controller and a right motor controller.

Create two Motor Controllers in the Differential Drive, and two more outside the Differential Drive, inside the Drive Train subsystem.

In the Differential Drive, set the left and right motors to the appropriate motor controllers.

Finally, for the two motor controllers that aren’t in the differential drive, set them to follow the motor controllers in the differential drive.

## Add the Joysticks to the Operator Interface

Add two joysticks to the Operator Interface, one is the left stick and the other is the right stick. The y-axis on the two joysticks are used to drive the robots left and right sides.

Note

Be sure to export your program to C++ or Java before continuing to the next step.

## Create a Method to Write the Motors on the Subsystem

java

     11// ROBOTBUILDER TYPE: Subsystem.
     12
     13package frc.robot.subsystems;
     14
     15
     16import frc.robot.commands.*;
     17import edu.wpi.first.wpilibj.livewindow.LiveWindow;
     18import edu.wpi.first.wpilibj2.command.SubsystemBase;
     19
     20// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS
     21import edu.wpi.first.wpilibj.AnalogGyro;
     22import edu.wpi.first.wpilibj.AnalogInput;
     23import edu.wpi.first.wpilibj.CounterBase.EncodingType;
     24import edu.wpi.first.wpilibj.Encoder;
     25import edu.wpi.first.wpilibj.drive.DifferentialDrive;
     26import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
     27
     28    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS
     29
     30
     31/**
     32 *
     33 */
     34public class DriveTrain extends SubsystemBase {
     35    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS
     36public static final double PlaceDistance = 0.1;
     37public static final double BackAwayDistance = 0.6;
     38
     39    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS
     40
     41    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
     42private PWMVictorSPX left1;
     43private PWMVictorSPX right1;
     44private DifferentialDrive drive;
     45private PWMVictorSPX left2;
     46private PWMVictorSPX right2;
     47private Encoder leftencoder;
     48private Encoder rightencoder;
     49private AnalogGyro gyro;
     50private AnalogInput rangefinder;
     51
     52    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
     53
     54    /**
     55    *
     56    */
     57    public DriveTrain() {
     58        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
     59left1 = new PWMVictorSPX(0);
     60 addChild("left1",left1);
     61 left1.setInverted(false);
     62
     63right1 = new PWMVictorSPX(1);
     64 addChild("right1",right1);
     65 right1.setInverted(true);
     66
     67drive = new DifferentialDrive(left1, right1);
     68 addChild("Drive",drive);
     69 drive.setSafetyEnabled(true);
     70drive.setExpiration(0.1);
     71drive.setMaxOutput(1.0);
     72
     73
     74left2 = new PWMVictorSPX(2);
     75 addChild("left2",left2);
     76 left1.addFollower(left2);
     77left2.setInverted(false);
     78
     79right2 = new PWMVictorSPX(3);
     80 addChild("right2",right2);
     81 right1.addFollower(right2);
     82right2.setInverted(false);
     83
     84leftencoder = new Encoder(0, 1, false, EncodingType.k4X);
     85 addChild("left encoder",leftencoder);
     86 leftencoder.setDistancePerPulse(1.0);
     87
     88rightencoder = new Encoder(2, 3, false, EncodingType.k4X);
     89 addChild("right encoder",rightencoder);
     90 rightencoder.setDistancePerPulse(1.0);
     91
     92gyro = new AnalogGyro(0);
     93 addChild("gyro",gyro);
     94 gyro.setSensitivity(0.007);
     95
     96rangefinder = new AnalogInput(1);
     97 addChild("range finder", rangefinder);
     98
     99
    100
    101    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    102    }
    103
    104    @Override
    105    public void periodic() {
    106        // This method will be called once per scheduler run
    107
    108    }
    109
    110    @Override
    111    public void simulationPeriodic() {
    112        // This method will be called once per scheduler run when in simulation
    113
    114    }
    115
    116    // Put methods for controlling this subsystem
    117    // here. Call these from Commands.
    118
    119    public void drive(double left, double right) {
    120        drive.tankDrive(left, right);
    121    }
    122
    123    public void stop() {
    124        drive.tankDrive(0.0, 0.0);
    125    }
    126
    127}

C++ (Header)

    11// ROBOTBUILDER TYPE: Subsystem.
    12#pragma once
    13
    14// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES
    15#include <frc2/command/SubsystemBase.h>
    16#include <frc/AnalogGyro.h>
    17#include <frc/AnalogInput.h>
    18#include <frc/Encoder.h>
    19#include <frc/drive/DifferentialDrive.h>
    20#include <frc/motorcontrol/PWMVictorSPX.h>
    21
    22    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES
    23
    24/**
    25 *
    26 *
    27 * @author ExampleAuthor
    28 */
    29class DriveTrain: public frc2::SubsystemBase {
    30private:
    31    // It's desirable that everything possible is private except
    32    // for methods that implement subsystem capabilities
    33    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    34frc::AnalogInput m_rangefinder{1};
    35frc::AnalogGyro m_gyro{0};
    36frc::Encoder m_rightencoder{2, 3, false, frc::Encoder::k4X};
    37frc::Encoder m_leftencoder{0, 1, false, frc::Encoder::k4X};
    38frc::PWMVictorSPX m_right2{3};
    39frc::PWMVictorSPX m_left2{2};
    40frc::DifferentialDrive m_drive{m_left1, m_right1};
    41frc::PWMVictorSPX m_right1{1};
    42frc::PWMVictorSPX m_left1{0};
    43
    44    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    45public:
    46DriveTrain();
    47
    48    void Periodic() override;
    49    void SimulationPeriodic() override;
    50    void Drive(double left, double right);
    51    void Stop();
    52    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS
    53
    54    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS
    55    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS
    56static constexpr const double PlaceDistance = 0.1;
    57static constexpr const double BackAwayDistance = 0.6;
    58
    59    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTANTS
    60
    61
    62};

C++ (Source)

    11// ROBOTBUILDER TYPE: Subsystem.
    12
    13// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES
    14#include "subsystems/DriveTrain.h"
    15#include <frc/smartdashboard/SmartDashboard.h>
    16
    17    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES
    18
    19DriveTrain::DriveTrain(){
    20    SetName("Drive Train");
    21    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    22    SetSubsystem("Drive Train");
    23
    24 AddChild("range finder", &m_rangefinder);
    25
    26
    27 AddChild("gyro", &m_gyro);
    28 m_gyro.SetSensitivity(0.007);
    29
    30 AddChild("right encoder", &m_rightencoder);
    31 m_rightencoder.SetDistancePerPulse(1.0);
    32
    33 AddChild("left encoder", &m_leftencoder);
    34 m_leftencoder.SetDistancePerPulse(1.0);
    35
    36 AddChild("right2", &m_right2);
    37 m_right2.SetInverted(false);
    38m_right1.AddFollower(m_right2);
    39
    40 AddChild("left2", &m_left2);
    41 m_left2.SetInverted(false);
    42m_left1.AddFollower(m_left2);
    43
    44 AddChild("Drive", &m_drive);
    45 m_drive.SetSafetyEnabled(true);
    46m_drive.SetExpiration(0.1_s);
    47m_drive.SetMaxOutput(1.0);
    48
    49
    50 AddChild("right1", &m_right1);
    51 m_right1.SetInverted(true);
    52
    53
    54 AddChild("left1", &m_left1);
    55 m_left1.SetInverted(false);
    56
    57
    58    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    59}
    60
    61void DriveTrain::Periodic() {
    62    // Put code here to be run every loop
    63
    64}
    65
    66void DriveTrain::SimulationPeriodic() {
    67    // This method will be called once per scheduler run when in simulation
    68
    69}
    70
    71// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS
    72
    73    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CMDPIDGETTERS
    74
    75
    76// Put methods for controlling this subsystem
    77// here. Call these from Commands.
    78
    79void DriveTrain::Drive(double left, double right) {
    80    m_drive.TankDrive(left, right);
    81}
    82
    83void DriveTrain::Stop() {
    84    m_drive.TankDrive(0.0, 0.0);
    85}

Create a method that takes the joystick inputs, in this case the left and right driver joystick. The values are passed to the DifferentialDrive object that in turn does tank steering using the joystick values. Also create a method called stop() that stops the robot from driving, this might come in handy later.

## Read Joystick Values and Call the Subsystem Methods

Create a command, in this case called Tank Drive. Its purpose will be to read the joystick values and send them to the Drive Base subsystem. Notice that this command Requires the Drive Train subsystem. This will cause it to stop running whenever anything else tries to use the Drive Train.

Create two parameters (`DoubleSupplier` for Java or `std::function<double()>` for C++) for the left and right speeds.

Create a parameter preset to retrieve joystick values. Java: For the left parameter enter `() -> getJoystick1().getY()` and for right enter `() -> getJoystick2().getY()`. C++: For the left parameter enter `[this] {return getJoystick1()->GetY();}` and for the right enter `[this] {return getJoystick2()->GetY();}`

Note

Be sure to export your program to C++ or Java before continuing to the next step.

## Add the Code to do the Driving

java

    11// ROBOTBUILDER TYPE: Command.
    12
    13package frc.robot.commands;
    14import edu.wpi.first.wpilibj2.command.Command;
    15import java.util.function.DoubleSupplier;
    16
    17// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS
    18import frc.robot.subsystems.DriveTrain;
    19
    20    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=IMPORTS
    21
    22/**
    23 *
    24 */
    25public class TankDrive extends Command {
    26
    27    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
    28        private final DriveTrain m_driveTrain;
    29    private DoubleSupplier m_left;
    30    private DoubleSupplier m_right;
    31
    32    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS
    33
    34    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    35
    36
    37    public TankDrive(DoubleSupplier left, DoubleSupplier right, DriveTrain subsystem) {
    38
    39    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
    40        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
    41        m_left = left;
    42        m_right = right;
    43
    44    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING
    45        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
    46
    47        m_driveTrain = subsystem;
    48        addRequirements(m_driveTrain);
    49
    50    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
    51    }
    52
    53    // Called when the command is initially scheduled.
    54    @Override
    55    public void initialize() {
    56    }
    57
    58    // Called every time the scheduler runs while the command is scheduled.
    59    @Override
    60    public void execute() {
    61        m_driveTrain.drive(m_left.getAsDouble(), m_right.getAsDouble());
    62    }
    63
    64    // Called once the command ends or is interrupted.
    65    @Override
    66    public void end(boolean interrupted) {
    67        m_driveTrain.stop();
    68    }
    69
    70    // Returns true when the command should end.
    71    @Override
    72    public boolean isFinished() {
    73        return false;
    74    }
    75
    76    @Override
    77    public boolean runsWhenDisabled() {
    78        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DISABLED
    79        return false;
    80
    81    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DISABLED
    82    }
    83}

C++ (Header)

    11// ROBOTBUILDER TYPE: Command.
    12
    13#pragma once
    14
    15    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES
    16
    17#include <frc2/command/CommandHelper.h>
    18#include <frc2/command/Command.h>
    19
    20#include "subsystems/DriveTrain.h"
    21
    22    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INCLUDES
    23
    24/**
    25 *
    26 *
    27 * @author ExampleAuthor
    28 */
    29class TankDrive: public frc2::CommandHelper<frc2::Command, TankDrive> {
    30public:
    31    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
    32    explicit TankDrive(std::function<double()> left, std::function<double()> right, DriveTrain* m_drivetrain);
    33
    34    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
    35
    36void Initialize() override;
    37void Execute() override;
    38bool IsFinished() override;
    39void End(bool interrupted) override;
    40bool RunsWhenDisabled() const override;
    41
    42
    43private:
    44    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLES
    45
    46std::function<double()> m_left;
    47std::function<double()> m_right;
    48
    49DriveTrain* m_drivetrain;
    50
    51
    52    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLES
    53};

C++ (Source)

    11// ROBOTBUILDER TYPE: Command.
    12
    13// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
    14
    15#include "commands/TankDrive.h"
    16
    17TankDrive::TankDrive(std::function<double()> left, std::function<double()> right, DriveTrain* m_drivetrain) :
    18    m_left(left),
    19    m_right(right),
    20m_drivetrain(m_drivetrain)
    21{
    22
    23    // Use AddRequirements() here to declare subsystem dependencies
    24    // eg. AddRequirements(m_Subsystem);
    25    SetName("TankDrive");
    26    AddRequirements({m_drivetrain});
    27
    28    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
    29
    30}
    31
    32// Called just before this Command runs the first time
    33void TankDrive::Initialize() {
    34
    35}
    36
    37// Called repeatedly when this Command is scheduled to run
    38void TankDrive::Execute() {
    39    m_drivetrain->Drive(m_left(),m_right());
    40}
    41
    42// Make this return true when this Command no longer needs to run execute()
    43bool TankDrive::IsFinished() {
    44    return false;
    45}
    46
    47// Called once after isFinished returns true
    48void TankDrive::End(bool interrupted) {
    49    m_drivetrain->Drive(0,0);
    50}
    51
    52bool TankDrive::RunsWhenDisabled() const {
    53    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DISABLED
    54    return false;
    55
    56    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DISABLED
    57}

Add code to the execute method to do the actual driving. All that is needed is pass the for the left and right parameters to the Drive Train subsystem. The subsystem just uses them for the tank steering method on its DifferentialDrive object. And we get tank steering.

We also filled in the `end()` method so that when this command is interrupted or stopped, the motors will be stopped as a safety precaution.

## Make Default Command

The last step is to make the Tank Drive command be the “Default Command” for the Drive Train subsystem. This means that whenever no other command is using the Drive Train, the Joysticks will be in control. This is probably the desirable behavior. When the autonomous code is running, it will also require the drive train and interrupt the Tank Drive command. When the autonomous code is finished, the DriveWithJoysticks command will restart automatically (because it is the default command), and the operators will be back in control. If you write any code that does teleop automatic driving, those commands should also “require” the DriveTrain so that they too will interrupt the Tank Drive command and have full control.

The final step is to choose the joystick parameter preset previously set up.

Note

Be sure to export your program to C++ or Java before continuing.