# Programming Mecanum - Refined How a Mecanum Drivetrain is programmed largely depends on the driver's preference for how the controller is configured. In our provided example, the left joystick controls forward/back and strafe then the right joystick controls turning. This code is based on the sample provided by *FIRST* for Blocks (BasicOmniOpMode) available in the [Robot Controller Console](https://docs.revrobotics.com/duo-control/hello-robot-blocks/where-to-program#rev-hardware-client). ## **Mecanum Configuration**

Port Type	Port Number	Device Type	Name
Motor	0	REV Robotics Ultraplanetary HD Hex Motor	frontLeft
Motor	1	REV Robotics Ultraplanetary HD Hex Motor	backLeft
Motor	2	REV Robotics Ultraplanetary HD Hex Motor	frontRight
Motor	3	REV Robotics Ultraplanetary HD Hex Motor	backRight

### Mecanum Example Code {% file src="" %} ## Mecanum Code Breakdown {% hint style="info" %} Before diving into mecanum, double check the direction your motors and wheels are spinning. They may need to be reversed if you're experiencing jittering or inverted controls! Adjust the ![](https://docs.revrobotics.com/~gitbook/image?url=https%3A%2F%2F268621232-files.gitbook.io%2F%7E%2Ffiles%2Fv0%2Fb%2Fgitbook-x-prod.appspot.com%2Fo%2Fspaces%252F-MHCAE012xNfg1h3SM9v%252Fuploads%252Fsd50OrsprUDav0TPnrSt%252Fimage.png%3Falt%3Dmedia%26token%3Dca1e5857-bb4c-48c3-a2e2-54044e9fc94b\&width=300\&dpr=4\&quality=100\&sign=d3125b20\&sv=1) block to change the set direction during initialization. {% endhint %} This example makes use of [functions](https://docs.revrobotics.com/duo-control/hello-robot-blocks/part-4-going-beyond/exploring-functions) to help organize the code!

With functions are main loop appears simplified

At the very beginning of our program, our MOTOR\_SETTINGS function is called. Within it the drivetrain motors are set to RUN\_WITHOUT\_ENCODER and are set to run the appropriate direction.

Setting our motor settings within the function

### Establishing Variables Next, we need to create some new variables in order to use mecanum.

Let's break those down first: | Variable | Purpose | | --------------- | -------------------------------- | | forwardBack | Moving forward and backwards | | strafe | Strafing side to side | | turn | Turning left and right | | leftFrontPower | Sets the front left motor power | | rightFrontPower | Sets the front right motor power | | leftBackPower | Sets the back left motor power | | rightBackPower | Sets the back right motor power | ### Calculating Motor Movement with the Gamepad At the beginning of the MECANUM\_DRIVE function, our variables for each movement direction are being set to the value generated by the movement of the matching joystick axis.

Our variables are set to their appropriate joystick movement

Since we now have four motors in play, our equation for setting the needed power to each motor gets a little more complicated. Our robot first needs to determine the combined movement of the gamepads's left joystick:

Motor power being calculated with the left joystick movement

Then calculate with the right stick's value:

Motor power being calculated with both joysticks

All our calculations together allows for movement when the left joystick is moved at an angle, such as for strafing along a diagonal! ### Quick Check! - Understanding Motor Power Let's take a closer look at how our motor power is being determined. For example, our leftFrontPower variable will equal: $$ leftFrontPower = (forwardBack + strafe) + turn $$ So what if we move our left joystick all the way to the left side along the X-axis. To our robot, our equation will read something like this: $$ leftFrontPower = (0 + -1)+ 0 $$ Take a moment to think: What would be the power of our other motors?

What would be the power of our other motors?

Our other motor power would look like the following: * rightFrontPower = (0 - (-1)) + 0 * leftBackPower = (0 - (-1)) - 0 * rightBackPower = (0 + -1) - 0 Or simplified: * leftFrontPower = -1 * rightFrontPower = 1 * leftBackPower = 1 * rightBackPower = -1 As we can see our motors are spinning the same direction as their diagonal partner, meaning the robot will strafe left!

What about a more complicated example? What if we had the left joystick at an angle, all the way to the left and halfway towards the top? Or had our left stick forward and right stick all the way right?

What about a more complicated example?

Looking at our leftFrontPower again as an example the robot might calculate the following: What if we had the left joystick at an angle, all the way to the left and halfway towards the top? * leftFrontPower = (0.5 + -1) + 0 In this situation, our leftFront motor would set the power to -0.5! Or had our left stick forward and right stick all the way right? * leftFrontPower = (1 + 0) + 1 In this case, while our equation equals to 2, our motor cannot power higher than 1 so will cap out at full power! We'll discuss [normalizing motor values](#normalizing-input-values) to remain within range below. Adding telemetry to your program, as seen below, can help to see the values the motors are assigned during different scenarios: ![](https://1359443677-files.gitbook.io/~/files/v0/b/gitbook-x-prod.appspot.com/o/spaces%2FUOOiQ4S2QcMWmVoSmeQ8%2Fuploads%2Fq8dAdhBDBbxvUPUPdOHz%2Fimage.png?alt=media\&token=23696092-84bd-4d03-8d79-4bfc54c7d595)

### Setting Motor Power For our last step, our robot sets the power of each pair of motors based on all our calculations!

Setting each motor's power to their variable

## Normalizing Input Values {% hint style="info" %} This section of code is not within the [provided copy of the mecanum code](#mecanum-example-code) used in this tutorial. Follow the steps below to add it! {% endhint %} While driving, there's a possibility a value may fall outside the range of the motor's power (-1 to 1). To help make sure no inputs are lost because of this, we can use a technique called "normalizing". What normalizing does is take all of our calculated values and scales them appropriately to remain inside the intended range.

### Creating a Max Variable First we need to create a new variable called "max".

In Blocks, we use something called a "list", also known as an "array" to store a set of numbers. In this case, we will be storing all of our motor powers.

But first, we need to add a ![](https://1359443677-files.gitbook.io/~/files/v0/b/gitbook-x-prod.appspot.com/o/spaces%2FUOOiQ4S2QcMWmVoSmeQ8%2Fuploads%2FqBkR165GYhHqFF15J2Ka%2Fimage.png?alt=media\&token=347d767f-e573-427c-a8d7-f627fcfea623) block from our "Math" menu. We will change this using the dropdown to "max", meaning it is returning the largest value from our list of motor powers.

Setting our variable to the highest motor value

Since our motor power will sometimes be negative, such as when turning in reverse, we want to make sure we're using the absolute value of our motor powers.

Finding the "absolute" block in the "Math" menu

### Creating the If/Else Statement Next, we will set up our[ If/Else](https://docs.revrobotics.com/duo-control/part-1/programming-servos/using-a-gamepad-with-a-servo#introducing-if-else-statements) to check if our "max" is higher than 1 and therefore outside the motor's range.

Using this statement, we'll readjust each of our motor's power back to be within range proportionally by dividing each by the max value.

Now our full drivetrain function will look like the following: