Omni wheel controller code ( help me )

Hello …

I have problem . How can I make conreoller code for omni wheel

Please give me a code or help me by any thing …

Our robotic like this
/…\ <---- This is like our vex robotic
…/

Thanks

I am waiting for you !

Cody has a great tutorial on all the types with some sample code.

https://vexforum.com/t/holonomic-drives-2-0-a-video-tutorial-by-cody/27052/1

You have an X holonomic. The first set of code should do what you want it to.

What does it mean ( holonomic )?

Thanks so much about coding

The type of drives that have the ability to move side ways as well as forward and backward.

A lot of the categories are here

http://botsnstuff.com/wiki/Category:Holonomic

A holomonic drive is one which can move in any direction instantly without rotating. The main type used is X-drive, like yours, but it can also be done with mecanum wheels or the ‘H’ drive - a standard tank drive with one or two omni wheels placed at 90 degrees to the other wheels.

Have a look on the bots ‘n’ stuff website, they have some good information on the different types of drive trains.

Omni wheels configured in an X, +, Mecanum or Kiwi (three wheels 120 degrees apart) is called a holonomic drive.

It’s kind of just another term for omni drive.

I’m Cody, the guy who wrote the tutorial and code, I’m here lurking, feel free to ask any questions in that thread if you’d like.

thanks so much … :slight_smile:

Thanks so much … :wink:

thanks my friend

but I have question …
this is code for the joystick

but How can make " X … holonomic " for Autonomies ?
I am waiting for you !

That’s a big question.

This code lets you move the robot forward/backward and left/right and it lets you rotate. It’s entirely possible to break a movement command into forward/backward, left/right parts and run control loops on them.

I’ve done it with limited success.

can you help and give me an example … ?

Please …

Here is a snippet of old code that shows the general idea.

A function is used that takes three variables, forwards, turn and right. I call this function to make the robot drive and turn as needed. Sorry, this is probably a bit more complicated than you wanted (edit: but less complicated than Cody just posted :slight_smile: ) but it should give the general idea.

/*-----------------------------------------------------------------------------*/
/*  Mecanum drive                                                              */
/*-----------------------------------------------------------------------------*/

void
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
    // left drive
    SetMotor( MotorLF, drive_l_front);
    SetMotor( MotorLB, drive_l_back);

    // right drive
    SetMotor( MotorRF, drive_r_front);
    SetMotor( MotorRB, drive_r_back);
}

A drive forward function.


/*-----------------------------------------------------------------------------*/
/*  Drive forwards until encoder count has elapsed or timeout reached          */
/*  optionaly stop motors at end                                               */
/*-----------------------------------------------------------------------------*/

void
DriveForwardsEncoder( int distance, bool stopdrive = true )
{
    long    cur_position;
    long    target_position;
    long    currentTime;

    cur_position = nMotorEncoder MotorLF ];

    target_position = cur_position + distance;

    // Start driving - bias to left
    if(distance > 0)
        DriveSystemMecanumDrive( DRIVE_FORWARD_SPEED, 0,  0 );
    else
        DriveSystemMecanumDrive( -DRIVE_FORWARD_SPEED, 0, 0 );

    // wait for end - 5 seconds maximum
    for( currentTime = nSysTime; currentTime > (nSysTime - DEFAULT_DRIVE_TIMEOUT);)
        {
        cur_position = nMotorEncoder MotorLF ];
        if( distance >= 0)
            {
            if( cur_position >= target_position )
                break;
            }
        else
            {
            if( cur_position <= target_position )
                break;
            }

        wait1Msec(10);
        }

    // Stop driving
    if( stopdrive )
        DriveSystemMecanumDrive( 0, 0, 0 );
}

I can try…

My code aimed to allow a translation from any one point, to another.

DISCLAIMER: I don’t claim that ANY of the following code is/was stable, you basically should take only concepts from this.

We had PID code, if you don’t know what PID is or how it works, stop here and go look into that first, then come back.

We had some pretty intense drive code, can’t post it all (it’s 28,000 chars), but here’s the parts we care about…


void driveHolo(MotorGroup * fl, MotorGroup * fr, MotorGroup * br, MotorGroup * bl, int x, int y, int r) {

	goGroup(fl,	- y	- x	- r);		// Front left wheel
	goGroup(fr,	  y	- x	- r);		// Front right wheel
	goGroup(br,	  y	+ x	- r);		// Back right wheel
	goGroup(bl,	- y	+ x	- r);		// Back left wheel

}

