# Omnidrive help

I am trying to program an omni drive equation for a four wheel system and I was wondering if anyone had a link to a post explaining how, could start me off in the right direction, or hook me up with some “sample code”.

Well, actually, you are extremely lucky. I have developped the code for a omnidrive system that my team is not building this year (I thought we would build it, but the other team members voted not to). If you PM me, I’d be glad to share it with you.

Assuming you have your omniwheels in a typical 45 degree setup “cutting the corners” of your drive base, the code is conceptually very simple.

Start with three values: frontward velocity, rightward velocity, and clockwise turn rate. (Negative values will represent backward, leftward, or counterclockwise.)

Compute the four motor speeds like this:
left front = forward + rightward + turn
left rear = forward - rightward + turn
right rear = forward + rightward - turn
right front = forward - rightward - turn

Of course, there are more details to work out, like how to deal with overflowing the valid range of motor control, and how to determine the control values in the first place.

I am posting the code. It’s not perfect, but it gets the point across and should be good enough for a first robot. I put tons of explanations and comments in the code so that it can be understood. It’s difficult to understand how the holonomic drive works at all, however, so if you are confused, reread. If you are familiar with the holonomic drive it’ll be much easier.

``````
/* This is the code for a holonomic drive.

Team 321 was thinking of building one, but decided not to due to the inherent risks of the mechanics not working out.
SIMPLICITY = GOOD = TANK DRIVE.  However, it is also true that CHALLENGE = GOOD = HOLONOMIC DRIVE.
Team 321 went for the first of the two equations, which was probably a wise choice.

DESCRIPTION OF A HOLONOMIC DRIVE

This section is for those unfamiliar with the concept of a holonomic drive.

Let me describe the design we were thinking of:
It looked like this: there were four balls, and each ball was connected to two contacts so that each ball had the option of
moving vertically or horizontally or both at the same time. Since this setup required eight motors, we reduced it to four by
linking the horizontal movement of ball 1 and ball 2, the horizontal movement of ball 3 and ball 4, the vertical movement of
ball 1 and ball 4, and the vertical movement of ball 2 and ball 3 (the vertical movements being linked are not shown on diagram
below). Linking those, the robot can still do any combination of translation (motion up, down, left, or right)
and rotation (spinning) you want. See pic.

|||||||||||||||||||||||||||||||
|||                         |||
|||                         ||| <--external frame
|||       BODY OF THE       |||
|||                         |||
|||          ROBOT          |||
|||                         |||
|||                         |||
||| ball # 4                |||
||| ^                       |||
|||||||||||||||||||||||||||||||

We consider motor 1 to be the motor responsible for the horizontal movement of balls 1 and 2, motor 2 to be the motor responsible
for the vertical movement of balls 2 and 3, motor 3 to be the motor responsible for the horizontal movement of balls 3 and 4,
and motor 4 to be responsible for the vertical movement of balls 1 and 4.

If the motor responsible for the horizontal movement of horizontally linked wheels 3 and 4 is at full forward
(assume forward is to the right), and the motor responsible for the horizontal movement of horizontally linked wheels 1 and 2 is
also at full forward, then you get your robot strafing to the right. If both of those are in full reverse, then robot strafes
to the left. If you want to turn, you have the motor responsible for the horizontal motion of ball 1 and ball 2 to go forward
(to the right), the motor responsible for the vertical motion of ball 2 and ball 3 go in reverse (down), the motor responsible
for the horizontal motion of balls 3 and 4 to go in reverse (to the left), and the motor responsible for the vertical motion
of balls 1 and 4 go forward (up).

-----> right
|||||||||||||||||||||||||||||||
up |||                         |||
^  |||                         |||
|  |||       BODY OF THE       ||| |
|  |||                         ||| |
|||         ROBOT           ||| |
|||                         ||| down
|||                         |||
||| ball # 4                |||
||| ^                       |||
|||||||||||||||||||||||||||||||
left <----

Can you see how that makes the robot turn? I know it's confusing. Reread what I said.

In a similar way, a balance between turning and motion can be found.  Obviously, while the robot is turning,
it loses some of its ability to move up, down, right, or left. Why? You can move faster while not spinning than while spinning.

*/

void Default_Routine(void)
{

/*This ball drive phenomenon is controlled by two things:

1) A single joystick's x and y position determine the translational (x and y) motion of the robot.  This joystick
is connected to port 1 normally, so here p1_x and p1_y are of significance to us.

2) The driver's second hand is on a dial.  The dial spins back to neutral when released.  The dial also returns a number
from 0 to 254, 0 being fully turned to the left, 255 being fully turned to the right, and 127 at neutral.  The dial can
very quickly be spun from 127 to 0 or 254, no need to worry about that.  This dial is wired to port 2 such that the 0 to 254
value it returns is stored in p2_x.

Thus:
amount of desired x-motion: p1_x
amount of desired y-motion: p1_y
amount of desired rotational motion: p2_x

p1_x-value less than 127: move to the left desired
p1_x-value greater than 127: move to the right desired
p1_y-value less than 127: move backward desired
p1_y-value greater than 127: move forward desired
p2_x-value less than 127: turn counterclockwise desired
p2_x-value greater than 127: turn clockwise desired

Of course, if the robot is spinning or turning (undergoing rotational motion) it cannot move as quickly.  The goal of this program
is to balance turning and moving as optimally as possible.

One weakness of this program is that at any point, the robot views forward as what is forward for it.  It would be
much simpler to control the robot if forward meant the direction that the user operating the controls is facing.
However, this would be rather difficult.  The robot would have to know its rotation at every given point in time.  This could
be achieved using accelerometers, but the precision of that would likely deteriorate after several minutes of operation
due to the adding up of small errors in the accelerometer measurements.

Also, this code has near-neglegible round-off error.
*/

//declare variables, these are all used later in the code

//these will be used for ranges from -127 to 127
char my_x, my_y, my_r;
int MT_1, MT_2, MT_3, MT_4;
float TP = 1.0, RP = 1.0;
int max;
char M1, M2, M3, M4;

//The first step is: we are receiving 0-254 values.  I want them to be -127 to 127, neutral being zero.
//This is so that future calculations are easier.

//ensure that you never get 128 for the value and then calculate the neutral-at-zero value
if (p1_x == 255) {
my_x = 127;	}
else {
my_x = p1_x-127;	}

if (p1_y == 255) {
my_y = 127;	}
else {
my_y = p1_y-127;	}

if (p2_x == 255) {
my_r = 127;	}
else {
my_r = p2_x-127;	}

/*Now to step 2.  This is to calculate the "motor tentatives".  This is the value at which each motor should move.
Since these values often exceed 127, they are normalized in the next step. */

/*One more note.  Since turning requires the robot to give up some of its translational movement capabilities during that time,
there must be a set of modifiable constants to balance translation and rotation.  The ratio of these constants will determine
how much more (or less) priority is given to translation than to rotation.  We could assume this ratio to be 1:1, and this
is recommended until the robot is tested and it is determined that translation or rotation should have more priority.  However,
since it was important to give the user the ability to modify this priority ratio, the priority constants are in the code and
declared in the beginning of the function.  Only one number is actually needed, since all that matters is the ratio of the
translational (x-y movement) priority constant (called TP) and the rotational movement priority constant (called RP).  Both
constants have been saved for ease nevertheless.  Increasing TP will give more priority to x-y movement and increasing RP will
give more priority to rotation and turning.  Also, do not set the constants too small.  Try not to set either below 1.

VERY IMPORTANT TO NOTE:  The only times it'll actually matter what the priority constants are set to is when you are turning and
moving at the same time and a balance must be achieved between the two.

Also, remember that in the diagram:

|||||||||||||||||||||||||||||||
|||                         |||
|||                         ||| <--external frame
|||       BODY OF THE       |||
|||                         |||
|||          ROBOT          |||
|||                         |||
|||                         |||
||| ball # 4                |||
||| ^                       |||
|||||||||||||||||||||||||||||||

We consider motor 1 to be the motor responsible for the horizontal movement of balls 1 and 2, motor 2 to be the motor responsible
for the vertical movement of balls 2 and 3, motor 3 to be the motor responsible for the horizontal movement of balls 3 and 4,
and motor 4 to be responsible for the vertical movement of balls 1 and 4.

Now, enough said.
*/

MT_1 = (int) (TP*my_x + RP*my_r);
MT_2 = (int) (TP*my_y - RP*my_r);
MT_3 = (int) (TP*my_x - RP*my_r);
MT_4 = (int) (TP*my_y + RP*my_r);

//Step 3. Now to normalize the values - first find the greatest value.  We want to make that value 127 or -127.
//Note that we are on a -127 to 127 scale right now.

//abs (int num) returns the absolute value of num
max = abs (MT_1);
if (abs (MT_2) > max) {
max = abs (MT_2);	}
if (abs (MT_3) > max) {
max = abs (MT_3);	}
if (abs (MT_4) > max) {
max = abs (MT_4);	}

//Now using the max absolute value, normalize.  Make that value -127 or 127.

M1 = (int) (((float) MT_1)/max*127);
M2 = (int) (((float) MT_2)/max*127);
M3 = (int) (((float) MT_3)/max*127);
M4 = (int) (((float) MT_4)/max*127);

//Now, all that's left is to convert -127 to 127 values to 0 to 254 values and put them to PWMs.  I assume motor 1 is pwm01,
//motor 2 is pwm02, and so on.

pwm01 = M1+127;
pwm02 = M2+127;
pwm03 = M3+127;
pwm04 = M4+127;

//LED code can be added here, the old LED outputs don't make much sense here anymore.

} /* END Default_Routine(); */

//returns the absolute value of num
int abs (int num) {
if (num < 0) {
return -1*num;	}
return num;		}
``````

There’s actually not that much code, it’s all comments. Here’s the pure code.

``````void Default_Routine(void)
{

char my_x, my_y, my_r;
int MT_1, MT_2, MT_3, MT_4;
float TP = 1.0, RP = 1.0;
int max;
char M1, M2, M3, M4;

if (p1_x == 255) {
my_x = 127;	}
else {
my_x = p1_x-127;	}

if (p1_y == 255) {
my_y = 127;	}
else {
my_y = p1_y-127;	}

if (p2_x == 255) {
my_r = 127;	}
else {
my_r = p2_x-127;	}

MT_1 = (int) (TP*my_x + RP*my_r);
MT_2 = (int) (TP*my_y - RP*my_r);
MT_3 = (int) (TP*my_x - RP*my_r);
MT_4 = (int) (TP*my_y + RP*my_r);

max = abs (MT_1);
if (abs (MT_2) > max) {
max = abs (MT_2);	}
if (abs (MT_3) > max) {
max = abs (MT_3);	}
if (abs (MT_4) > max) {
max = abs (MT_4);	}

M1 = (int) (((float) MT_1)/max*127);
M2 = (int) (((float) MT_2)/max*127);
M3 = (int) (((float) MT_3)/max*127);
M4 = (int) (((float) MT_4)/max*127);

pwm01 = M1+127;
pwm02 = M2+127;
pwm03 = M3+127;
pwm04 = M4+127;  }

int abs (int num) {
if (num < 0) {
return -1*num;	}
return num;		}
``````

This should be a whitepaper if anyone can understand what I’m talking about in this code.