This tutorial will help you create your first Op Mode for the First Tech Challenge.

Let’s get started! Launch Android Studio and open the FTC App Master project. On the left side browser, go into FtcRobotController > java > com.qualcomm.ftcrobotcontroller > opmodes. Select K9TeleOp, and then copy and paste the file into the opmodes folder. It will ask you to rename the file. Enter the name “MyTeleOp”.

Now, remove all the code from that file. First, we want to import a few libraries, as shown below.

package com.qualcomm.ftcrobotcontroller.opmodes;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;

Let’s start with the definition of the class, MyTeleOp, which is a subclass of OpMode. Here, we are also defining some variables. The CLAW_MIN_RANGE and CLAW_MAX_RANGE are the claw range values. There is a claw_1Position and a claw_2Position, which are used for setting the position of the servo. The clawDelta signifies the amount the claw moves if the button is pressed once. There are four DC motors being defined: motorLeft and motorRight (these are for driving), plus arm and testDC. There are two servo motors being used for the claw: claw_1 and claw_2. Finally, there is a constructor.

public class MyTeleOp extends OpMode {

	final static double CLAW_MIN_RANGE = 0.0;
	final static double CLAW_MAX_RANGE = 1.0;

	double claw_1Position;
	double claw_2Position;

	double clawDelta = 0.1;

	DcMotor motorRight;
	DcMotor motorLeft;
	DcMotor arm;
	DcMotor testDC;

	Servo claw_1;
	Servo claw_2;

	public MyTeleOp() {

	}

The FTC Driver Station has three main control states: Init, Loop, and Stop. Init occurs when you press Init on the Driver Station. Loop occurs when you click Play on the Driver Station. Stop occurs when you click Stop on the Driver Station. These three modes have their own functions.

Here is the Init function:

	@Override
	public void init() {
		motorRight = hardwareMap.dcMotor.get("motor_2");
		motorLeft = hardwareMap.dcMotor.get("motor_1");

		arm = hardwareMap.dcMotor.get("motor_3");
		testDC = hardwareMap.dcMotor.get("motor_4");

		motorLeft.setDirection(DcMotor.Direction.REVERSE);

		testDC.setPower(0.0);

		claw_1 = hardwareMap.servo.get("servo_1");
		claw_2 = hardwareMap.servo.get("servo_6");

		// Claw 1 is Left facing Front
		// Claw 2 is Right facing Front

		claw_1Position = -0.5;
		claw_2Position = 0.9;

	}

In the Init function, all we are really doing is assigning the motors from the hardware map to the motor variables, and setting the default speeds and positions. Now, this is the Loop function:

    @Override
	public void loop() {

		float throttle = 0;

		if (gamepad1.left_trigger > 0) {
			throttle = -gamepad1.left_trigger;
		}

		if (gamepad1.right_trigger > 0) {
			throttle = gamepad1.right_trigger;
		}

		float direction = gamepad1.left_stick_x;
		float right = throttle - direction;
		float left = throttle + direction;

		right = Range.clip(right, -1, 1);
		left = Range.clip(left, -1, 1);

		right = (float)scaleInput(right);
		left =  (float)scaleInput(left);

		motorRight.setPower(right);
		motorLeft.setPower(left);

		arm.setPower(0.0);

		if (gamepad1.a) {
			arm.setPower(0.10);
		}

		if (gamepad1.y) {
			arm.setPower(-0.20);
		}

		if (gamepad1.x) {
			claw_1Position += clawDelta;
			claw_2Position -= clawDelta;

		}

		if (gamepad1.b) {
			claw_1Position -= clawDelta;
			claw_2Position += clawDelta;
		}

		claw_1Position = Range.clip(claw_1Position, CLAW_MIN_RANGE, CLAW_MAX_RANGE);
		claw_2Position = Range.clip(claw_2Position, CLAW_MIN_RANGE, CLAW_MAX_RANGE);

		claw_1.setPosition(claw_1Position);
		claw_2.setPosition(claw_2Position);
	}

Here, the loop function iterates through for an infinite loop, until you press Stop. Each time, we are setting the throttle to zero at first, so that by default the robot will not move. Then, if gamepad1.left_trigger is pressed (the value is greater than 0), we want to set the throttle to the negative value of the gamepad left trigger, as this will make the robot go backward. if the gamepad1.right_trigger is pressed (the value would also be greater than 0), we want to set the throttle to the value from the right trigger, as this will make the robot go forward.

The direction is coming from the x-value of the left joystick. The speed assigned to the right motor is the throttle minus the direction, and the speed assigned to the left motor is the throttle plus the direction. If the direction was positive (right), the left motor would have more power than the right motor, making the robot go right. If the direction was negative (left), the right motor would have more power than the left motor, making the robot go left. Then, we clip the speeds to keep the power inside the maximum and minimum values for the motor.Also, an added benefit is that you can make sharper turns by using just the joystick.

The arm default power should be zero, so we are setting it to be zero by default. If any button gets pressed, we will change the power. So, if gamepad1.a is pressed, we will set the power of the arm to 0.10, making the arm go down. If gamepad1.y is pressed, we will set power to -0.20, making it go up. The reason why the power for going up is more is because the weight of the arm is acting against motion of lifting the arm. If gamepad1.x is pressed, we will move the position for the servos. Since the servos are facing the same direction, one will have to increase the position, and one will have to decrease the position. The opposite of this is happening if gamepad.b is pressed. Then, we clip the values of the claw position to the values of the claw range, and then set the position of the servo motors. This prevents us from setting a position that is outside the range of the servo motor.

Now, we have the Stop function, and a scale array function (this was provided by Qualcomm).

	@Override
	public void stop() {

	}


	double scaleInput(double dVal)  {
		double[] scaleArray = { 0.0, 0.05, 0.09, 0.10, 0.12, 0.15, 0.18, 0.24,
				0.30, 0.36, 0.43, 0.50, 0.60, 0.72, 0.85, 1.00, 1.00 };

		int index = (int) (dVal * 16.0);

		if (index < 0) {
			index = -index;
		}

		if (index > 16) {
			index = 16;
		}

		double dScale = 0.0;
		if (dVal < 0) {
			dScale = -scaleArray[index];
		} else {
			dScale = scaleArray[index];
		}

		return dScale;
	}

And that’s it! This op mode basically allows you to drive the robot, move the arm up and down, and open and close the claw! You can also download the MyTeleOp.java file [media-downloader media_id=”355″ texts=”here”].