FTC X-Drive code math issue

[Cyberphil]

Hey CD,

FTC 14133 is having some trouble getting our drive train working properly. We can get forward or backwards motion correctly working, but the math causing the robot to strafe seems to be mixed up in some way. The wheels begin to fight each other. We know it is a very simple mathematical error, but we spent 3+ hours today trying to find the error with no success.

Here is the link to the drivetrain portion of the code:

FTC 14133 2022-2023 Drivetrain Code

For reference, l = left, r = right, f = front, b = back

We are using the atan2 function to determine the angle the joystick is facing, and then using the sine of that angle times the speed generated from the combination of the x and y components of the joystick (Pythagorean theorem).

double leftPowerY = -gamepad1.left_stick_y;      //find the value of y axis on the left joystick;
double leftPowerX = gamepad1.left_stick_x;      //find the value of x axis on the left joystick;
double rightPowerX = gamepad1.right_stick_x;     //find the value of x axis on the right joystick;


double angleR = Math.atan2(leftPowerY, leftPowerX)-(Math.PI/2); //Calculating angle of which the joystick is commanded to in radians
double angleD = Math.toDegrees(angleR); //Calculating angle of which the joystick is commanded to in degrees
double speed = Math.sqrt((leftPowerY * leftPowerY) + (leftPowerX * leftPowerX)); //Calculating the magnitude of the joystick

Each wheel is sent through a sine function with the angle of the joystick plus some odd multiple of pi/4 to represent the wheels at all 4 corners of the robot (45, 135, 225, and 315 deg) to calculate each wheel’s individual power

double rfpower = (Math.sin(angleR + (1 * Math.PI / 4)) * speed) + (rightPowerX * rotationK);     //Power level for leftfront
double lfpower = -(Math.sin(angleR + (3 * Math.PI / 4)) * speed) + (rightPowerX * rotationK);     //Power level for rightfront
double lbpower = (Math.sin(angleR + (5 * Math.PI / 4)) * speed) + (rightPowerX * rotationK);      //Power level for rightback
double rbpower = -(Math.sin(angleR + (7 * Math.PI / 4)) * speed) + (rightPowerX * rotationK);    //Power level for leftback

We have all wheels configured to be “forward”, so when they are all running forward the robot rotates CCW, and backwards > CW. We have tried different combinations of forward and reverse, and different combinations of negative and positive, all of which have struggled to get either the strafing or forward/backwards working properly.

Does anyone have any idea where we are going wrong, or at least suggestions to test and troubleshoot?

Thanks!

[bovlb]

final double gearratio=(76/21)*(68/13); //Ratio of the entire drivetrain from the motor to the wheel
final double maxSpeed = 6000 * countsperrev * (1/60); //Counts per S Todo: Determine the real max speed, likely through test
final double inchesperdegrotation = 2 * Math.PI * wheelBaseR * (1/360);

In all three of these lines you’re doing an integer division, which won’t do what you intend. Add “.0” to the end of all these numbers.

[Cyberphil]

Thank you, very much! I’m assuming it does some kind of rounding?

I’m not sure if this was the root of the issue, but it will certainly allow our code to work more as intended.

Edit: Upon review it looks like it completely throws away the remainder, which is certainly not what we want… That explains why our encoder counts were not real world angles or distances. Thank you!

[bovlb]

Right. It rounds down (discards the remainder), so 1/360 becomes 0. Very unhelpful behaviour on Java’s part, and I see students tripping over this all the time.

I don’t know if this is what’s causing your problem. Could you tell us more about your physical drive mechanism? Can you get a clearer picture of what’s going on by running the robot on blocks? Maybe take a video?

[Cyberphil]

Makes total sense now, just not something we were aware of in Java. Thank you!

This is a top-down view of our robot. Positive motor direction for each motor is shown with the arrows in red.

We have a table depicting how we intend the wheels should move vs. how they are actually moving in sheets. The below table shows where we commanded the controller (0 deg being forward) and what the corresponding directions of the wheels are:

I don’t have access to the robot, but I can ask the student to make a quick video I can upload for reference.

[bovlb]

I don’t understand why you would be getting zero in these calculations. Is that based on observing the wheel, or reading the telemetry?

The code you linked and the code you quoted aren’t exactly the same, and I think each has a flaw. I suggest it should be:

double angleR = Math.atan2(leftPowerY, leftPowerX)-(Math.PI/2); // Angle in radians CCW from straight ahead

double lfpower = -(Math.sin(angleR + (1 * Math.PI / 4)) * speed) + (rightPowerX * rotationK);     //Power level for rightfront
double lbpower = (Math.sin(angleR + (3 * Math.PI / 4)) * speed) + (rightPowerX * rotationK);      //Power level for rightback
double rbpower = -(Math.sin(angleR + (5 * Math.PI / 4)) * speed) + (rightPowerX * rotationK);    //Power level for leftback
double rfpower = (Math.sin(angleR + (7 * Math.PI / 4)) * speed) + (rightPowerX * rotationK);     //Power level for leftfront

[JRumple]

I’ve worked with Mecanum drive code and wanted to dive into X-drive this year to get a holonomic drive where I could trust the encoder values.

Thanks to this discussion, I won’t have to start from scratch. I’m going to use your sample code to get started with.

