Basic Drivetrain

The TimedRobot Skeleton

We will use this template frequently, so you can feel comfortable creating new projects like this from scratch. Open WPILib and follow these instructions.

  • Create a new project by pressing Ctrl+Shift+P and entering WPILib: Create a new project.

  • Select Template > Java > Timed Skeleton (Advanced) as the project type.

  • Complete the setup by specifying a project folder, entering a name, and adding our team number.

  • Click on Generate Project.

In the Explorer window, navigate to src/main/java/frc/robot. Open the file Robot.java. You will see the following:

 1public class Robot extends TimedRobot {
 2  /**
 3  * This function is run when the robot is first started up and should be used for any
 4  * initialization code.
 5  */
 6  public Robot() {}
 7
 8  @Override
 9  public void robotPeriodic() {}
10
11  @Override
12  public void autonomousInit() {}
13
14  @Override
15  public void autonomousPeriodic() {}
16
17  @Override
18  public void teleopInit() {}
19
20  @Override
21  public void teleopPeriodic() {}
22
23  @Override
24  public void disabledInit() {}
25
26  @Override
27  public void disabledPeriodic() {}
28
29  @Override
30  public void testInit() {}
31
32  @Override
33  public void testPeriodic() {}
34
35  @Override
36  public void simulationInit() {}
37
38  @Override
39  public void simulationPeriodic() {}
40}

Look at the pattern. There appear to be pairs of functions—-two for autonomous, two for teleop, two for test, etc. These correspond to the modes available by the FRC Driver Station.

_images/driver-station-modes.png

Each mode has two functions. The ones ending in Init are called only once, when the corresponding mode is enabled in the Driver Station. The ones with the suffix Periodic are called every 20 milliseconds.

The first two functions are Robot() and robotPeriodic(). The first function runs when the robot starts up and should be used for initialization code, while the second function is called every 20 milliseconds, regardless of the mode.

Our primary focus is coding for the autonomous and teleop modes, which are the fundamental modes in any FRC game.

Simple Drivetrain

Note

This and the rest of the coding exercises assume you are working with a AM14U5 - 6 Wheel Drop Center Robot Drive Base, with four brushed CIM motors and REV SparkMaxes as your motor controllers.

This is the full code for our first drivetrain program. Before using it, take a moment to review it, then follow the instructions below. Importantly, after you load the Timed Skeleton template, you should type the code rather than simply copying and pasting it.

 1// Copyright (c) FIRST and other WPILib contributors.
 2// Open Source Software; you can modify and/or share it under the terms of
 3// the WPILib BSD license file in the root directory of this project.
 4
 5package frc.robot;
 6
 7import com.revrobotics.spark.SparkLowLevel.MotorType;
 8import com.revrobotics.spark.SparkMax;
 9
10import edu.wpi.first.wpilibj.TimedRobot;
11import edu.wpi.first.wpilibj.XboxController;
12
13/**
14* The methods in this class are called automatically corresponding to each mode, as described in
15* the TimedRobot documentation. If you change the name of this class or the package after creating
16* this project, you must also update the Main.java file in the project.
17*/
18public class Robot extends TimedRobot {
19  /**
20  * This function is run when the robot is first started up and should be used for any
21  * initialization code.
22  */
23
24  SparkMax leftMotor1;
25  SparkMax leftMotor2;
26  SparkMax rightMotor1;
27  SparkMax rightMotor2;
28  XboxController joystick;
29
30  public Robot() {
31    leftMotor1 = new SparkMax(1, MotorType.kBrushed);
32    leftMotor2 = new SparkMax(2, MotorType.kBrushed);
33    rightMotor1 = new SparkMax(3, MotorType.kBrushed);
34    rightMotor2 = new SparkMax(4, MotorType.kBrushed);
35
36    // Initialize joystick
37    joystick = new XboxController(0);
38  }
39
40  @Override
41  public void robotPeriodic() {}
42
43  @Override
44  public void autonomousInit() {}
45
46  @Override
47  public void autonomousPeriodic() {}
48
49  @Override
50  public void teleopInit() {}
51
52  @Override
53  public void teleopPeriodic() {
54    double speed = -joystick.getLeftY() * 0.6;
55    double turn = joystick.getRightX() * 0.3;
56
57    double left = speed + turn;
58    double right = speed - turn;
59
60    leftMotor1.set(left);
61    leftMotor2.set(left);
62    rightMotor1.set(-right);
63    rightMotor2.set(-right);
64  }
65
66  @Override
67  public void disabledInit() {}
68
69  @Override
70  public void disabledPeriodic() {}
71
72  @Override
73  public void testInit() {}
74
75  @Override
76  public void testPeriodic() {}
77
78  @Override
79  public void simulationInit() {}
80
81  @Override
82  public void simulationPeriodic() {}
83}

  1. Don’t start from the beginning. Instead, begin with lines 24-28, where we define variables related to the motors and the joystick.

  2. You will encounter a warning or error by defining a SparkMax object. This happens because WPILib does not know yet what a SparkMax is. To resolve this issue, we need to import the class com.revrobotics.spark into our project. Instead of typing it directly, follow these steps:

  • Install the REV Library (called REVLib) by clicking on the WPILib icon on the left sidebar.

  • Place the cursor on one of the lines where we define the SparkMax object. Click on the lightbulb that appears, then select import com.revrobotics.spark.

  • For any other error that appears when we type a special function or property, do this step. If WPILib doesn’t import the library, only then you should type it out at the beginning of the progrma.

  1. Continue typing lines 31–37, where we initialize the objects we previously defined. Ensure that the numbers match the CAN ID assigned to each motor. We are using CIM motors, which are brushed motors. Additionally, we need to set up a joystick.

  2. Inside teleopPeriodic() we will code our Arcade Drive mode. Remember that this function runs once every 20 milliseconds. It will continuously read joystick inputs to determine speed and turn values, calculate the output for each side of the robot, and then use the function set() to assign a speed to each motor.

  3. Notice that the speed and turn variables have a multiplier that is less than zero. This effectively scales the joystick output by a percentage.

  4. When you are done, save the file and build the project by pressing Ctrl+Shift+P and entering WPILib: Build Robot Code. Once you are ready to deply the code to the roborio (i.e., your computer established a connection to the bot), press Ctrl+Shift+P and entering WPILib: Deploy Robot Code.

