Controlling Accessories with Servo Motors

Hardware Setup

For this guide, we will be programming a REV Smart Robot Servo to control a basic mechanism, like a claw to grab one of the foundations in the 2019/2020 FTC game Skystone. The servo is included in the Kit of Parts, and it looks something like this:

I attached a servo horn (the little “flipper”) onto my Servo to demonstrate rotations. However, you can attach your servo to any mechanism that you have created and want to move, like a claw.

The servo can be wired into ports 0 through 5 on the “SERVO” section of the REV Expansion Hub as shown:

In this picture, I wired my servo to port 0. Make sure that you plug in the wire in a way so that the wire colors are in the correct order. Above the word “SERVO” on the REV Expansion Hub, there is a small diagram that shows the order the colors must be: the black (the ⏚ symbol) must be on the left and the white (the “S”) must be on the right. Notice how in the picture the wires are plugged in on the same order as the diagram (the middle wire is black instead of red, but this is okay as long as the left and right wires are correct).

You will need to name your servo in your phone’s hardware configuration so that we can control it in code. Edit your hardware configuration, select your REV Hub, and select “Servos”. There should be a list ports 0 through 5; select “Servo” and type in a meaningful name for the servo. In this tutorial, we will name it “claw”. If you are using Java, this is starting hardware configuration code you will need. Replace “MyOpNodeName” with your OpMode’s name:

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.Servo;
@TeleOp(name="MyOpNodeName")
public class MyOpNodeName extends LinearOpMode {
    Servo claw;

    @Override
    public void runOpMode() {
        // Put initialization blocks here.
        claw = hardwareMap.servo.get("claw");
        waitForStart();
        // Put run blocks here.
        while(opModeIsActive()) {
            //Loop code will go here
        }
    }
}

Moving the Servo

Concept

Using the REV SRS Programmer, a servo can be set into two modes: angular and continuous. Out of the box, a servo is in angular mode by default. For this tutorial, we will be keeping or server in angular mode.

A servo motor in angular mode works a bit differently than DC Motors (recall that we used DC Motors to turn the wheels on the robot). When we controller the DC Motors, we set the power (speed) that we wanted them to run at, and then the motors ran continuously at that speed. This was great for controlling the drivetrain wheels, as we wanted the robot to continuously move when controlled.

However, when controlling an angular servo, we simply set the position (angle) that we want the servo to go to, and the servo will move to that position and stay there. This is great for controlling mechanisms that need to move to certain positions, like a claw that we needed to set to “opened” or “closed”. Servo position values go from 0 to 1, with 0 being the servo’s left-most position and 1 being the servo’s right-most position.

This interactive widget demonstrates how setting servo positions works. Enter a value into the box and click the button to see what position the servo’s horn moves to. Try to find the left-most position and right-most position, and see what values in between (like 0.2) do.

Notice that the servo cannot turn a full 360 degrees in angular mode; its limit is about 230 degrees. This is important to keep in mind when attaching your servo to your mechanism.

Code

Let’s set the position of a servo in our code. In the loop section of your code, set your servo’s position to 1.

claw.setPosition(1);

If you are using Blocks, the servo blocks are under Actuators -> Servo. This will only show up in your toolbar if you named at least one servo in your Robot Controller’s hardware configuration.

If you run this code and your servo is configured and plugged in, you should see it move to its maximum position. Try other values (0, 0.2, 0.5, etc) to see what other positions your servo is able to move to. The behavior will be similar to the interactive widget shown earlier.

Controlling the Servo with Gamepad Buttons

We now have a way to set the servo’s position in our code. That’s great, but what if we want to control it using buttons on the gamepad? In our case, let’s say we want the servo to go to its right-most position (1) when we press the “A” button on the gamepad.

For this, we are going to have to use conditionals a type of logic that is used heavily in all types of programming. With conditionals, we can create “if” statements that “do” something if some condition is satisfied. The condition type is considered to be a boolean, meaning that it is either true or false satisfied or unsatisfied. For our program, we want the “if” to ask if “A” is being pressed, and if it is, we want our code to move the servo. Let’s change our code so that it uses this conditional:

if(gamepad1.a) { claw.setPosition(1); }

If you are using Blocks, the block structure for the conditional (the “if do”) is under the Logic section of the toolbar.

In this code, the condition (right next to the “if”) is “gamepad1.a” (is button A being pressed?). So, the code inside of the conditional (the servo set position) will only run if we hit A. If you save this code, run the OpMode, and press the “A” button on the gamepad, the servo should move.

Currently, when the A button is not pressed, the conditional will not run any code. However, what if we want to check if a different button is pressed when the A button isn’t pressed, and then run different code if that condition is met. We can do this by adding an “else if” portion to our conditional statement. Let’s extend our code so that the “B” button moves the servo to a different position. In this example, the “B” button moves the servo to position 0.2:

if(gamepad1.a) { claw.setPosition(1); } else if(gamepad1.b) { claw.setPosition(0.2); }

If you are using Blocks, to add an “else if” portion to the conditional (the “if do”) click the gear icon on the conditional. This will bring up a small window with a menu on the left and with some blocks on the right. The blocks on the right represent the structure of your conditional. Grab the “else if” block from the menu and place it under the “if” block in the conditional structure. Click the gear again to close this window.

The conditional we just created follows this logic: “if the A button is pressed, set the servo position to 1. Otherwise (else), if the B button is pressed, set the servo position to 2″. Save and run this OpMode, and now both the A and B buttons should be able to change the servo’s position!

The Full Code

This is the full example code that we wrote in this tutorial. We have an angular servo motor named “claw” that moves to position 1 (right-most) when “A” is pressed on the gamepad and moves to position 0.2 when “B” is pressed on the gamepad. This code can easily be extended to allow for more button functions (add more “else if” portions to the conditional). The positions that the servo moves to can also be adjusted depending on the mechanism that you want to control it with.

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Servo;
@TeleOp(name="MyOpModeName")
public class MyOpModeName extends LinearOpMode {
  Servo claw;
  @Override
  public void runOpMode() {
    // Put initialization blocks here.
    claw = hardwareMap.servo.get("claw");
    waitForStart();
    if (opModeIsActive()) {
      // Put run blocks here.
      while (opModeIsActive()) {
        // Put loop blocks here.
        if (gamepad1.a) {
          claw.setPosition(1);
        } else if (gamepad1.b) {
          claw.setPosition(0.2);
        }
        telemetry.update();
      }
    }
  }
}