Holonomic X-Drive Tutorial Series

Over the summer, I decided to keep myself busy by not only completing my Eagle Scout project, but also by doing some coding for something that seems to really be underdeveloped. As I’m sure you can infer by the title, the tutorial series will be over holonomic x-drives. One issue I’ve seen with development for holonomic x-drives is that it does a poor job of making the programmer’s life easy when creating autonomous routines, not to mention it’s somewhat difficult to understand how you can start adding x+y-r for a motor to get it to work. This tutorial will go over how holonomic drives work, how vectoring plays an important role in these drive systems, and how to apply trigonometry to get a useful function.

As described in the title, this will be a tutorial series. The first part will be devoted to understanding the concepts of holonomic drives, along with developing all of the formulas for each wheel. The second part will consist of programming those formulas. The third part will discuss the theory of using joystick with these functions, and writing code for the joystick to work with the functions. I’ll also have a fourth part where we conclude and talk about what can be further developed.

I will not be publishing the tutorials on my blog, the code will be published on GitHub (release will come with part 2).

This post is to announce the release of the first part of the tutorial. I will be updating this post with the releases of the other parts. I hope to have them all released by the end of the month.

[LIST]
*]Part 1 - Theory of Movement
*]Part 2 - Programming Radians to Holonomic
*]Part 3 - Theory and Programming of Joystick Cartesian to Polar Conversions
*]Part 4 - Conclusion
[/LIST]

P.S. I feel somewhat obliged to tell you there are Java animations creating using the Cinderella geometry software. Java will give you a warning, but there should not be anything malicious on the applet

Part 2 has been posted. You can also access full versions of the code for PROS and ROBOTC on GitHub. I believe there is an issue with Internet Explorer viewing the embedded code, I know Chrome works. The header has also been copied below.

/*
* HolonomicRadians.h
*
* Licensend under the Lesser General Public License v3.0. http://goo.gl/fB9rz (c) 2013
*      Author: Elliot Berman
*/

#define maxMotorSpeed 127
#define numberOfMotors  4

//**-----------------------SUPPORT FUNCTIONS-----------------------------------**//

float FindMaxFloat(float a, float b,  float c = (0), float d = (0), float e = (0),
									 float f = (0), float g = (0), float h = (0), float i = (0), float j = (0))
{
	float values] = {a,b,c,d,e,f,g,h,i,j};
	float maxValue = 0;
	for(int z = 0; z < 9; z++) {
		if(abs(values[z]) > maxValue) maxValue = abs(values[z]);
	}
	return maxValue;
}





//**-----------------------MAIN FUNCTIONS------------------------------------**//

void HolonomicRadianOutput(float radians, float speed = 1, byte rotation = 0)
{
	// Please refer to README.txt for a full explanation of the formulas used.
	if(speed > 0)
	{
		float frontLeftOutput 	= -maxMotorSpeed * cos(PI/4 - radians),
					frontRightOutput 	=  maxMotorSpeed * cos(PI/4 + radians),
					rearRightOutput 	=  maxMotorSpeed * cos(PI/4 - radians),
					rearLeftOutput		= -maxMotorSpeed * cos(PI/4 + radians);

		frontLeftOutput += rotation;
		frontRightOutput += rotation;
		rearRightOutput += rotation;
		rearLeftOutput += rotation;


		float maxOutput = FindMaxFloat(frontLeftOutput, frontRightOutput, rearRightOutput, rearLeftOutput);
		// The goal is to rescale all values to -127<=out<=127. See README.txt for algebraic explanation.
		speed *= (maxMotorSpeed / maxOutput);


		frontLeftOutput *= speed;
		frontRightOutput *= speed;
		rearLeftOutput *= speed;
		rearRightOutput *= speed;

		motor[frontLeft] = (byte)frontLeftOutput;
		motor[frontRight] = (byte)frontRightOutput;
		motor[rearLeft] = (byte)rearLeftOutput;
		motor[rearRight] = (byte)rearRightOutput;
	}
	else if (rotation > 20 || rotation < -20)
	{
		motor[frontLeft] = rotation;
		motor[frontRight] = rotation;
		motor[rearLeft] = rotation;
		motor[rearRight] = rotation;
	}
	else
	{
		motor[frontLeft] = 0;
		motor[frontRight] = 0;
		motor[rearLeft] = 0;
		motor[rearRight] = 0;
	}
}

#define JOY_Y_DEADBAND 20
#define JOY_X_DEADBAND 20

typedef struct
{
	float radians;
	float speed;
} PolarJoystick;

void getPolarJoy(float *radians, float *speed, TVexJoysticks joy_x = Ch2, TVexJoysticks joy_y = Ch1) {
	byte x_val = vexRT[joy_x];
	byte y_val = vexRT[joy_y];
	if((abs(x_val) < 20) && (abs(y_val) < 20)) {
		*radians = 0;
		*speed = 0;
	}
	else {
		*radians = (PI/2)-atan2(y_val,x_val);
		float *speed = sqrt((abs(y_val) * abs(y_val)) + (abs(x_val) * abs(x_val)));
		*speed = *speed/127;
		if(speed > 1) *speed = 1;
	}
}

