Skip to content

Commit

Permalink
Cleanup mecanum programming page (#387)
Browse files Browse the repository at this point in the history 
- Clean up 2 motor drive example to use "rx".

Since all of the remaining code examples use "rx" (right stick x)
as the variable name for in-place turns, do that here also.

- Clean up block code for initial strafing.

- Clean up counteract strafe example.

- Update blocks code for scaling values.

- Make final Java code consistent with earlier code.

This corrects the Java code to use the same motor
variables as earlier examples, as well as the Blocks
code.

- Update final blocks code.

- Update field-centric code to match other examples.

- Improve the text description in a few places.
  • Loading branch information
@pmichaud
pmichaud authored Aug 24, 2023
1 parent 580a065 commit 5ae9413
Show file tree
Hide file tree
Showing 8 changed files with 66 additions and 88 deletions.
66 changes: 21 additions & 45 deletions source/docs/software/tutorials/block-code/mecanum-drive-sample.blk
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,22 +5,16 @@
<variable id="xJZJH):(0WQtG_CIq`mH">rx</variable>
<variable id="]#MBGP`G6{E0o46rh-XV">denominator</variable>
</variables>
<block type="procedures_defnoreturn" id="WtpyD@tEu+EoY=P9iY,D" deletable="false" editable="false" x="50" y="50">
<block type="procedures_defnoreturn" id="WtpyD@tEu+EoY=P9iY,D" deletable="false" x="50" y="50">
<field name="NAME">runOpMode</field>
<comment pinned="false" h="32" w="532">This function is executed when this Op Mode is selected from the Driver Station.</comment>
<data>{"commentPositionLeft":173,"commentPositionTop":-8}</data>
<statement name="STACK">
<block type="linearOpMode_waitForStart" id=":`$wN_ci`O;o6P0~.(+g">
<next>
<block type="controls_if" id="}Q?5mbxSO8Di;i!__6[y">
<value name="IF0">
<block type="linearOpMode_opModeIsActive" id=")o%YFX/#9kvv^sgXr4~5"/>
</value>
<statement name="DO0">
<block type="comment" id="cBQNfb%_qY1[|[F8%CTW">
<field name="COMMENT">Reverse the right side motors</field>
<field name="COMMENT">Reverse the right side motors. This may be wrong for your setup.</field>
<next>
<block type="comment" id="U{iU)`F=sAwX$5$|R0%;">
<field name="COMMENT">Reverse left motors if you are using NeveRests</field>
<field name="COMMENT">If your robot moves backwards when commanded to go forwards, reverse the left side instead.</field>
<next>
<block type="dcMotor_setProperty_Direction" id="#!y|w+,AdUJ^[P{o/.`c">
<field name="IDENTIFIER">frontRightMotorAsDcMotor</field>
Expand All @@ -42,14 +36,16 @@
</shadow>
</value>
<next>
<block type="linearOpMode_waitForStart" id=":`$wN_ci`O;o6P0~.(+g">
<next>
<block type="controls_whileUntil" id="^0Kagy/e+Ea{rbZPv2G6">
<field name="MODE">WHILE</field>
<value name="BOOL">
<block type="linearOpMode_opModeIsActive" id="e)cr{!2VIET83s,JWC.="/>
</value>
<statement name="DO">
<block type="comment" id="{Ob+XSe*L5~c;N`)5OUd">
<field name="COMMENT">Remember, this is reversed!</field>
<field name="COMMENT">Remember, Y stick value is reversed</field>
<next>
<block type="variables_set" id="9e|V1x:nDOx#x#R)t%*E">
<field name="VAR" id="J,KtXc!}`~PyFLlNl~Fv">y</field>
Expand All @@ -69,21 +65,11 @@
</block>
</value>
<next>
<block type="variables_set" id="C/APDv:JO+C}%J;HCkzf">
<field name="VAR" id="{7*gXSsX229h0IdVUt^[">x</field>
<value name="VALUE">
<block type="gamepad_getProperty_Number" id="m,eYk3}buPUWhe?y|KKw">
<field name="IDENTIFIER">gamepad1</field>
<field name="PROP">RightStickX</field>
<data>{"IDENTIFIER":"gamepad1"}</data>
</block>
</value>
<next>
<block type="comment" id="F]{PtaU3e%K:-:*P@y0Y">
<field name="COMMENT">Counteract imperfect strafing</field>
<field name="COMMENT">Factor to counteract imperfect strafing</field>
<next>
<block type="variables_set" id="$W}{4%cRq!^5=kz+n:@%">
<field name="VAR" id="xJZJH):(0WQtG_CIq`mH">rx</field>
<field name="VAR" id="{7*gXSsX229h0IdVUt^[">x</field>
<value name="VALUE">
<block type="math_arithmetic" id="y_nDH*Os5TJ3wLI1jyoQ">
<field name="OP">MULTIPLY</field>
Expand All @@ -105,20 +91,21 @@
</block>
</value>
<next>
<block type="comment" id="GoDY:CEZ]Sr`1z$qE|;3">
<field name="COMMENT">Denominator is the largest motor power</field>
<block type="variables_set" id="C/APDv:JO+C}%J;HCkzf">
<field name="VAR" id="xJZJH):(0WQtG_CIq`mH">rx</field>
<value name="VALUE">
<block type="gamepad_getProperty_Number" id="m,eYk3}buPUWhe?y|KKw">
<field name="IDENTIFIER">gamepad1</field>
<field name="PROP">RightStickX</field>
<data>{"IDENTIFIER":"gamepad1"}</data>
</block>
</value>
<next>
<block type="comment" id="P7mk|[Mz5;9`8YT#WcG*">
<field name="COMMENT">(absolute value) or 1.</field>
<block type="comment" id="GoDY:CEZ]Sr`1z$qE|;3">
<field name="COMMENT">Denominator is the largest motor power (absolute value) or 1.</field>
<next>
<block type="comment" id="Sr8f!z6e/qTHgMJ0|X*-">
<field name="COMMENT">This ensures all the powers maintain </field>
<next>
<block type="comment" id="~@wrPBOfaOXPDiFK(3.`">
<field name="COMMENT">the same ratio, but only when at least one is</field>
<next>
<block type="comment" id="+O#mz`?rI^)TO#PTRLR:">
<field name="COMMENT">out of the range [-1, 1].</field>
<field name="COMMENT">This ensures all powers maintain the same ratio, but only if one is outside of the range [-1, 1].</field>
<next>
<block type="variables_set" id="K]3A9R1@h;h-}Pvb][[=">
<field name="VAR" id="]#MBGP`G6{E0o46rh-XV">denominator</field>
Expand Down Expand Up @@ -435,15 +422,6 @@
</value>
</block>
</value>
<next>
<block type="telemetry_update" id="exvivmft-pX$n!1,8Ea$"/>
</next>
</block>
</next>
</block>
</next>
</block>
</next>
</block>
</next>
</block>
Expand Down Expand Up @@ -479,8 +457,6 @@
</block>
</next>
</block>
</statement>
</block>
</next>
</block>
</statement>
Expand Down
Binary file modified .../docs/software/tutorials/images/mecanum-drive/mecanum-drive-blocks-sample-1.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified .../docs/software/tutorials/images/mecanum-drive/mecanum-drive-blocks-sample-2.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified .../docs/software/tutorials/images/mecanum-drive/mecanum-drive-blocks-sample-3.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified .../docs/software/tutorials/images/mecanum-drive/mecanum-drive-blocks-sample-4.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified .../docs/software/tutorials/images/mecanum-drive/mecanum-drive-blocks-sample-5.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file modified ...oftware/tutorials/images/mecanum-drive/mecanum-drive-blocks-sample-complete.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
88 changes: 45 additions & 43 deletions source/docs/software/tutorials/mecanum-drive.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ Before thinking about mecanum, envision a scenario where you have a 2 motor tank

.. code-block::
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!
leftMotor.setPower(y);
rightMotor.setPower(y);
Expand All @@ -54,7 +54,7 @@ Before thinking about mecanum, envision a scenario where you have a 2 motor tank
.. image:: images/mecanum-drive/mecanum-drive-blocks-sample-1.png
:width: 45em

Although at first adding rotation might seem like a difficult task, it’s actually super simple. All you need to do is subtract the x value from the right side, and add it to the left:
Although at first adding rotation might seem like a difficult task, it’s actually super simple. All you need to do is subtract the right X stick value from the right wheels, and add it to the left:

.. tab-set::

Expand All @@ -63,11 +63,11 @@ Although at first adding rotation might seem like a difficult task, it’s actua

.. code-block::
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double x = gamepad1.right_stick_x;
double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!
double rx = gamepad1.right_stick_x;
leftMotor.setPower(y + x);
rightMotor.setPower(y - x);
leftMotor.setPower(y + rx);
rightMotor.setPower(y - rx);
.. tab-item:: Blocks
:sync: blocks
Expand All @@ -76,11 +76,11 @@ Although at first adding rotation might seem like a difficult task, it’s actua
:width: 45em


Here, if the Y stick is pressed upwards, both of the motors will be fed a positive value, causing the robot to move forward. If it is pressed downwards, both of the motors will be fed a negative value, causing the robot to move backwards. A similar principle applies for rotation: if the X stick is pushed rightward, the left wheels will spin forward while the right spin backward, causing rotation. The opposite applies for pushing the stick left. If both sticks are pushed at the same time, say the Y stick is at 1 and the X stick is also at 1, the value of the left wheels will be :math:`1+1=2` (which gets clipped to 1 in the SDK) and the right wheels will be :math:`1-1=0`, which causes a rightward curve.
Here, if the left stick is pressed upwards, both of the motors will be fed a positive value, causing the robot to move forward. If it is pressed downwards, both of the motors will be fed a negative value, causing the robot to move backwards. A similar principle applies for rotation: if the right stick is pushed rightward, the left wheels will spin forward while the right spin backward, causing rotation. The opposite applies for pushing the stick left. If both sticks are pushed at the same time, say the left Y stick is at 1 and the right X stick is also at 1, the value of the left wheels will be :math:`1+1=2` (which gets clipped to 1 in the SDK) and the right wheels will be :math:`1-1=0`, which causes a rightward curve.

Applying omnidirectional movement with :term:`mecanum wheels <Mecanum Wheel>` operates under the same principle as adding turning into the tank example. The left stick X values will be added or subtracted to each wheel depending on how that wheel needs to rotate to get the desired movement. The only difference between adding turning is that rather than wheels on the same side being the same sign, wheels diagonal to each other will be the same sign.
Applying omnidirectional movement with :term:`mecanum wheels <Mecanum Wheel>` operates under the same principle as adding turning into the tank example. The left stick X values will be added or subtracted to each wheel depending on how that wheel needs to rotate to get the desired movement. The only difference from turning is that rather than wheels on the same side being the same sign, wheels diagonal to each other will be the same sign.

We want a positive X value to correlate to rightward strafing. If we refer back to the vectoring image, this means that the front left and back right need to rotate forward, while the back left and front right need to rotate backwards. So, we should add the x value to the front left and back right and subtract it from the back right and front left:
We want a positive left stick X value to correlate to rightward strafing. If we refer back to the vectoring image, this means that the front left and back right need to rotate forward, while the back left and front right need to rotate backwards. So, we should add the x value to the front left and back right and subtract it from the back right and front left:

.. tab-set::

Expand All @@ -89,7 +89,7 @@ We want a positive X value to correlate to rightward strafing. If we refer back

.. code-block::
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!
double x = gamepad1.left_stick_x;
double rx = gamepad1.right_stick_x;
Expand All @@ -110,7 +110,7 @@ We want a positive X value to correlate to rightward strafing. If we refer back

On most drivetrains, you will need to reverse the left side for positive power to move forwards with most motors, and reverse the right side with NeveRests. The presence of gearing between the motor gearbox and the wheel may swap this, which is the case for the goBILDA Strafer and the REV Mecanum Drivetrain Kit.

This is the same as the tank example, except now with 4 motors and the strafing component added. Similarly to the tank example, the Y component is added to all wheels, and the right x (rx) is added to the left and subtracted from the right. Now, we have added another component that will allow us to strafe rightward. In doing that, however, we have actually allowed for strafing in any direction. If you think about it, pressing the joystick to the left will do the same thing in reverse, which is what is needed to strafe left. If it is pressed at 45 degrees, the x and y components of the joystick will be equal. This will cause two diagonal motors to cancel, allowing for diagonal movement. This same effect applies to every angle of the joystick.
This is the same as the tank example, except now with 4 motors and the strafing component added. Similarly to the tank example, the Y component is added to all wheels, and the right X (rx) is added to the left wheels and subtracted from the right. Now, we have added a left X component (x) that allows us to strafe rightward. In doing that, however, we have actually allowed for strafing in any direction. If you think about it, pressing the left joystick to the left will do the same thing in reverse, which is what is needed to strafe left. If it is pressed at 45 degrees, the x and y components of the joystick will be equal. This will cause two diagonal motors to cancel, allowing for diagonal movement. This same effect applies to every angle of the joystick.

Now that we have a functioning mecanum driving program, there are a few things that can be done to clean it up. The first of these would be multiplying the left X value by something to counteract imperfect strafing. Doing this will make the drive feel more accurate on non axis aligned directions, and make field centric driving more accurate. In this tutorial, we will use 1.1, but it’s really up to driver preference.

Expand All @@ -121,7 +121,7 @@ Now that we have a functioning mecanum driving program, there are a few things t

.. code-block::
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double y = -gamepad1.left_stick_y; // Remember, Y stick is reversed!
double x = gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.right_stick_x;
Expand Down Expand Up @@ -185,44 +185,44 @@ Robot-Centric Final Sample Code
public void runOpMode() throws InterruptedException {
// Declare our motors
// Make sure your ID's match your configuration
DcMotor motorFrontLeft = hardwareMap.dcMotor.get("motorFrontLeft");
DcMotor motorBackLeft = hardwareMap.dcMotor.get("motorBackLeft");
DcMotor motorFrontRight = hardwareMap.dcMotor.get("motorFrontRight");
DcMotor motorBackRight = hardwareMap.dcMotor.get("motorBackRight");
DcMotor frontLeftMotor = hardwareMap.dcMotor.get("frontLeftMotor");
DcMotor backLeftMotor = hardwareMap.dcMotor.get("backLeftMotor");
DcMotor frontRightMotor = hardwareMap.dcMotor.get("frontRightMotor");
DcMotor backRightMotor = hardwareMap.dcMotor.get("backRightMotor");
// Reverse the right side motors.
// This may be wrong for your setup.
// Reverse the right side motors. This may be wrong for your setup.
// If your robot moves backwards when commanded to go forwards,
// reverse the left side instead.
// See the note about this earlier on this page.
motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed
double x = gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.right_stick_x;
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
double frontLeftPower = (y + x + rx) / denominator;
double backLeftPower = (y - x + rx) / denominator;
double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator;
motorFrontLeft.setPower(frontLeftPower);
motorBackLeft.setPower(backLeftPower);
motorFrontRight.setPower(frontRightPower);
motorBackRight.setPower(backRightPower);
frontLeftMotor.setPower(frontLeftPower);
backLeftMotor.setPower(backLeftPower);
frontRightMotor.setPower(frontRightPower);
backRightMotor.setPower(backRightPower);
}
}
}
.. tab-item:: Blocks
:sync: blocks

Expand Down Expand Up @@ -295,31 +295,33 @@ Field-Centric Final Sample Code
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
@TeleOp
public class FieldCentricMecanumTeleOp extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
// Declare our motors
// Make sure your ID's match your configuration
DcMotor motorFrontLeft = hardwareMap.dcMotor.get("motorFrontLeft");
DcMotor motorBackLeft = hardwareMap.dcMotor.get("motorBackLeft");
DcMotor motorFrontRight = hardwareMap.dcMotor.get("motorFrontRight");
DcMotor motorBackRight = hardwareMap.dcMotor.get("motorBackRight");
DcMotor frontLeftMotor = hardwareMap.dcMotor.get("frontLeftMotor");
DcMotor backLeftMotor = hardwareMap.dcMotor.get("backLeftMotor");
DcMotor frontRightMotor = hardwareMap.dcMotor.get("frontRightMotor");
DcMotor backRightMotor = hardwareMap.dcMotor.get("backRightMotor");
// Reverse the right side motors.
// This may be wrong for your setup.
// Reverse the right side motors. This may be wrong for your setup.
// If your robot moves backwards when commanded to go forwards,
// reverse the left side instead.
// See the note about this earlier on this page.
motorFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
// Retrieve the IMU from the hardware map
imu = hardwareMap.get(IMU.class, "imu");
IMU imu = hardwareMap.get(IMU.class, "imu");
// Adjust the orientation parameters to match your robot
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
Expand All @@ -332,7 +334,7 @@ Field-Centric Final Sample Code
if (isStopRequested()) return;
while (opModeIsActive()) {
double y = -gamepad1.left_stick_y; // Remember, this is reversed!
double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed
double x = gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.right_stick_x;
Expand All @@ -350,18 +352,18 @@ Field-Centric Final Sample Code
double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio, but only when
// at least one is out of the range [-1, 1]
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
double frontLeftPower = (rotY + rotX + rx) / denominator;
double backLeftPower = (rotY - rotX + rx) / denominator;
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;
motorFrontLeft.setPower(frontLeftPower);
motorBackLeft.setPower(backLeftPower);
motorFrontRight.setPower(frontRightPower);
motorBackRight.setPower(backRightPower);
frontLeftMotor.setPower(frontLeftPower);
backLeftMotor.setPower(backLeftPower);
frontRightMotor.setPower(frontRightPower);
backRightMotor.setPower(backRightPower);
}
}
}

0 comments on commit 5ae9413

Please sign in to comment.