Danger

Whenever you deploy brand-new code to a robot, there are multiple risks: your robot may become uncontrollable, or the forces applied to the motors may be strong enough to cause self-destruction! In the case of a drivetrain, it’s always a good idea to start with the bot on stilts or in a cart, allowing the wheels to turn freely. Only when you are certain that the behavior is correct should you consider placing it on the ground. When you do, ensure it is in an appropriate space (a carpeted surface with plenty of room to avoid other objects or people, etc.).

Don’t damage the robot, the facilities, or other humans!

Now, let’s try the following exercises:

  1. Modify this code to set up the drivetrain in Tank Drive Mode. (Tank Drive mode allows independent control of each side of the drivetrain, with separate joystick axes controlling each side.)

  2. Can you assign predefined speeds using joystick buttons? Look at the code below and implement it using the buttons of your choice. You’ll see that all you need is a way to check the status of each button (pressed = TRUE, not pressed = FALSE) and use if/else commands Here is a list of XboxController functions that return booleans.

if (joystick.getAButton()) {
  /* Add code to move robot forward */
} else if (joystick.getBButton()) {
  /* Add code to rotate robot in some direction */
} else {
  /* If nothing is pressed, disable the motor instead */
  leftMotor1.set(0);
  leftMotor2.set(0);
  rightMotor1.set(0);
  rightMotor2.set(0);
}

A More Efficient Drivetrain

Our previous code worked, but it wasn’t ideal. For example, we didn’t follow the Don’t Repeat Yourself (DRY) principle. Maybe instead of setting individual speeds for each motor, we could set a single speed per side (left or right). We can also assign leader and follower motors. Additionally, we didn’t define what state the motors should be in after use (coast or brake).

Note

Coast mode allows the motor to “coast” or “idle” when power is no longer applied. This means running the motor then cutting power output to it will not stop “on a dime”, but will instead continue to spin by inertia.

Brake mode does the opposite. What it won’t do, however, is completely offset gravitational forces. For example, using it on an elevator will make it drop more slowly, but won’t prevent it from dropping at all (depending on gear ratios of course).

– By Fletch1373 <https://www.chiefdelphi.com/t/what-is-brake-coast-mode/163649/3?u=falvear>.

The following code, taken from the REVLib library, provides a more efficient way to implement an Arcade Drive mode.

