Set Up Your Robot

To get started with these exercises, you’ll need a mecanum chassis & Driver Hub. Note which ones you have and use the same ones as you move through the exercises.

Put a battery in your robot and make sure the Driver Hub connects to the robot’s WiFi network.

On the Driver Hub, touch the three dots in the upper-right corner to access the menu, and then choose Configure Robot. You will need to either choose an existing configuration or create a new one that correctly identifies which device is plugged in to which spot on the Control Hub. You will need the names assigned to each device in the robot configuration to access those devices from your code. It is highly recommended to use short, descriptive names for everything - for example DT-RF for the right-front motor in your drivetrain. Once you have finished examining or creating your configuration, go back to the main screen.

In the Driver Hub menu, select Program & Manage. Use the information on the screen to connect a laptop to the robot’s WiFi network, open a web browser, and navigate to the URL shown on the Driver Hub to access the programming interface.

You can create as many OpModes, runnable programs for your robot, as you want. You select which OpMode you want to run on the Driver Hub before starting the robot. Click the “+” symbol toward the upper-left corner of the screen to create a new file. Choose “Blank OpMode” and “Iterative OpMode” for the File Type. Use your first name and team number with “.java” at the end for the file name - “Brad4366.java” for example. Note that your file name must start with a capital letter! Choose “TeleOp” for the OpMode type.

The top line starts with package. This groups all of our code together and lets us share things between OpModes. At the top of the file, you find several lines that begin with the word import. These are references to other code that allow us to operate the robot. We’ll add more references as we add more functionality to our robots.

Your file should have the following imports.

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

Further down in the code, look for a line that starts with @TeleOp. This controls how & where the OpMode shows up on the Driver Hub. To add a more descriptive name, so you can find your program, make it look more like this:

@TeleOp(name="Brad's Learning OpMode")

We will build off of this program as we work through the rest of the exercises.

Make a Motor Move

Before you begin, verify that the following libraries are part of your imports.

import com.qualcomm.robotcore.hardware.DcMotor;

The first thing we need is an instance variable or a member variable to hold on to a representation of one of our motors. Add this immediately following the opening line of your class:

private DcMotor myMotor = null;

We can now use this motor variable throughout the rest of the code in this OpMode.

The init method runs once when you click the “INIT” button on the Driver Hub. This is a great place to set up the hardware devices on our robot. Add the following line inside the init method to find a motor called “DT-RF” from our robot configuration - if yours is named something different, you should also change what you put in your code.

myMotor = hardwareMap.get(DcMotor.class, "DT-RF");

We don’t need to do anything with the next two methods in your code yet. The init_loop method runs repeatedly after the init method finishes and continues until you press the play button on the Driver Hub. The start method runs once when you press the play button.

We do need to do something with the loop method, which will run over and over again after the start method finishes, and will keep going until you press stop on the Driver Hub. Add this line inside your loop method:

myMotor.setPower(0.8);

The setPower method on a DcMotor takes one argument, a number between -1 and 1, that makes the motor spin with the specified power and direction. This makes the motor spin at 80% power until you press stop.

We’ve finished our first program! Click the red button with the wrench in the lower-right corner to compile all the programs on your robot and prepare them to run. Once that succeeds, you can choose your OpMode on your Driver Hub and watch a wheel spin!

Try changing the power & direction of your motor in the code, rebuild, and run your OpMode again.

Make a Motor Move to a Position

All of the motors we use have encoders, sensors that can precisely measure movement, built into them. We can use that to move a motor to a specific position.

First, we need to configure our motor to work this way. We should also reset the encoder count when we start our OpMode - otherwise, we’ll run into odd behavior when we start the robot multiple times.

Add the following to the init method, after the line you already added to set your motor variable.

myMotor.setTargetPosition(0);
myMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
myMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);

This tells the motor to go to 0 ticks on the encoder, resets the encoder count to 0, and then puts the motor in an automatic positional control mode.

On the drivetrains we’re working with, one complete rotation of a wheel is about 1000 encoder ticks. In your loop method, add the following to tell your motor to spin the wheel around approximately one complete rotation.

myMotor.setTargetPosition(1000);

Build and run your program. If you try to move the wheel by hand, you’ll feel the robot fight against you and keep the wheel where it’s supposed to be. Adjust your program to make the wheel rotate faster or slower and watch how it responds.

