TeleOp Usage

The follower can also control the drivetrain during TeleOp.

Unlike the autonomous guides, we'll start a new OpMode for TeleOp.

Creating the Follower

Create the follower inside init(), just like in autonomous:

private Follower follower;

@Override
public void init() {
    follower = Constants.create(hardwareMap);
}

Manual Driving

Instead of following a path, TeleOp uses follower.manual() to send driver controls to the drivetrain.

manual() takes forward, lateral, and turning power:

follower.manual(forward, lateral, turn);

Use the gamepad sticks for these values:

double forward = -gamepad1.left_stick_y;
double lateral = gamepad1.left_stick_x;
double turn = gamepad1.right_stick_x;

follower.manual(forward, lateral, turn);
follower.update();

These calls should run inside loop():

@Override
public void loop() {
    double forward = -gamepad1.left_stick_y;
    double lateral = gamepad1.left_stick_x;
    double turn = gamepad1.right_stick_x;

    follower.manual(forward, lateral, turn);
    follower.update();
}

Important

The follower still needs to be updated every loop while using manual drive.

Robot Centric TeleOp

A basic TeleOp looks like this:

ExampleTeleOp.java

package org.firstinspires.ftc.teamcode;

import com.pedropathing.follower.Follower;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.teamcode.pedro.Constants;

@TeleOp(name = "Example TeleOp")
public class ExampleTeleOp extends OpMode {

    private Follower follower;

    @Override
    public void init() {
        follower = Constants.create(hardwareMap);
    }

    @Override
    public void loop() {
        follower.manual(
                -gamepad1.left_stick_y,
                gamepad1.left_stick_x,
                gamepad1.right_stick_x
        );

        follower.update();
    }
}

These controls are robot-centric, meaning forward and lateral movement are relative to the direction the robot is facing.

Field Centric Driving

For field-centric controls, use ManualDrive.fieldCentric().

Add these imports:

import com.pedropathing.drivetrain.DrivePowers;
import com.pedropathing.follower.ManualDrive;

Create field-centric drive powers using the current heading:

DrivePowers powers = ManualDrive.fieldCentric(
        -gamepad1.left_stick_y,
        gamepad1.left_stick_x,
        gamepad1.right_stick_x,
        follower.pose().heading()
);

Then pass them to the follower:

follower.manual(powers);
follower.update();

Inside loop(), that looks like:

@Override
public void loop() {
    DrivePowers powers = ManualDrive.fieldCentric(
            -gamepad1.left_stick_y,
            gamepad1.left_stick_x,
            gamepad1.right_stick_x,
            follower.pose().heading()
    );

    follower.manual(powers);
    follower.update();
}

Tip

Field-centric driving uses the heading reported by the localizer, so make sure localization is working correctly before using it.

Using Pedro's localization

PedroPathing is always checking where your robot is on the field. You can harness this by using follower.pose() to get the position of your robot on the field.

@Override
public void loop() {
    DrivePowers powers = ManualDrive.fieldCentric(
            -gamepad1.left_stick_y,
            gamepad1.left_stick_x,
            gamepad1.right_stick_x,
            follower.pose().heading()
    );

    follower.manual(powers);
    follower.update();
    Pose robotPose = follower.pose(); // returns a Pose object

    telemetry.addData("Robot X", robotPose.x());
    telemetry.addData("Robot Y", robotPose.y());
    telemetry.addData("Robot Heading", Math.toDegrees(robotPose.heading())); 
    // Math.toDegrees() is a built-in java method
}

Units

follower.pose() always returns values in inches (x and y) and radians (heading). Make sure to convert your units if you need to using Java's built in Math class.

You can use Pose objects in your methods (and subsystems if you use them), and send your robot's Pose to them, so you code can use your location (eg. Turrets, making sure you are in a zone)

While localization in an auto typically has very little to no drift, autonomous lasts just 30 seconds. The 2 minutes in TeleOp can result in a bit of drift. You can update your position using follower.setPose.

Only use this if you either have a way to relocalize (eg. a LimeLight or another camera), or you know your position (eg. in a corner).

Let's add a way to relocalize in a corner to our current TeleOp:

@Override
public void loop() {
    DrivePowers powers = ManualDrive.fieldCentric(
            -gamepad1.left_stick_y,
            gamepad1.left_stick_x,
            gamepad1.right_stick_x,
            follower.pose().heading()
    );
    follower.manual(powers);

    // relocalise button
    if (gamepad1.startWasPressed) {
        Pose cornerPose = new Pose(10.5, 10.5, Math.toRadians(90));
        // On the fly Pose creation, we dont recommend this for Autonomous. Only accepts radians for heading
        follower.setPose(cornerPose); // overrides our pose  
    }

    follower.update();
    Pose robotPose = follower.pose(); // returns a Pose object
    telemetry.addData("Robot X", robotPose.x());
    telemetry.addData("Robot Y", robotPose.y());
    telemetry.addData("Robot Heading", Math.toDegrees(robotPose.heading())); 
    // Math.toDegrees() is a built-in java method
}

This code will set our robot's position to a corner when we press "Start".

Passing your position from Autonomous to TeleOp

Pedro doesn't automatically save your position at the end of Autonomous. You have to send it from your Autonomous to your TeleOp manually. Pedro doesn't have a built-in way for this, but there are several ways you can do this. For this example, we'll make a storage class.

Create a new class. We'll call it OpModeStorage:

OpModeStorage.java

package org.firstinspires.ftc.teamcode;
//Make sure this matches your file location

import com.pedropathing.math.Pose;

public class OpModeStorage {
    public static Pose autonomousEndPose = new Pose(0, 0, 0);
    // default pose to avoid null errors
}

This should be accessible by your autos and TeleOp, so ideally, not in any folder.

Now in your all your autonomous modes, create a stop() method. (LinearOpMode has it different), and add this line:

ExampleAuto.java

// in your autonomous


@Override
public void stop() {
    OpModeStorage.autonomousEndPose = follower.pose(); //saves your position in that file
}

As long as your robot doesn't power off, that pose will stay saved after the OpMode ends.

Now back in your TeleOp code, add this code in your start() method

TeleOp.java

@Override
public void start(){
    follower.setPose(OpModeStorage.autonomousEndPose);
    follower.update();
}

Your autonomous will now save the Pose it is at when the OpMode is turned off, and save it in the OpModeStorage file. Afterwards your TeleOp will set its starting position to what it finds in OpModeStorage

You can use this same method to save any other values (eg, states, patterns, etc), from your autonomous and put it into your TeleOp.