Create a new project using the Timed Skeleton template, and type the relevant portions of this code.

  1// Copyright (c) FIRST and other WPILib contributors.
  2// Open Source Software; you can modify and/or share it under the terms of
  3// the WPILib BSD license file in the root directory of this project.
  4
  5package frc.robot;
  6
  7import com.revrobotics.spark.SparkLowLevel.MotorType;
  8import com.revrobotics.spark.config.SparkMaxConfig;
  9import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 10import com.revrobotics.spark.SparkMax;
 11import com.revrobotics.spark.SparkBase.PersistMode;
 12import com.revrobotics.spark.SparkBase.ResetMode;
 13
 14import edu.wpi.first.wpilibj.TimedRobot;
 15import edu.wpi.first.wpilibj.XboxController;
 16import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 17
 18public class Robot extends TimedRobot {
 19  SparkMax leftLeader;
 20  SparkMax leftFollower;
 21  SparkMax rightLeader;
 22  SparkMax rightFollower;
 23  XboxController joystick;
 24
 25  public Robot() {
 26    // Initialize the SPARKs
 27    leftLeader = new SparkMax(1, MotorType.kBrushless); /* BEWARE: Are our motors brushless or brushed? Modify accordingly*/
 28    leftFollower = new SparkMax(2, MotorType.kBrushless);
 29    rightLeader = new SparkMax(3, MotorType.kBrushless);
 30    rightFollower = new SparkMax(4, MotorType.kBrushless);
 31
 32    /*
 33    * Create new SPARK MAX configuration objects. These will store the
 34    * configuration parameters for the SPARK MAXes that we will set below.
 35    */
 36    SparkMaxConfig globalConfig = new SparkMaxConfig();
 37    SparkMaxConfig rightLeaderConfig = new SparkMaxConfig();
 38    SparkMaxConfig leftFollowerConfig = new SparkMaxConfig();
 39    SparkMaxConfig rightFollowerConfig = new SparkMaxConfig();
 40
 41    /*
 42    * Set parameters that will apply to all SPARKs. We will also use this as
 43    * the left leader config.
 44    */
 45    globalConfig
 46        .smartCurrentLimit(50)
 47        .idleMode(IdleMode.kBrake);
 48
 49    // Apply the global config and invert since it is on the opposite side
 50    rightLeaderConfig
 51        .apply(globalConfig)
 52        .inverted(true);
 53
 54    // Apply the global config and set the leader SPARK for follower mode
 55    leftFollowerConfig
 56        .apply(globalConfig)
 57        .follow(leftLeader);
 58
 59    // Apply the global config and set the leader SPARK for follower mode
 60    rightFollowerConfig
 61        .apply(globalConfig)
 62        .follow(rightLeader);
 63
 64    /*
 65    * Apply the configuration to the SPARKs.
 66    *
 67    * kResetSafeParameters is used to get the SPARK MAX to a known state. This
 68    * is useful in case the SPARK MAX is replaced.
 69    *
 70    * kPersistParameters is used to ensure the configuration is not lost when
 71    * the SPARK MAX loses power. This is useful for power cycles that may occur
 72    * mid-operation.
 73    */
 74    leftLeader.configure(globalConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
 75    leftFollower.configure(leftFollowerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
 76    rightLeader.configure(rightLeaderConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
 77    rightFollower.configure(rightFollowerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
 78
 79    // Initialize joystick
 80    joystick = new XboxController(0);
 81  }
 82
 83  @Override
 84  public void robotPeriodic() {
 85    // Display the applied output of the left and right side onto the dashboard
 86    SmartDashboard.putNumber("Left Out", leftLeader.getAppliedOutput());
 87    SmartDashboard.putNumber("Right Out", rightLeader.getAppliedOutput());
 88  }
 89
 90  @Override
 91  public void autonomousInit() {
 92  }
 93
 94  @Override
 95  public void autonomousPeriodic() {
 96  }
 97
 98  @Override
 99  public void teleopInit() {
100  }
101
102  @Override
103  public void teleopPeriodic() {
104    /**
105    * Get forward and rotation values from the joystick. Invert the joystick's
106    * Y value because its forward direction is negative.
107    */
108    double forward = -joystick.getLeftY();
109    double rotation = joystick.getRightX();
110
111    /*
112    * Apply values to left and right side. We will only need to set the leaders
113    * since the other motors are in follower mode.
114    */
115    leftLeader.set(forward + rotation);
116    rightLeader.set(forward - rotation);
117  }
118
119  @Override
120  public void disabledInit() {
121  }
122
123  @Override
124  public void disabledPeriodic() {
125  }
126
127  @Override
128  public void testInit() {
129  }
130
131  @Override
132  public void testPeriodic() {
133  }
134
135  @Override
136  public void simulationInit() {
137  }
138
139  @Override
140  public void simulationPeriodic() {
141  }
142}

Also, take note of the SmartDashboard tool. This tool allows us to visualize the status of any variable in real time. Open SmartDashboard and see what values are displayed. Can you use it to display the status of other joystick buttons?

See more