Mecanum Drive and Debugging with Telemetry

Mecanum wheels have angled rollers, so each wheel pushes the robot both forward and sideways. By spinning the four wheels in different combinations, the robot can drive forward, strafe sideways, turn, or do all three at once. In a robot-centric drive, the sticks are relative to the front of the robot: pushing the stick forward always moves the robot toward its own front, wherever it is facing.

To combine motions, add the three parts for each wheel. If the driver pushes forward and right at the same time, some wheels get a sum larger than 1.0. Motors cannot go above full power, so large values would be cut off and the robot would drift in the wrong direction. The fix is normalisation: find the largest absolute power, and if it is above 1.0, divide all four powers by it. The wheels keep the same proportions, so the robot moves in the direction the driver intended.

1@TeleOp(name = "Mecanum Drive", group = "Drive")
2public class MecanumDrive extends LinearOpMode {
3    @Override
4    public void runOpMode() {
5        DcMotor leftFront  = hardwareMap.get(DcMotor.class, "left_front");
6        DcMotor rightFront = hardwareMap.get(DcMotor.class, "right_front");
7        DcMotor leftBack   = hardwareMap.get(DcMotor.class, "left_back");
8        DcMotor rightBack  = hardwareMap.get(DcMotor.class, "right_back");
9        leftFront.setDirection(DcMotor.Direction.REVERSE);   // test on your robot
10        leftBack.setDirection(DcMotor.Direction.REVERSE);
11
12        waitForStart();
13
14        while (opModeIsActive()) {
15            double axial   = -gamepad1.left_stick_y;  // forward is negative, so flip it
16            double lateral =  gamepad1.left_stick_x;  // strafe
17            double yaw     =  gamepad1.right_stick_x; // turn
18
19            double lf = axial + lateral + yaw;
20            double rf = axial - lateral - yaw;
21            double lb = axial - lateral + yaw;
22            double rb = axial + lateral - yaw;
23
24            double max = Math.max(Math.max(Math.abs(lf), Math.abs(rf)),
25                                  Math.max(Math.abs(lb), Math.abs(rb)));
26            if (max > 1.0) {
27                lf /= max;  rf /= max;  lb /= max;  rb /= max;
28            }
29
30            leftFront.setPower(lf);
31            rightFront.setPower(rf);
32            leftBack.setPower(lb);
33            rightBack.setPower(rb);
34
35            telemetry.addData("Front L/R", "%.2f / %.2f", lf, rf);
36            telemetry.addData("Back  L/R", "%.2f / %.2f", lb, rb);
37            telemetry.update();
38        }
39    }
40}

Telemetry: your robot's voice

Telemetry sends text from the robot to the Driver Station screen. telemetry.addData(caption, value) adds a line, and telemetry.update() sends all the lines added since the last update. Without update(), nothing appears. Use telemetry to check what the program believes: the joystick values it reads, the powers it calculates, the servo position it commands. When the robot behaves strangely, the numbers usually show you why.

1// import com.qualcomm.robotcore.util.ElapsedTime;
2ElapsedTime runtime = new ElapsedTime();
3
4waitForStart();
5runtime.reset();
6
7while (opModeIsActive()) {
8    // ... drive code ...
9    telemetry.addData("Run time", "%.1f s", runtime.seconds());
10    telemetry.addData("Left stick", "x=%.2f  y=%.2f",
11            gamepad1.left_stick_x, gamepad1.left_stick_y);
12    telemetry.addData("Claw target", claw.getPosition());
13    telemetry.update();   // nothing is shown without this line
14}

Common errors and how to fix them

1. 1
   Read the message

   Read the whole error on the Driver Station or in the build output. It often names the file, the line and the problem.
2. 2
   Reproduce it

   Make the problem happen again on purpose so you know exactly what triggers it.
3. 3
   Look at the numbers

   Add telemetry for the values involved and compare what the program believes with what the robot does.
4. 4
   Change one thing

   Make a single change, rebuild and test. If you change five things at once, you will not know which one helped.
5. 5
   Write it down

   Record the symptom, the cause and the fix in your debugging log for the engineering portfolio.

You now have everything for this week's assignment: a configured robot, motors and servos in code, a TeleOp structure, gamepad controls and a debugging process. Build your driver-controlled program step by step, test each addition, and keep your debugging log up to date.