One recommendation I have is when you’re building tables to figure out the software commands, I recommend listing the inputs at the top of the table and the outputs along the side. In other words there is a column for every input and a row for every output.

Don’t worry about combined inputs, like 45 degrees or 135 degrees. Isolate each input and determine the correct sign for the each motor direction.

Then take each cell one at a time.

In a holonomic drive you have three inputs: Drive
Backwards, Strafe Right, and Rotate Clockwise. I chose Drive Backwards because a positive 1 from the joystick Y axis is when the stick is pulled back towards the driver. I chose clockwise because moving the joystick to the right produces a positive X value.

Then If the command is Drive Backwards, RF motor needs to go backwards (or -1), LF motor needs to go forwards (or +1), LB motor needs to forwards (+1), and RB motor needs to go backwards (-1).

Do the same for the command to strafe right and to turn clockwise. The table should have a bunch of 1s, 0s, and -1s. (It looks like in robot-centric X-Drive there are no 0s in your table.)

Now you multiply the value in the cell with the input in the column heading.

This will give you something like
RF Motor Power = -left joystick Y + left joystick X - right joystick X. (I’m pretty sure this equation is wrong, but hopefully you can see how the table gives you the formula by just reading across the row.

Each of the individual cells are easy to determine on their own. What confuses most people about holonomic drive is they are trying to keep everything in their head. (Its the same thing that trips up students trying to do word problems in math class.)

Write down each of the individual answers in the table. The structure of the table gives you the formulas.

What will really blow your mind is that if you convert the table into m x x matrix A and convert your inputs into an n x 1 matrix B, you can multiple A x B and the resulting matrix is an m x 1 matrix of the outputs. In class we often work with square matrices, but they also allow you to do things like three inputs and four outputs.

It looks like you have a good handle on the trigonometry for the X-drive. This means you won’t have any problem building the table for a Driver Centric (often referred to as Field Centric) control for your robot.

Your inputs are South, East, and Clockwise. Your outputs are Backwards, Right, and Clockwise. When you draw your reference frames (or coordinate axes), remember that when the Control Hub is mounted flat, a positive gyroscope angle is counter-clockwise. This is the opposite of most math examples. Of course your positive Y-axis is also down thanks to how the joystick Y is wired in the gamepad.

Then in the code you take the gamepad inputs and first run them through the Driver-Centric to Robot-Centric transformation. You then take the outputs from that and run them through your X-Drive transformation. With that you have your Driver-Centric X-Drive control.

Don’t forget that you want to normalize the outputs so any one motor command doesn’t exceed 1 for full power. If you forget this some of the motor powers will be clipped and you’ll get unexpected drift.

Just add up the magnitude of all the outputs. If they are greater than one, divide each motor value by the sum of the magnitudes of the outputs. If they are less than 1, do nothing. The easiest way to do this in code is to assign the sum of the absolute value of all the outputs to the variable “denominator.” If the denominator is less than 1, reassign it the value of one. Then assign each motor power to the output divided by the denominator.

Take each transformation in turn. Don’t try to figure out the combined equation to jump from gamepad to motor power. Computers are really good at math; humans, not so much. Build the transformation tables one cell at a time. The tables give you the code.

Do all of this in a duplicate OpMode. If it all goes south (pun intended), you still have your Robot-Centric code which is still working. Be sure to include all that you have read here in your Engineering Notebook, your Engineering Portfolio, and your Control Award Submission Form. Make sure you include this diagram of the robot motor configuration. It is critical to understanding how the tables were formed and which direction you need the motors to spin. (Control Award is about using software to make the Driver’s job easier. Driver-Centric Control definitely does that. Plus look at how many teams assume it is hard and avoid it altogether.)

Good Luck!

[Cyberphil]

Both observing the wheel and odometry. At any of the pi/4 angles we would expect the two diagonal wheels perpendicular to the direction of travel to be zero, but we’re not seeing that.

[Cyberphil]

Thank you very much for the reply!

Just to be clear, I am the mentor of the team and have been teaching the students. We got stuck for a long time which is not conducive to inspiration (or low attention spans), so I decided to reach out to CD to keep the ball rolling.

Not sure if you saw my other reply, but we’ve performed all the steps above and then some. Our control algorithm is a bit more complicated as we wanted to perform heading-based motion with the goal of using the same, or at least nearly the same, code for autonomous. We have done mecanum for the past two years, so we are very familiar with the math you described, and while the suggested approach would work for x-drive we wanted to take it to the next level this year, allowing a heading-based input to be commanded.

The maximum speed direction for an x-drive is when traveling at a 45 degree angle, so simply adding joystick inputs will work for basic motion, but if you want to move based on a heading you can use some trig, hense the atan2 to find the joystick heading and sine functions to get the component of the heading relevant to each wheel.

In other words, the speeds of the wheels should look like this for all 360 degrees of possible motion before normalization:

The adding algorithm (what you described) looks like this instead, the same graph scaled slightly differently:

Now, at 0 and 90 degrees you can see none of the wheels are moving at full speed, which is where the normalization comes into play, as you discussed. After normalization the graph looks like this:

Your method (also our previous method) generates the same final output graph but is not based on heading. It’s a minor difference, but it’s something we are trying this year.