I’ve read your posts half a dozen times, and I think I must be missing something. Why is what you’re doing better than the normal way we program a Holonomic Drive where you add/subtract the joystick channels to achieve movement in all four directions? You have 100 lines of code for something that’s doable in 4. As far as I can see, it’s even inferior to the other way because at the moment the code does not allow the robot to turn.

If this were for Autonomous coding I can see the value, but that doesn’t look like what you wrote. Can you explain why I should use this on my robot this year?

The code does allow the robot to turn (it’s the rotation parameter). I hadn’t talked about rotation until programming the function, I’ll put in a few notes about it the theory post.

Other than this working really well for Autonomous, this could be useful in many different applications in Operator Control. For one, you could tie a gyroscope to the angle output so no matter what orientation the robot is at, the robot will go “forward” from the driver’s perspective. Using other sensors, it should be significantly easier to develop self-correcting holonomic control, along with traditional “go forward 600 ticks on the encoder” (except you could go any direction, obviously). I made a decision to also point out that this code could be used for driver control, and not only autonomous. In the next part of the tutorial, I’ll go over the (somewhat basic) trig of converting the joystick coordinates to work with this function.

For the average, run-of-the-mill team, I could understand why this code wouldn’t be useful. For those teams looking to get more out of holonomic drives, this could be the very thing they need.

From a physics standpoint, adding/subtracting joystick channels doesn’t take advantage of holonomic drive systems, for many of the reasons above.

I missed the rotation bit. My bad.

I see how you could implement a Gyroscope in, I guess. A fixed perspective would not be the way that I would want to drive a robot personally, but I see how someone might find it easier. It’s an interesting concept.

No idea what you mean by the Physics bit. If you set the channels up correctly you’re able to achieve perfect motion without any of the math. We’ve got our robot doing it right now.

I see bugs in both versions. Brownie points for those who spot them. For example, try this.

    double  a[6] = {1.0, 2.0, 0.5, 3.0, 5.0, 9.0 };
  
    printf("%f\n", FindMaxdouble(a));

:slight_smile:

My point with the physics bit is that this function is a direct translation of the physics behind holonomic drives, as read in part 1.

You’re right about being able to achieve the same thing with the code that’s currently accepted. You are definitely able to do more with this bit of code than the previous one. It’s up to you to determine whether or not this code could be useful for you. Personally, if I was only trying to do the basic holonomic movement, I, too, would use the “normal” code. But as I’d like to do more with holonomic, I developed this code. :slight_smile:

I ported this code over to ROBOTC, to see what the issue was. (I don’t have access to a Cortex to test PROS on at the moment, will do it tonight) It seemed to run fine - it returned 9. But after a closer look at the code, I realized I forgot to include to only compare the absolute values of all the variables. I’d thought about it, included it in one of my iterations, and then rewrote it all, only to forget about it. Don’t know if that’s what you were talking about, but that will updated within the next hour or two.

No, the issue is with this code (and I’m not compiling for the cortex either, just viewing it).

double FindMaxdouble(double a ])
{
        double maxdouble = 0;
        for (int i = 0; i < sizeof(a); i++)
        {
                if (a* > maxdouble) maxdouble = a*;
        }
        return maxdouble;
}

You are using the “sizeof” function, this will always return 4, the size of the pointer to the array.

The ROBOTC version of your code works differently.

Edit:
Now I think about it what you wrote may work if compiled with C99 extensions (which I usually avoid), let me check that.**


        double maxdouble = 0;

This would return the max positive value or if you sent in all negatives it would return 0. Initialization should be the most minimum float value allowed (whatever that is as I am far too lazy to look it up).

Also, the atan2 is tricky. You seem to start from 180 degrees, not 0 so maybe you have this covered like I have seen. Tangent has the same values in the 180-360 degree range as you do 0-180 range. The phase of tangent is only 180 degrees (or pi/2 radians).

So somehow you need to account for this to use the other half of the joystick.

Next, a total nit pick, there’s no need for doing absolute values when squaring a number. The sign always drops out and I bet calling abs() on it would make for more computational work, than less. No biggie though. You still get to the same answer.


sqrt((abs(y_val) * abs(y_val)) + (abs(x_val) * abs(x_val)));

And yes to Ephemeral_being, this polar coordinate drive system works incredibly well during diving as your are working off your field frame of reference versus the robot’s. A gyro is needed to know the relative frame of reference and do some more math. (In just a wee bit more than 4 lines of code)

Maybe he’ll post that part in the next article… It’s like waiting for Christmas! :wink:

Keep writing edjubuh, this is good stuff!

