Writing your first robot program

This lesson assumes that you have already installed the FRC development tools on your computer, or you are using a computer provided by the team.

Open Visual Studio Code, and follow WPIlib’s instructions for starting a new project. Choose ‘Template’ when selecting a project type. Make sure to set the team number to 5024, Java as your programming language, and Command Robot

We will be writing a simple tank drive with two wheels, one on each side of the robot. Here is a visual:

Preparing the project

WPIlib adds some extra template code to a new project to help teams out. We will not be needing that.

Start by deleting all the files inside of src/main/java/frc/robot/subsystems, then the same for src/main/java/frc/robot/commands.

Next, replace the contents of src/main/java/frc/robot/Robot.java with the following code:

package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.command.Scheduler;


public class Robot extends TimedRobot {
  
  public static OI m_oi;

  @Override
  public void robotInit() {
    m_oi = new OI();
    
  }

  @Override
  public void teleopInit() {
    
  }


  @Override
  public void teleopPeriodic() {
    Scheduler.getInstance().run();
  }


}

You now have a blank slate of a class. The Robot class is automatically called by the robot’s hyperviser code running on the RoboRIO. You do not need to worry about how this works, as it is automatic.

What do we want to do?

Deciding what to do with a robot is hard. Luckily, our testing robot has some wheels (as described above). So let’s write some code to make our bot move.

Controls

Firstly, we should start off with some controls. Our drive team uses Xbox controllers to drive our robots, so we will be writing some code for one of these controllers. To drive our bot, we will be using just a stingle joystick on our Xbox controller. This is not the configuration we use for competitions, but it is simpler to teach.

Let’s add some code to the Operator Interface (OI) file (src/main/java/frc/robot/OI.java). This file is where we define every human input.

package frc.robot;

import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID;

public class OI {
    // Note, I have removed all of the comments for this example. Feel free to leave them in your code. They are helpful.

    public XboxController m_driverController = new XboxController(0);

    public double getThrottle(){
        return m_driverController.getY(GenericHID.Hand.kLeft) * -1;
    }

    public double getTurn(){
      return m_driverController.getX(GenericHID.Hand.kLeft);
    }
}

Let’s start off with out imports.

import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID;

These imports simply tell the JVM that we will be using the XboxController and GenericHID helper classes from WPIlib.

We need to specify which controller we are reading from (we usually have more than one controller plugged in at a time).

public XboxController m_driverController = new XboxController(0);

In this case, we are reading from controller 0 (The driver’s controller).

Next, we define our getThrottle method.

public double getThrottle(){
    return m_driverController.getY(GenericHID.Hand.kLeft) * -1;
}

This method will read data from the driver’s Xbox controller and return it. Notice the * -1? That is required when reading from the Y axis of a joystick.

Now, we do the same for getTurn, except we are reading from the X axis this time, and do not need to invert the value.

public double getTurn(){
    return m_driverController.getX(GenericHID.Hand.kLeft);
}

That is it for reading user inputs. Now, on to controlling motors.

Controlling motors

In order to move the robot’s wheels, we need to control their motors. This can be done with a Subsystem.

Create a new file called DriveTrain.java in the src/main/java/frc/robot/subsystems folder, and add the following template code.

package frc.robot.subsystems;

import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;

public class DriveTrain extends Subsystem {

    WPI_TalonSRX left;
    WPI_TalonSRX right;
    DifferentialDrive drive;

    public DriveTrain() {

    }

    @Override
    protected void initDefaultCommand() {
        
    }

}

You will also need to add the CTRE vendordep. To do this, open the command palette using ctrl+shift+P and search “vendor”, select “Manage Vendor Libraries”, then “Install New Library (Online)”, and paste the previous link. See the vendor dependencies page for more information. Feel free to ignore this step until you want to test the code. At that point, ask a returning team member for help.

The code you have added is just a template, and follows the same style as the Operator Interface. Next, we need to add two motors and link them together. Add the following to the DriveTrain method:

left = new WPI_TalonSRX(1);
right = new WPI_TalonSRX(3);

drive = new DifferentialDrive(left, right);
drive.setSafetyEnabled(false);

The first two lines set up two motors, with IDs 1 and 3. These numbers are used by the RoboRIO to identify which motor to control. The last two lines link the two motors together into a wrapper class that we have assigned to the variable drive.

Now, we simply need a method to turn joystick data into motor commands:

public void arcadeDrive(double speed, double rotation) {
    drive.arcadeDrive(speed, rotation);
}

This passes speed and rotation to drive’s arcadeDrive method.

Feeding joystick data to the DriveTrain

We now need to read the joystick data, and push it to the DriveTrain. This can be done with a Command.

Create a new file called DriveControl.java in the src/main/java/frc/robot/commands folder, and add the following template code.

package frc.robot.commands;

import edu.wpi.first.wpilibj.command.Command;
import frc.robot.Robot;

public class DriveControl extends Command {

	@Override
	protected void execute() {
        
	}

	@Override
	protected boolean isFinished() {
		return false;
	}

}

This is a basic command class. As long as isFinished returns false, the execute method will be called once every 20ms by the robot. We can use this to our advantage by writing some code to read our joystick data here.

Inside the execute method, add these two lines. They will call the getThrottle and getTurn methods we made in the first section, and store the data.

double speed = Robot.m_oi.getThrottle();
double rotation = Robot.m_oi.getTurn();

Now, we just need to pass this data to the subsystem we just made. This can be done with the following line. It will call the arcadeDrive method we just made, and give it our speed and rotation valuse.

Robot.m_driveTrain.arcadeDrive(speed, rotation);

Adding the subsystem to the robot

That’s almost everything! We just need to write a few lines of code to tie everything together.

In the src/main/java/frc/robot/Robot.java file, we need to add the following to the list of imports in order to import our new Command and Subsystem:

import frc.robot.commands.DriveControl;
import frc.robot.subsystems.DriveTrain;

Next, we need to tell the robot that we made a subsystem and command. Add the following under the line that says public static OI m_oi;:

public static DriveTrain m_driveTrain;

DriveControl m_driveControl;

Next, we need to create the two objects

m_driveControl = new DriveControl();
m_driveTrain = new DriveTrain();

These go under the m_oi = new OI(); line.

Finally, add the following code to the teleopInit() method we created earlier in the Robot class:

if (m_driveControl != null){
    m_driveControl.start();
}

This code checks that m_driveControl has not started, and then starts it. Without this, the code we have written will not work.

Conclusion

That’s it! You now have your first piece of robot code (and it can actually drive!).

This guide was not designed to teach you how to program, but to show you around the general file structure of our robot code, and where each type of code goes. Feel free to come talk to a returning mentor, or team lead to see what you can add to your code next.