holonomic user control

I need help trying to figure out how to write the code for the controller. I’m wanting to test both an x-drive and a mecanum drive, but those should both be almost the same right? and I’m using ROBOTC 4.x to program.

See the last post in this thread.
Holonomic drives: A video tutorial by Smartkid

Here’s a good thread on Mecanum wheels from a while back.


There actually might be a small issue with that code, it’s the negatives next to C1LY. We’re currently running a custom version of the code configured like this successfully in PROS.

The following code seems to be correct. Notice the C1LY’s are - + - + not + - - +

/* Cody's RobotC optimized X-Holonomic Code
 * ========================================
 * Pretty much as fast and clean as it gets.
 * Usage:	Drive code for "X" Holonomic drives
 * License:	Public Domain, use at your own risk.
 * Author:	Cody Smith (Smith@Polynomic3D.com)

// Controller (C), # (1/2), Stick (Left/Right), Axis (X/Y)
#define C1LX vexRT[Ch4]
#define C1LY vexRT[Ch3]
#define C1RX vexRT[Ch1]
#define C1RY vexRT[Ch2]
#define C2LX vexRT[Ch4Xmtr2]
#define C2LY vexRT[Ch3Xmtr2]
#define C2RX vexRT[Ch1Xmtr2]
#define C2RY vexRT[Ch2Xmtr2]

// Code only uses C1LX, C1LY and C1RX, just inc. all of them for teh lolz

task main() {

	while(true) {
		// All drive logic (Change M1-4 to match your motor config)
		// Y component, X component, Rotation
		motor[M1] =  -C1LY - C1LX - C1RX;	// Front left wheel
		motor[M2] = C1LY - C1LX - C1RX;	// Front right wheel
		motor[M3] = -C1LY + C1LX - C1RX;	// Back right wheel
		motor[M4] =  C1LY + C1LX - C1RX;	// Back left wheel
		/*       ROBOT
		 * =================
		 *     ♦ FRONT ♦
		 *   ♦           ♦
		 * ♦  M1       M2  ♦
		 *         ↑
		 *        ←X→
		 *         ↓
		 * ♦  M4       M3  ♦
		 *   ♦           ♦
		 *     ♦       ♦  

		// Wait 20ms for the next motor update

At the time of writing that I didn’t have a robot to test with, looks like I got something a tiny bit wrong. If someone can confirm this for me in ROBOTC I’ll update the thread. I also changed the wait statement to a 20ms wait since I now know that the motor controllers update in 20ms intervals. Also having a constant ∆T is good for lots of reasons (control loops, physics constraints, etc).

Hope the code helps. -Cody

Neither version matches what I do, however, I think some of the issue is whether the motors are reversed in the motors&sensors setup dialog. For reference, this is the code I use which assumes positive control values always move the robot/motor forwards (ie. right hand motors are usually reversed in the setup dialog).

DriveSystemMecanumDrive( int forward, int turn, int right )
    long drive_l_front;
    long drive_l_back;
    long drive_r_front;
    long drive_r_back;

    // Set drive
    drive_l_front = forward + turn + right;
    drive_l_back  = forward + turn - right;

    drive_r_front = forward - turn - right;
    drive_r_back  = forward - turn + right;

    // normalize drive so max is 127 if any drive is over 127
    int max = abs(drive_l_front);
    if (abs(drive_l_back)  > max)
        max = abs(drive_l_back);
    if (abs(drive_r_back)  > max)
        max = abs(drive_r_back);
    if (abs(drive_r_front) > max)
        max = abs(drive_r_front);
    if (max>127) {
        drive_l_front = 127 * drive_l_front / max;
        drive_l_back  = 127 * drive_l_back  / max;
        drive_r_back  = 127 * drive_r_back  / max;
        drive_r_front = 127 * drive_r_front / max;

    // Send to motors
    // code removed but was essentially
    // motor left_motor_front ] = drive_l_front;
    // etc.

This may be called as follows (although I usually set this up quite differently and use buttons for rotation).

    DriveSystemMecanumDrive( vexRT[Ch3], vexRT[Ch4], vexRT[Ch1] );

It works! the problem (not a very big deal) is that some genius at robomatter decided to deprecate wait10msecs, but just had to do wait1msec(20). also, is it possible to make it go diagonally? maybe it does on x-drive, I have only tried it on macanum so far.

Nevermind I figured it out, I feel so stupid now XD also I’m soon going to test it on the x-drive.

Good to know, thanks. Nice to see that the code is working out well for you.

Tested it on x-drive. And I found, much to my surprise that was faster going in all directions. so we I what kind of drive to use for the next game.