Once you’re done with this, remove the setTargetPosition line and replace RUN_TO_POSITION with RUN_USING_ENCODER, and remove the other setMode line.

Control a Motor with a Game Pad Joystick

Your Driver Hub can connect up to 2 game pads, which provide input to your robot and allow your drivers to have direct control over what your robot does on the field. Plug a controller into your Driver Hub and press the START and A buttons simultaneously to identify it as gamepad 1. When you need a second controller later, you’ll press START and B at the same time to mark it as gamepad 2.

The buttons on the game pad show up in your code as boolean values - true or false. The joysticks show up as numbers that range between -1 and 1. The y-axes go from 1 at the bottom to -1 at the top, and the x-axes go from -1 to the left & 1 to the right. The triggers on the back of the gamepad give you a number between 0 and 1.

Let’s directly turn how far up or down you move the left joystick into how fast our motor spins. Change your line in the loop method that controls your motor speed to this:

myMotor.setPower(gamepad1.left_stick_y);

Build & run your OpMode and see what happens! Try to get it to move when you move the right stick from left to right instead.

Make a Servo Move to a Position

We have access to 2 different kinds of servos: regular or positional servos, and continuous rotation servos. From a code perspective, continuous rotation servos work exactly like motors have so far; you just need to declare their variables & their hardware map using the CRServo class instead of the DcMotor class. This exercise will use a positional servo.

Grab a servo, plug it into your robot, and add it to your robot hardware configuration.

Just like our motor, we need to import the Servo library:

import com.qualcomm.robotcore.hardware.Servo;

We also need to create an instance variable for our servo at the beginning of our class:

private Servo myServo = null;

Next, we need to get a reference to our servo out of the hardware map in our init method:

myServo = hardwareMap.get(Servo.class, "servo");

Standard servos only have one useful method, setPosition, that takes a number between 0 and 1. Add that to our loop method:

myServo.setPosition(0.5);

Build and run your OpMode. Try moving the servo to different positions.

Control Multiple Motors and Servos with a Game Pad

You now know everything you need to make your robot start doing something approximating driving!

Set up the other three motors in your drivetrain the same way you set up the first one. You will probably want to use a more descriptive name for your variables now that we have more than one thing to work with - driveRF or something like that might be a better choice.

All of our motors spin clockwise when given positive power. This creates a slight issue with most of our drivetrain designs. The motors on one side of the robot are mounted “upside-down,” so giving them the same power as the motors on the other side will have your robot spinning in circles instead of driving straight.

You’ll want to add something like this to your init method to explicitly set the directions for all 4 of your motors:

driveLF.setDirection(DcMotor.Direction.REVERSE);
driveRF.setDirection(DcMotor.Direction.FORWARD);

Make all of your drivetrain motors move with the same joystick axis, enabling you to drive your robot backward & forward, and make your servo position set based on one of the gamepad triggers. While you’re at it, go ahead and reset the encoder counts and have all motors run using encoders for all of your drive motors, too.

Using If Statements

If we want to make things happen when we press the buttons on our game pad, we’ll need to use if statements. if statements let us run bits of code only when certain boolean - true or false - values, like what the game pad object gives us for the buttons, are true.

So, if we wanted to set the position of our servo to 0 when we press the X button, we’d do something like this:

if(gamepad1.x) {
	servo.setPosition(0);
}

We can chain several if statements together with else if statements so that only the first condition that is met will run the associated code.

if(gamepad1.x) {
	// do one thing 
} else if(gamepad1.y) {
	// do something else
}

You can also use an else statement to have your code do something if none of the conditions are met:

if(gamepad1.x) {
	// do one thing when the button is pressed
} else {
	// do something else when the button isn’t pressed
}

Remove your previous code to move your servo. Instead, use if, else if, and else statements to make the servo move to 0 when the left bumper is pressed, 1 when the right bumper is pressed, and 0.5 when neither is pressed.

Report Data with Telemetry

Especially while we’re actively developing new functionality & debugging our code, it’s often helpful to be able to get more information about what the robot is doing. We can use the built-in telemetry object to send bits of data to the Driver Hub.

One of the most valuable pieces of information we should send back to the Driver Hub, especially when we start developing autonomous OpModes, is the current encoder count of a motor. We can get that by calling the getCurrentPosition method on a motor.

Servos also have a getCurrentPosition method, but standard servos don’t actually know their current position, so this function returns where the servo is supposed to be, not where it actually is.