bool goHoloXReallyDumb(Axle * a, Axle * b, Gyroscope * g, ControlLoopConfig clX, ControlLoopConfig clY, ControlLoopConfig clR, double targetX, double targetY, Time timeout) {

	print("STARTING GO-HOLO-X-CL\r\n");

	double x = 0, y = 0;									// Actual X, Y positon best calculated

	double angle, distance;								// Used in the polar conversion

	short samplesX = 0, samplesY = 0, samplesR = 0;		// Points!
	bool happyX, happyY, happyR;

	// Prepare control loop data structs
	ControlLoopData clXd, clYd, clRd;
	controlLoopInit(&clXd);
	controlLoopInit(&clYd);
	controlLoopInit(&clRd);

	timeout = millis() + timeout;			// Time to give up (timeout)

	// OK clean the slate
	resetTicksAxle(a);	resetTicksAxle(b);

	while(isAutonomous()	 || isEnabled()) {

		// === GATHER DATA

			// Get the distance traveled by each wheel minus the rotational movement
			double flDistLF = getDistanceFromAxle(a, LEFT);
			double brDistLF = getDistanceFromAxle(a, RIGHT);
			double frDistLF = getDistanceFromAxle(b, LEFT);
			double blDistLF = getDistanceFromAxle(b, RIGHT);

			// Immediately reset the encoders so we don't miss ANY ticks
			resetTicksAxle(a);	resetTicksAxle(b);

			// Get the average distance traveled by each axle
			double aveDistALF = (flDistLF - brDistLF) / 2;
			double aveDistBLF = (frDistLF - blDistLF) / 2;

		// === AUGMENT & INTEGRATE LAST FRAMES DATA

			// --- STEP 1: WHEEL -> ROBOT SPACE

				cartToPol(aveDistALF, aveDistBLF, &distance, &angle);
				angle -= M_PI/4;
				polToCart(distance, angle, &aveDistALF, &aveDistBLF);

				// AT THIS POINT (HEADING) Y == A  && (HEADING) X == B

				// How far did we just go in the X & Y direction?
				x += aveDistALF;		y -= aveDistBLF;

			// How much farther do we need to move in the X & Y direction?
			double xRemaining = targetX - x;
			double yRemaining = targetY - y;

		// === PREFORM CHECKS

			if(within(targetX - x, -.5, .5))	{ samplesX++; happyX = true; } else happyX = false;
			if(within(targetY - y, -.5, .5))	{ samplesY++; happyY = true; } else happyY = false;

			// Exit condition
			if(samplesX >= 50 && samplesY >= 50 &&  samplesR >= 50 && happyX && happyY && happyR) {

				driveHolo(a->left, b->left, a->right, b->right, 0, 0, 0);

				print("ENDING GO-HOLO-X-CL with a return of true\r\n");

				return true;

			}

		// === SETUP THE MOVE FOR THIS FRAME

			// --- STEP 3: RUN LOOPS & DRIVE

				int speedX = (int) computePid(&clX, &clXd, -xRemaining, 0);
				int speedY = (int) computePid(&clY, &clYd, -yRemaining, 0);

				driveHolo(a->left, b->left, a->right, b->right, speedX, speedY, 0);

		// === CLEANUP & RESET FOR NEXT FRAME

			// Did we time out?
			if(millis() > timeout) {

				driveHolo(a->left, b->left, a->right, b->right, 0, 0, 0);

				return false;

			}

			delay(100);

	}

	return false;	// We became disabled outside of autonomous

}