Also, the atan2 is tricky. You seem to start from 180 degrees, not 0 so maybe you have this covered like I have seen. Tangent has the same values in the 180-360 degree range as you do 0-180 range. The phase of tangent is only 180 degrees (or pi/2 radians).

So somehow you need to account for this to use the other half of the joystick.

atan2’s range is from (-180,180], which acts the same in trig functions as [0,360). I double checked on Wikipedia. Regardless, it works correctly. The code has been pretty thoroughly tested… wouldn’t have missed something that obvious :smiley:

[QUOTE]
Next, a total nit pick, there’s no need for doing absolute values when squaring a number. The sign always drops out and I bet calling abs() on it would make for more computational work, than less. No biggie though. You still get to the same answer.


sqrt((abs(y_val) * abs(y_val)) + (abs(x_val) * abs(x_val)));

Probably right… Apparently ROBOTC doesn’t like squaring, it never seemed to work correctly. Will remove abs() on the magnitude function in the next updated version.

BTW, I was planning on writing/publishing this part in the middle of last week, but I was holed up getting ready for competition. I do plan have the next part out by the end of the week.

Not much more than 4 lines, this is what I usually do. rotate the forward and right vectors.

void
DriveSystemMecanumTask()
{
    int forward, right, turn;
    int temp;
    float theta;

    // Get joystick values
    DriveSystemGetControlValues( &forward, &right, &turn );

    // Field centric control
    if( theDrive.type == Mecanum4MotorFieldCentric )
    {
        // Get gyro angle in radians
        theta = degreesToRadians( GyroGetAngle() );

        // rotate coordinate system
        temp  = forward * cos(theta) - right * sin(theta);
        right = forward * sin(theta) + right * cos(theta);
        forward = temp;
    }

    // Send to drive
    DriveSystemMecanumDrive( forward, turn, right );
}

Last time I tried making the gyro work with a holonomic drive to keep the frame of reference constant I had horrible problems with the gyro being way out of wack just spinning in place 2 or 3 times. Does anyone have anecdotal evidence that this method actually works well?

Unfortunately none of our teams are doing holo this year so I have nothing to play with. I’ll see if I can have somebody built me an X-Drive so I can mess around with it.

So I just tried the ROBOTC version, it needs some TLC.

Issues.

  1. you include this non-existant file
    #include “HolonomicPolarJoystick.h”;

  2. Adding the jumper in digital 1 to enable the joystick, run the code and it crashes when using joystick 1 & 2, the issue is this function.

void getPolarJoy(float *radians, float *speed, TVexJoysticks joy_x = Ch2, TVexJoysticks joy_y = Ch1) {
    byte x_val = vexRT[joy_x];
    byte y_val = vexRT[joy_y];
    if((abs(x_val) < 20) && (abs(y_val) < 20)) {
        *radians = 0;
        *speed = 0;
    }
    else {
        *radians = (PI/2)-atan2(y_val,x_val);
        float *speed = sqrt((abs(y_val) * abs(y_val)) + (abs(x_val) * abs(x_val)));
        *speed = *speed/127;
        if(speed > 1) *speed = 1;
    }
}

Two big problems here.
You have a parameter and a variable both called speed, the variable is a pointer, but it’s not pointing at anything, hence the crash.
You compare speed to 1, but speed is the address not the data.

Perhaps this would be better.

void getPolarJoy(float *radians, float *speed, TVexJoysticks joy_x = Ch2, TVexJoysticks joy_y = Ch1) {
    int  x_val = vexRT[joy_x];
    int  y_val = vexRT[joy_y];
    if((abs(x_val) < 20) && (abs(y_val) < 20)) {
        *radians = 0;
        *speed = 0;
    }
    else {
        *radians = (PI/2)-atan2(y_val,x_val);
        float tmp = sqrt( (y_val * y_val) + (x_val * x_val) );
        *speed = tmp/127;
        if(*speed > 1) *speed = 1;
    }
}

Give ROBOTC the correct instructions and it’s quite happy doing this.

I will try the PROS version later when I have time, I have a few comments about that also regarding the use of header files with C code in them.

Yes, it works well, there is some drift over a few minutes but there are ways around that as well.

Apparently, it’s poor practice to make changes and publish them before testing :smiley: … I’ve made several additional changes to the code. There were some issues over commits and syncing over several computers (I’m new to GitHub). Everything should be fixed now.

The third part of the series is up. It’s a quickie, and goes over how to convert joystick coordinates to polar coordinates, used in the functions.

For the fourth part, I’ll explain the test program, talk about what else you can do with this library, and I would say more, but that’s really going to be about it.

P.S. I can’t edit the original post, so it won’t be updated, unfortunately.

The fourth and final part of this series has been posted. Check it out!

We go over the test code, what it does, and how it works. For the conclusion, I toss around some ideas for how the code can be extended to operate with sensors.