Sending telemetry data is a two-step process. First, we need to provide the telemetry object with a label and the associated data we want to display. At the end of your loop method, add the following line:

telemetry.addData("LF", driveLF.getCurrentPosition());

You can go ahead and add more lines for each of your other motors, too.

After you add all of your telemetry data, you need to send all the data you’ve built up back to the Driver Hub. After all of your telemetry.addData calls, add:

telemetry.update();

Compile and run your code. As you drive your robot around, you should see the values on your Driver Hub change.

Drive with Mecanum Wheels

You’ll need a Mecanum Drive Worksheet for this section!

Mecanum wheels give our drivetrain three degrees of freedom - driving forward and backward, strafing left and right, and rotating clockwise and counterclockwise.

To make our code more readable, we can create variables to capture each motion, which we can use to calculate how we want to move later. Before the lines that set the power of your drive motors, add the following lines:

double drive = -gamepad1.left_stick_y
double strafe = gamepad1.left_stick_x
double rotate = gamepad1.right_stick_x

On your worksheet, start by labeling the directions of the force applied to the robot when each wheel rotates forward and backward. Note that the direction of the force is perpendicular to the direction of the rollers on each wheel.

Consider what happens when we add multiple forces acting in different directions together, as indicated in the diagram on the worksheet. Use that information to fill in the table on the worksheet, indicating whether each motor needs to spin in a positive or negative direction to move the robot in each specified direction.

Once you have that figured out, you can update the code that sets the power of your drive motors by adding up the positive or negative values according to the table. For example, if your front left row is all positive, you’d have the following in your code:

driveLF.setPower(drive + strafe + rotate)

Or, if they were all negative, you’d have this:

driveLF.setPower(-drive - strafe - rotate)

Set up all your motors and make sure your robot drives as you expect.

Learn About State Machines

Explain how a state machine works here!

Plan An Autonomous OpMode with a State Machine

One of the more straightforward ways to structure an autonomous OpMode is in a state machine. You can visualize a state machine as a flow chart, with different states, or steps, that the robot performs, connected by transitions, or conditions when we move from one state to another.

We’ll start by planning a simple, two-state autonomous OpMode that drives forward 3000 encoder ticks.

Code Your Autonomous OpMode

Create a new, blank, Iterative, autonomous OpMode. Verify that the autonomous library is added, along with the motor and servo libraries.

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;

Give a name to your auto code.

@Autonomous(name="Brad’s Auto OpMode")

Copy and paste your motor and servo declarations at the beginning of your class, as well as the contents of your init method from the teleop OpMode you’ve been working on. You’ll also want to copy and paste the set power methods for your drive motors at the end of your loop method, and you’ll need to create drive, strafe, and rotate variables at the beginning of the loop.

For each of your states, you’ll have an if statement to check which state you’re in. Inside each of those statements, you’ll set your drive, strafe, and rotate values to control how the robot moves. For each transition out of that state, you’ll have another if statement within the if statement for the state to check the transition condition and change the state appropriately - so your loop method ends up looking something like this:

double drive, strafe, rotate;
if(state == 1) {
  drive = 1;
  strafe = 1;
  rotate = 1;
  if(readyToTransition()) {
    state = 2;
  }
}
// more states go here
// set motor powers here

Following our plan from the last section, write the code to make your robot drive forward 3000 encoder ticks and then stop.

Make a Bigger State Machine Autonomous OpMode

You’ll need a State Machine Planning worksheet for this section!

Now that you’ve got the basic idea down, design a state machine & update your code to make your robot drive in a square using a combination of turns and strafing.

Detect Contact with a Limit Switch

There are many different sensors we can use with our robot. Distance sensor, color sensor and touch sensor are the most common. Since we don’t always start motors in the same position, reaching an exact position becomes very difficult. A touch sensor will allow us to know when our mechanism is at a known location.

Grab a touch sensor, plug it into your robot in the Digital section. Digital channels have 2 ports per location. When you add the touch sensor to your robot hardware configuration, it will need to be the odd port number of the 2.

Just like our motor, we need to import the TouchSensor library:

import com.qualcomm.robotcore.hardware.TouchSensor;

We also need to create an instance variable for our sensor at the beginning of our class:

private TouchSensor myTouchSensor = null;

Next, we need to get a reference to our sensor out of the hardware map in our init method:

myTouchSensor = hardwareMap.get(TouchSensor.class, "touch");

Touch sensors output a true or false, just like a gamepad button. To command to read the state of the touch sensor is:

myTouchSensor.isPressed();

Add the output of the touch sensor to a telemetry line and add that to our loop method:

// apparently we need to write this part

Build and run your OpMode. Try moving the servo to different positions based on whether the touch sensor is pressed or not.

Measure Rotation with the Gyroscope

During auto, and also in teleop, it is sometimes helpful to know what direction the robot is facing. The control hub has a built-in gyroscope that can provide that information.

Go into the control hub robot configuration under I2C, port 0, select the appropriate IMU and name it imu. Add the following to your imports:

import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;

We also need to and create an instance variable for our gyroscope, as well as some global variables at the beginning of our class:

private IMU myImu;
private YawPitchRollAngles imuAngles;
private Double heading, headingRad;

Next we also need to get a reference to our gyroscope out of the hardware map, define how the control hub is oriented, initialize our gyroscope and set the heading to zero in our init method:

myImu = hardwareMap.get(IMU.class, "imu");
RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; 
//**FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT
RevHubOrientationOnRobot.UsbFacingDirection  usbDirection  = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; 
//**FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT
RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
myImu.initialize(new IMU.Parameters(orientationOnRobot));
myImu.resetYaw();  //zeros Yaw angle (heading)

You will need to pick the appropriate directions for the logo and usb based on how the control hub is on your robot.

Create a new method for our gyroscope, and also call our gyroscope method in our loop method. Inside our gyroscope method add:

imuAngles = myImu.getRobotYawPitchRollAngles();
heading = imuAngles.getYaw(AngleUnit.DEGREES);
headingRad = imuAngles.getYaw(AngleUnit.RADIANS);

Add the heading and headingRad to our telemetry output.

Compile and run your code. As you drive your robot around, you should see the values of the heading on your Driver Hub change. Which way is the angle positive? What happens when the robot turns more than 180 degrees?

Use Proportional Control to Turn Accurately

Until now we have been assuming that our robot responds more like a video game than an actual physical robot. We assume the robot does what we want it to immediately and exactly. What our actual robot does is a little different. The actual robot will spin its wheels when the motors go from 0 to 100%, and when we turn the motors off the robot continues to move while it is slowing down to stop.

To better understand what we want our robot to do, imagine how a person would do the same task. If you asked a person to run as quickly as they could to a wall and stop 100mm away from the wall without touching the wall. Would they run as fast as they could (100% speed) until they were 100mm from the wall and then stop (0% speed)? Would they run at 100% speed until they were say 500mm away from the wall and then stop, hoping they end up at 100mm from the wall?

If you said that a person slows down as they get closer to the wall, that is exactly what we want our robot to do. When we use RUN_TO_POSITION the internal controls do that for us. While RUN_TO_POSITION may work well for driving forward or to the side, it is an inaccurate way to rotate precisely. To rotate precisely we will want to use the gyroscope to control how far we rotate.

When we talk about controls, it is better to think in terms of target value, actual value and error from target value. Target value is where we want the robot to be, actual value is where the robot is currently, and error is the difference between the two values. (This is why when we use RUN_TO_POSITION we set a target value.) For example: If we are starting from 0 and we want to be at 100, our target value is 100, our current value is 0 and our error is 100. As the current value gets closer to the target value, the error decreases.

Using error to control our power gives us the right behavior, but it doesn’t necessarily give us the right value. For example, if the units of error are in mm, you would need to be less than 1mm away from your target before you begin decreasing power. To resolve this, we may need to multiply the error by an additional value Kp.

P = Kp * error

Using proportional control, create a method to turn 90 degrees using the gyroscope.

Implement Field-Centric Driving

You’ll need the Field-Centric Driving worksheet for this section!

The top row of the chart is the direction we want the robot to move on the field. (Up is away from us.) The column on the left is the direction the robot is facing, the arrow pointing to the front of the robot. Begin by filling in the chart with arrows the robot needs to move (robot centric). The first cell is filled in for us. When you are finished, show your worksheet to a mentor who will verify all the arrows are correct.

Next, write the angle the arrow represents next to the arrow. Up is 0deg, right is 90deg, left is -90deg, down is 180deg (and/or -180deg), up & right is 45deg, and up & left is -45deg.