bool goHoloX(Axle * a, Axle * b, Gyroscope * g, ControlLoopConfig clX, ControlLoopConfig clY, ControlLoopConfig clR, double targetX, double targetY, Time timeout) {

	print("STARTING GO-HOLO-X-CL\r\n");

	double x = 0, y = 0;									// Actual X, Y positon best calculated

	int initialHeading = getHeading(g);					// Initial heading

	int lastHeading = initialHeading;					// Last heading

	double angle, distance;								// Used in the polar conversion

	short samplesX = 0, samplesY = 0, samplesR = 0;		// Points!
	bool happyX, happyY, happyR;

	// Prepare control loop data structs
	ControlLoopData clXd, clYd, clRd;
	controlLoopInit(&clXd);	controlLoopInit(&clYd);	controlLoopInit(&clRd);

	timeout = millis() + timeout;			// Time to give up (timeout)

	// OK clean the slate
	resetTicksAxle(a);	resetTicksAxle(b);

	while(isAutonomous()	 || isEnabled()) {

		// === GATHER DATA

			// What is our active heading?
			int heading = getHeading(g);

			// Determine degrees and direction
			int remainingDegrees =		initialHeading - heading;
			if(remainingDegrees > 180)	remainingDegrees -= 360;
			if(remainingDegrees < -180)	remainingDegrees += 360;

			// How far did we just rotate?
			int degTraveledLF = heading - lastHeading;

			// How far did we rotate?
			double rotDistLF = M_PI * a->diameter * (degTraveledLF / 360);

			// Get the distance traveled by each wheel minus the rotational movement
			double flDistLF =	getDistanceFromAxle(a, LEFT) - rotDistLF;
			double brDistLF =	getDistanceFromAxle(a, RIGHT) - rotDistLF;
			double frDistLF =	getDistanceFromAxle(b, LEFT) - rotDistLF;
			double blDistLF =	getDistanceFromAxle(b, RIGHT) - rotDistLF;

			// Immediately reset the encoders so we don't miss ANY ticks
			resetTicksAxle(a);	resetTicksAxle(b);

			// Assume measurements taken between now and last frame happened @ lastHeading + (degrees / 2)
			double aveHeadingLF = normalizeHeading(heading - ((double) degTraveledLF / 2));

			// Get the average distance traveled by each axle
			double aveDistALF = (flDistLF - brDistLF) / 2;
			double aveDistBLF = (frDistLF - blDistLF) / 2;

		// === AUGMENT & INTEGRATE LAST FRAMES DATA

			// --- STEP 1: WHEEL -> ROBOT SPACE

				cartToPol(aveDistALF, aveDistBLF, &distance, &angle);
				angle -= M_PI/4;
				polToCart(distance, angle, &aveDistALF, &aveDistBLF);

				// AT THIS POINT (HEADING) Y == A  && (HEADING) X == B

			// --- STEP 2: ROBOT -> INITIAL HEADING SPACE

				cartToPol(aveDistALF, aveDistBLF, &distance, &angle);

				if(heading > initialHeading)			angle += degToRad(aveHeadingLF - initialHeading);
				else if(heading < initialHeading)	angle -= degToRad(initialHeading - aveHeadingLF);

				polToCart(distance, angle, &aveDistALF, &aveDistBLF);

				// AT THIS POINT (INIT HEADING) Y == A  && (INIT HEADING) X == B

				/*if(heading > initialHeading) {
					angle = degToRad(initialHeading - heading);
				} else if(heading < initialHeading) {
					angle = degToRad(heading - initialHeading);
				}*/

				// How far did we just go in the X & Y direction?
				x += aveDistALF;		y -= aveDistBLF;

			// How much farther do we need to move in the X & Y direction?
			double xRemaining = targetX - x;
			double yRemaining = targetY - y;

		// === PREFORM CHECKS

			if(within(targetX - x, -2.5, 2.5))	{ samplesX++; happyX = true; } else happyX = false;
			if(within(targetY - y, -2.5, 2.5))	{ samplesY++; happyY = true; } else happyY = false;
			if(within(initialHeading - heading, -1, 1)) { samplesR++; happyR = true; } else happyR = false;

			// Exit condition
			if(samplesX >= 50 && samplesY >= 50 &&  samplesR >= 50 && happyX && happyY && happyR) {

				driveHolo(a->left, b->left, a->right, b->right, 0, 0, 0);

				print("ENDING GO-HOLO-X-CL with a return of true\r\n");

				return true;

			}

		// === SETUP THE MOVE FOR THIS FRAME

			// --- STEP 1: DETERMINE SET POINTS

				cartToPol(xRemaining, yRemaining, &distance, &angle);

				if(heading > initialHeading)			angle -= degToRad(heading - initialHeading);
				else if(heading < initialHeading)	angle += degToRad(initialHeading - heading);

				polToCart(distance, angle, &xRemaining, &yRemaining);

			// --- STEP 2: DETERMINE GAINS

				// What are our percentages?
				//double xPercent = (x / targetX) * 100;
				//double yPercent = (y / targetY) * 100;

			// --- STEP 3: RUN LOOPS & DRIVE

				int speedX = (int) computePid(&clX, &clXd, -xRemaining, 0);
				int speedY = (int) computePid(&clY, &clYd, -yRemaining, 0);
				int speedR = (int) computePid(&clR, &clRd, -remainingDegrees, 0);

				// Limit X speed
				if(speedX < 0 && speedX > -35)	speedX = -35;
				if(speedX > 0 && speedX < 35)	speedX = 35;

				// Limit Y speed
				if(speedY < 0 && speedY > -35)	speedY = -35;
				if(speedY > 0 && speedY < 35)	speedY = 35;

				// Limit R speed
				//if(speedY < 0 && speedY > -20)	speedY = -20;
				//if(speedY > 0 && speedY < 20)	speedY = 20;

				driveHolo(a->left, b->left, a->right, b->right, speedX, speedY, speedR);

		// === CLEANUP & RESET FOR NEXT FRAME 

			// Set last heading
			lastHeading = heading;

			// Did we time out?
			if(millis() > timeout) {

				driveHolo(a->left, b->left, a->right, b->right, 0, 0, 0);

				return false;

			}

			delay(100);

	}

	return false;	// We became disabled outside of autonomous

}

There are two versions of the code here, goHoloXReallyDumb() is the stripped down code I wrote at worlds when the planned code, goHoloX, wasn’t working right.

I was writing this code without a robot to test on.

driveHolo() is basically the same code for tele-op, except instead of being fed by a joystick, I fed it using PID loops. One for left/right, one for forward/backwards and one for rotation.

goHoloXReallyDumb() basically just sets up the PID loops, and updates them as the action unfolds.

The “happy” vars allows the robot to settle before the action ends.

It’s a pretty simple idea really. With a nice robot, a field and some time I’m sure I could get it working really well.

If you have encoders equipped and want to track your position and angle on the floor. We wrote this paper.
https://vexforum.com/t/omniwheel-holonomic-navigation-paper/39733/1

Please don’t revive such old threads as this one. Create a new thread and link the old one instead.