This example TeleOp demonstrates a simple way to smoothly control a Mecanum drivetrain.
This uses the techniques from Game Manual 0.
By default, the hardware device names are for the MecanumBot in virtual_robot. More on FTC simulators (same tab)
Preview:
//MI-FTC 2023 - https://mi-ftc.github.io
//If this is hard to read, try zooming in or opening this in Studio with a larger font size.
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
@TeleOp(name="Example Mecanum TeleOp", group="Tutorial")
public class ExampleMecanumTeleOp extends OpMode {
DcMotor leftFront;
DcMotor rightFront;
DcMotor leftBack;
DcMotor rightBack;
double drive;
double turn;
double strafe;
double greatest;
@Override
public void init() {
//Make sure these names match the Robot Controller configuration
//You can change either this or the RC config to match if necessary
leftFront = hardwareMap.get(DcMotor.class, "front_left_motor");
rightFront = hardwareMap.get(DcMotor.class, "front_right_motor");
leftBack = hardwareMap.get(DcMotor.class, "back_left_motor");
rightBack = hardwareMap.get(DcMotor.class, "back_right_motor");
//Your robot may need these inverted
leftFront.setDirection(DcMotor.Direction.REVERSE);
rightFront.setDirection(DcMotor.Direction.FORWARD); //State FORWARD too for clarity
leftBack.setDirection(DcMotor.Direction.REVERSE);
rightBack.setDirection(DcMotor.Direction.FORWARD);
}
@Override
public void loop() {
//Put any code here which should loop until Stop is pressed
// https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html
drive = gamepad1.right_trigger - gamepad1.left_trigger;
turn = gamepad1.left_stick_x * 0.5; //Adjust multiplier to change sensitivity
//The driver I worked with preferred slower turning
strafe = gamepad1.right_stick_x * 1.1;
greatest = Math.max(Math.abs(drive) + Math.abs(turn) + Math.abs(strafe), 1);
leftFront.setPower((drive + turn + strafe) / greatest);
//Think this way: Must add power for positive (right) turn, so add turn. Same for strafe.
rightFront.setPower((drive - turn - strafe) / greatest);
//Must remove power for positive (right) turn, so subtract turn.
leftBack.setPower((drive + turn - strafe) / greatest);
rightBack.setPower((drive - turn + strafe) / greatest);
//Telemetry, to isolate problems with code or controller
telemetry.addData("rightTrigger", gamepad1.right_trigger);
telemetry.addData("leftTrigger", gamepad1.left_trigger);
telemetry.addData("leftStickX", gamepad1.left_stick_x);
telemetry.addData("rightStickX", gamepad1.right_stick_x);
telemetry.addData("drive", drive);
telemetry.addData("turn", turn);
telemetry.addData("strafe", strafe);
telemetry.update();
}
}
Breakdown:
This will not repeat anything from the OpMode breakdown.
DcMotor leftFront;
DcMotor rightFront;
DcMotor leftBack;
DcMotor rightBack;
double drive;
double turn;
double strafe;
double GCD;
Here we are declaring all the variables we will use.
@Override
public void init() {
leftFront = hardwareMap.get(DcMotor.class, "front_left_motor");
rightFront = hardwareMap.get(DcMotor.class, "front_right_motor");
leftBack = hardwareMap.get(DcMotor.class, "back_left_motor");
rightBack = hardwareMap.get(DcMotor.class, "back_right_motor");
//Your robot may need these inverted
leftFront.setDirection(DcMotor.Direction.REVERSE);
rightFront.setDirection(DcMotor.Direction.FORWARD);
leftBack.setDirection(DcMotor.Direction.REVERSE);
rightBack.setDirection(DcMotor.Direction.FORWARD);
}
Now we initialize the motor variables to the actual HardwareDevices, which we can get from the HardwareMap. Again, their names right now are for virtual_robot's MecanumBot, but you can change these to match your configuration. We also set their directions, reversing the left-side motors in our case.
@Override
public void loop() {
// https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html
drive = gamepad1.right_trigger - gamepad1.left_trigger;
turn = gamepad1.left_stick_x * 0.5; //Adjust multiplier to change sensitivity
//The driver I worked with preferred slower turning
strafe = gamepad1.right_stick_x * 1.1;
GCD = Math.max(Math.abs(drive) + Math.abs(turn) + Math.abs(strafe), 1);
leftFront.setPower(drive + turn + strafe);
//Think this way: Must add power for positive (right) turn, so add turn. Same for strafe.
rightFront.setPower(drive - turn - strafe);
//Must remove power for positive (right) turn, so subtract turn.
leftBack.setPower(drive + turn - strafe);
rightBack.setPower(drive - turn + strafe);
We are now using the other four variables we declared at the start. drive combines the forward and reverse inputs, so that full right trigger is +1 and full left trigger is -1. Fully pressing both triggers will give 0. This is what I prefer; you can change this to your own liking. turn and strafe are multiplied by a decimal to modify their relative proportions. turn has half the influence of drive, and strafe has slightly more influence than drive to account for the robot moving slightly slower along that axis when given the same amount of power. Finally, we divide all the inputs by the greatest common denominator (or 1, whichever is larger) so we get the relative proportions of the inputs rather than the actual values. If we didn't do this, when you went to turn while giving full forward power, the motors would not have any power to spare to actually carry out the turn. By scaling them like this, drive will actually decrease to allow the desired portion of the total motor power available to be dedicated to turning. More information is available on Game Manual 0. Of course, you can change any of this to your liking.
//Telemetry, to isolate problems with code or controller
telemetry.addData("rightTrigger", gamepad1.right_trigger);
telemetry.addData("leftTrigger", gamepad1.left_trigger);
telemetry.addData("leftStickX", gamepad1.left_stick_x);
telemetry.addData("rightStickX", gamepad1.right_stick_x);
telemetry.addData("drive", drive);
telemetry.addData("turn", turn);
telemetry.addData("strafe", strafe);
telemetry.update();
Finally, we send some telemetry back to the Driver Station. Even though we call telemetry.update() on every iteration of the loop, it only actually gets sent back every 250ms. This can be changed with telemetry.setMsTransmissionInterval(). Anyway, we send back drive, turn, and strafe, as well as the controller values used to calculate them. Sending both allows us to quickly determine if problems are caused by the program or by the controller. (Controller issues happen quite often.)
Controls:
