Offseason Project: Holonomic Kiwi Drive Robot

[Ether]

Originally Posted by EthanMiller

I do have one question on scaling. If the equations used are (ignoring rotation for now, and with joyX1 being the X axis of the first joystick, and so on)

W1 = joyX1
W2 = (- joyX1 / 2) + sqrt(3) joyY1 / 2
W3 = (- joyX1 / 2) - sqrt(3) joyY1 /2

And the joysticks give a value between -157 and +156, and the motors take a power command of between -100 and 100, I'd divide the results of those equations by 157 and multiply by 100, right?

Then, to add rotation, it would be added to each equation before the scaling, and I'd have to check for out-of-rangeness, and correct it if it was. Would I do that before or after the scaling to correct it? I don't think it would matter, but just in case it does.

Whenever you do inverse kinematics to find wheel speeds from joystick commands, you should always normalize the wheel speeds after they are calculated.

So for your example, what I would recommend is this:

1) divide each of the joystick values by 1.57 before plugging them into the inverse kinematic formulas

1) Look at the wheel speed values W1 W2 W3 calculated using the formulas, and find the one that has the maximum absolute value. Call this answer "max"

2) If "max" is less than or equal to 100, you're done. If "max" is greater than 100, divide each wheel speed by "max".

Then, to add rotation, it would be added to each equation before the scaling, and I'd have to check for out-of-rangeness, and correct it if it was. Would I do that before or after the scaling to correct it? I don't think it would matter, but just in case it does.

It does matter.

The front was planned only until a gyro or compass could acquired.

All that is required to do field-oriented control is to run the "forward" and "strafe right" commands through a Cartesian coordinate rotation before feeding them into the inverse kinematic formulas.

There's a paper titled "1-26-2011 mecanum & omni with gyro for field-centric control" here that shows how to do this.

[EthanMiller]

Okay... bear with me here.

(I made an error in my previous post - the sticks return from -127 to 126, but that won't effect values)

If I divide my joystick values by 157 before throwing them into the equations, wouldn't the equations return a value between -1 and 1?

For example, my math suggests that if the joystick were at (0,157), the value returned would be W2 and W3 running towards each other at .866 speed. I would then multiply that value by 100 to get the motor speed. The 86.6 would then be looked at in the abs val as the max, and left alone, right?

This also suggests to me (I could be wrong) that in that configuration, the robot wouldn't be going to maximum speed it was capable of - Or at least, I would expect that to be with both motors spinning towards each other at 100%. Is there a way to correct for that, or am I just wrong?

And for rotation, that would make my formulas...

W1 = joyX1 + R
W2 = (- joyX1 / 2) + sqrt(3) joyY1 / 2 + R
W3 = (- joyX1 / 2) - sqrt(3) joyY1 /2 + R

where R is rotation, scaled down from the joystick by dividing by the maximum joystick reading?

And then, I'd have to check that none of the equations returned values greater than 100, and if so, make it 100? (100 after being scaled up, 1 before)

[MentorOfSteel]

Nice rebuttal, Ether!

Originally Posted by Ether

However, words enter the English language through repeated usage which facilitates communication, and in the field of robotics the adjective "holonomic" when applied to a robot has come to mean that the robot can translate (X,Y) and rotate (theta) simultaneously and independently.

I grudgingly agree, I am fighting a losing battle, but it is fun to rant about it once in a while.

Originally Posted by Ether

A unicycle has nonholonomic constraints (see Mason, pages 31 & 32), so is it not omnidirectional?

What is your definition of omnidirectional? Do you consider a “crab” drive to be omnidirectional? If not, what technical term do you suggest be used? But if so, what word should then be used for, say, a mecanum, to distinguish its freedom of motion from a crab’s?

I would not classify either a unicycle or a crab drive as omnidirectional. My definition of omnidirectional is that the kinematics allow instantaneous velocity in all directions in the configuration space. Omni drives and mecanum drives satisfy this definition. A crab drive can seemingly move in every direction at once (especially with some of the ridiculously skilled FRC drivers at the helm) but a closer look reveals that it must first take the time to properly orient it's wheels. Viewed instantaneously, a crab drive only has two (or fewer in some wheel configurations) degrees of freedom at any given instant.

I don't have an alternate term to describe the nature of the crab drive, but I'm not sure there needs to be one. Speaking mathematically, I do not see any qualitative difference between a crab drive and unicycle or an Ackerman steered car. All are controllable, kinematic systems with nonholonomic constraints, they just have different degrees of difficulty associated with doing the actual control. We might consider the crab drive to be a member of the class of vehicles with fully articulated wheels, but I don't know of an agreed upon name for that class of vehicles.

Originally Posted by Ether

The above is a good non-technical way to explain it. Some experts, when arguing the finer points, would say however that there are nonholonomic constraints which do result in an algebraic constraint on the position of the robot (Mason, p27)... although Mason goes on to say "the robotics literature often neglects this point, and in fact this book will henceforth also neglect it".

That is a great quote, I love Mason's style. But in that passage he was referring to unilateral constraints, which are different beasts altogether than the bilateral equality constraints we face with wheeled vehicles.

These are all minor details and I admit that they don't matter much from a practical perspective. But I like to think about them and talk about them, and I sincerely appreciate your interest and indulgence.

-George

[Ether]

My definition of omnidirectional is that the kinematics allow instantaneous velocity in all directions in the configuration space. Omni drives and mecanum drives satisfy this definition. A crab drive can seemingly move in every direction at once (especially with some of the ridiculously skilled FRC drivers at the helm) but a closer look reveals that it must first take the time to properly orient it's wheels. Viewed instantaneously, a crab drive only has two (or fewer in some wheel configurations) degrees of freedom at any given instant.

Let's compare a 4-wheel mecanum robot (Robot1) to a robot with 4 independently steerable and independently driven standard wheels (Robot2).

For Robot1, any instantaneous combination of dX/dt plus dY/dt plus dTheta/dt vehicle motions resolves into an instantaneous set of 4 wheel speeds [see reference 1]. As the desired vehicle dX/dt and dY/dt and dTheta/dt changes over time, the wheel speeds must "instantaneously" change to produce the new vehicle motion values. So Robot1 is limited by the dynamic response of the wheel speeds which is limited by the motor power and the vehicle mass, etc.

For Robot2, any instantaneous combination of dX/dt plus dY/dt plus dTheta/dt vehicle motions resolves into an instantaneous set of 4 wheel speeds AND 4 wheel steering angles [see reference 2]. As the desired vehicle dX/dt and dY/dt and dTheta/dt changes over time, the wheel speeds and the wheel steering angles must "instantaneously" change to produce the new vehicle motion values. So Robot2 is limited by the dynamic response of the wheel speeds and the wheel angles which is limited by the motor power and the vehicle mass and the turning friction, etc.

So the only difference is dynamic response. Within its dynamic capabilities, Robot2 can do anything (that is, mimic any motion, however complicated) that Robot1 can do. In that sense, from a kinematic standpoint they are equivalent.


[1] http://www.chiefdelphi.com/media/papers/download/2722

[2] http://www.chiefdelphi.com/media/papers/download/3027

[buchanan]

This would be a great off season project. In addition to the theory, which is indeed interesting, there are several practical engineering aspects of such machines you won't really appreciate until you try building one. 2077 built a robot almost exactly like you're describing last year. Some interesting things we learned...

Beyond the discussed-ad-nauseum limits on "pushing power" due to traction, there are a new set of speed and acceleration limits. Theory, of course, predicts and explains these things, but their significance may not jump out at you from the equations:

• Your total available motor power is only 75% of that of a four-motor machine, for obvious reasons.
• The arrangement makes some motor power available for acceleration in any direction at any time, but it also never makes ALL motor capacity available in any one direction.
• Since you're always moving two or three wheels "sideways" to at least a degree, some of your already-limited motor power is consumed by the friction in the omni-wheel rollers and against the ground. With the small roller diameter the latter can be significant on carpet.

Taken together, these mean you won't be winning any drag races with well-designed four motor tank drives. They can pretty much fully utilize the full power of four motors going forward or back; the three-wheel omni ends up applying closer to one CIM's worth of output to actually accelerating the mass of the machine. Don't let this scare you (too much); few robots you compete against will be optimally engineered anyway. It does mean you have little room to be wasteful in your design.

It will be surprisingly difficult to make the machine drive in a straight line, especially in directions other than the three where two wheels are turning the same speed and the third is stopped. The problem is that the beautiful theoretical math only works as expected with beautiful theoretical hardware. In particular, the math solves for wheel speeds, and unless you have a good wheel/motor regulation arrangement, what you're actually setting is motor drive (e.g. PWM) level. How this translates to wheel speed depends on the voltage/speed linearity of the motor and even more on the load. While this problem in principle affects other drive systems, it's worse on 3-wheel omni because you're almost always at different speed/load points for each wheel. We dealt with it using gyro feedback. Another approach would be closed-loop PID on each wheel so you really do get the wheel speeds your program asks for.

Another amusing thing we observed was behavior under hard acceleration. The fact that the wheels tend to be driving all different speeds means typically one will break loose and start spinning before the two. For something like a car where all the pushing wheels are going the same way, one wheel spinning means maybe a moderate swerve to one side. For a 3-wheel omni with the wheels 120 degrees apart, the behavior is more "interesting". We saw this on hard surfaces and were a bit baffled by it (blaming it on the software) until we figured out what was happening. It was not a problem on carpet.

Whether or not you choose to use such a system in competition, knowledge of how to make one will be an asset to your team. Your software people will enjoy it too. There are already written drive programs available, but they're not too hard to program from scratch (another thing I'd recommend doing as a learning experience). Have fun.

[MentorOfSteel]

Originally Posted by Ether

As the desired vehicle dX/dt and dY/dt and dTheta/dt changes over time, the wheel speeds and the wheel steering angles must "instantaneously" change to produce the new vehicle motion values.

Yes, if you allow discontinuous steering angles the associated infinite steering velocities then the crab drive is omnidirectional. But this may be where we reach an amiable impasse, because I do not allow infinite velocities in my kinematic analysis. If infinite velocities are allowed, then any controllable kinematic system can be made omnidirectional through the use of infinitely fast, infinitesimally small Lie bracket maneuvers.

Now, once dynamics are taken into consideration, then I admit that the crab drive becomes practically indistinguishable from (what I would call) true omnidirectional systems. With dynamic constraints in effect, the laws of nature dictate that trajectory must be smooth in the sense that it has continuous velocities. As a result, any dynamically feasible vehicle trajectory would not require discontinuous steering angles and the crab can do anything the mecanum can do. In fact, the crab can probably do more because it can generate larger reactions forces against the floor and thus achieve higher accelerations.

Thanks, by the way, for pointing out your papers on the subject. They will come in handy if I ever convince my students to attempt and omnidirectional (or pseudo-omnidirectional ) chassis.

-George

[Ether]

Originally Posted by MentorOfSteel

if you allow discontinuous steering angles the associated infinite steering velocities then the crab drive is omnidirectional. But this may be where we reach an amiable impasse, because I do not allow infinite velocities in my kinematic analysis.

The steerable-wheel vehicle requires discontinuous steering angles only if you allow discontinuous vehicle commands.

But if you allow discontinuous vehicle commands, then the mecanum requires infinite wheel speed accelerations (instantaneous changes in wheel speed). So I don't see a bright-line difference between the two; they both sink or swim together. Hey, we can agree to disagree; vive la difference :-)

Thanks, by the way, for pointing out your papers on the subject. They will come in handy if I ever convince my students to attempt and omnidirectional (or pseudo-omnidirectional ) chassis.

If your students are showing an interest in swerve drive, they might like to see a simple demo spreadsheet I put together which graphically shows the speed and steering angle of each of the 4 wheels for any given set of Xdot, Ydot, THETAdot vehicle commands. Look for the file "2011-05-07 swerve calculator" at this link.

[EthanMiller]

Originally Posted by Ether

But what if you give joystick commands Xj=6, Yj=-6. You would be commanding the vehicle to go 6*sqrt(2) = 8.5 feet per second diagonally. What would happen?

Plugging in Xj=6 and Yj=-6:

W1 = 6

W2 = -6/2 +0.866*(-6) = -8.2

W3 = -6/2 -0.866*(-6) = 2.2

Notice that the maximum absolute value exceeds 6. Multiplying all 3 wheel speeds by 6/(8.2) gives:

W1 = 4.4

W2 = -6

W3 = 1.6

... and the normalization gives wheel speeds pretty close to the previous example

Okay, I think I follow. I'll test it Tuesday after I'm done with my AP tests.

Originally Posted by buchanan

This would be a great off season project. In addition to the theory, which is indeed interesting, there are several practical engineering aspects of such machines you won't really appreciate until you try building one. 2077 built a robot almost exactly like you're describing last year. Some interesting things we learned...

Beyond the discussed-ad-nauseum limits on "pushing power" due to traction, there are a new set of speed and acceleration limits. Theory, of course, predicts and explains these things, but their significance may not jump out at you from the equations:

• Your total available motor power is only 75% of that of a four-motor machine, for obvious reasons.
• The arrangement makes some motor power available for acceleration in any direction at any time, but it also never makes ALL motor capacity available in any one direction.
• Since you're always moving two or three wheels "sideways" to at least a degree, some of your already-limited motor power is consumed by the friction in the omni-wheel rollers and against the ground. With the small roller diameter the latter can be significant on carpet.

Taken together, these mean you won't be winning any drag races with well-designed four motor tank drives. They can pretty much fully utilize the full power of four motors going forward or back; the three-wheel omni ends up applying closer to one CIM's worth of output to actually accelerating the mass of the machine. Don't let this scare you (too much); few robots you compete against will be optimally engineered anyway. It does mean you have little room to be wasteful in your design.

That's all right - the goal is more to demonstrate omnidirectional movement than to race, and it should all be on a flattish surface. The reasoning for the three wheels instead of four actually comes from a different thing - we only had three unused, good motors. The conclusion of three wheeled robot followed pretty easily.

Originally Posted by buchanan

It will be surprisingly difficult to make the machine drive in a straight line, especially in directions other than the three where two wheels are turning the same speed and the third is stopped. The problem is that the beautiful theoretical math only works as expected with beautiful theoretical hardware. In particular, the math solves for wheel speeds, and unless you have a good wheel/motor regulation arrangement, what you're actually setting is motor drive (e.g. PWM) level. How this translates to wheel speed depends on the voltage/speed linearity of the motor and even more on the load. While this problem in principle affects other drive systems, it's worse on 3-wheel omni because you're almost always at different speed/load points for each wheel. We dealt with it using gyro feedback. Another approach would be closed-loop PID on each wheel so you really do get the wheel speeds your program asks for.

We've got encoders on each of our wheels. It'll be something I'll be learning as I go. In the meantime, a driver will be controlling it - so, they'll close the loop. But yeah, PID is something I hope to learn during this. The integral and differential bits could be more difficult having not taken any calculus yet, but I don't know enough about it to really know.

Originally Posted by buchanan

Another amusing thing we observed was behavior under hard acceleration. The fact that the wheels tend to be driving all different speeds means typically one will break loose and start spinning before the two. For something like a car where all the pushing wheels are going the same way, one wheel spinning means maybe a moderate swerve to one side. For a 3-wheel omni with the wheels 120 degrees apart, the behavior is more "interesting". We saw this on hard surfaces and were a bit baffled by it (blaming it on the software) until we figured out what was happening. It was not a problem on carpet.

Look forward to seeing that - thanks for the warning.

Originally Posted by buchanan

Whether or not you choose to use such a system in competition, knowledge of how to make one will be an asset to your team. Your software people will enjoy it too. There are already written drive programs available, but they're not too hard to program from scratch (another thing I'd recommend doing as a learning experience). Have fun.

Probably will stick to a four wheeled system (or treads, we had great success with those this year) for competition, but we may go the omnidirectional drive route, and yeah, this'll be good practice. And yeah, we're doing all the programming ourselves. The plan is actually to use it and our other bot to teach two new members programming - so three of us will be programming it from scratch, which will be interesting. I'll help them, of course. Should be fun.

[buchanan]

... we only had three unused, good motors.The conclusion of three wheeled robot followed pretty easily.

Certainly a good reason; respecting real-world constraints is engineering too . Another reason to start w/ three wheels is that this sort of drive requires traction, and hence normal force (weight) on all driven wheels at all times. With three wheels you get that pretty much for free if you have the COG in the middle. There are ways to deal with it with four or more wheels, but for learning the technology reducing the number of design factors to be managed makes it easier to get started. Besides, three wheelers just look cool. One idea we toyed with last year was using five wheels (maximum legal CIMs) just to get more power on the ground. We decided not to for the reason I just mentioned, but if a three wheeler attracts attention, imagine the looks you'd get with five .

... PID is something I hope to learn during this. The integral and differential bits could be more difficult having not taken any calculus yet, but I don't know enough about it to really know.

CAN/PID (closed-loop, encoders directly to Jaguars) was our big learning push this year. We found it not so much a matter of learning math, but of physically configuring the system. It seems quite fussy, and if everything isn't right nothing works - we had wheels turning backwards and other strange phenomena. There are how-tos on tuning the PID parameters. Once we trusted our hardware we just followed one of those and got good results. IMO closed-loop PID and holonomic drive (we used mecanum this year) is a marriage made in heaven.

[EthanMiller]

Okay, our RobotC license expired, so I'm using LabVIEW. I haven't used LabVIEW in a few years, and never with an NXT, so I'm struggling a little bit. I figured out what I hoped would be right, and it compiles just fine and runs on the NXT until the Enabled signal is sent, at which point the NXT crashes and throws a "file error."

I'm using the LabVIEW firmware, and I think I set everything up right. The VI is attached, both as a pic and the file. Hopefully the new RobotC license will be acquired soon; I feel much more comfortable in that.

[EthanMiller]

Okay, got RobotC back and working. Thanks for all the help so far - it's really appreciated. So far, we've got translation and rotation working mostly... It's that mostly I'm worried about.

When I try to have the robot go sideways (Namely, JoyX=-128, JoyY=0), it sort of drives along an arc with a central point about 10 feet in front of the robot, as if the back wheel were going too fast. I can post code tomorrow; I don't have it with me now. I tried scaling the back wheel, but that didn't help.

This is without encoder feedback - once I get that figured out, I might use the constant speed feature to do the math into encoder ticks to do the trick - That would use PID, right?

[Ether]

Originally Posted by EthanMiller

When I try to have the robot go sideways (Namely, JoyX=-128, JoyY=0), it sort of drives along an arc with a central point about 10 feet in front of the robot, as if the back wheel were going too fast.

Buchanan addressed this in an earlier post in this thread:

Originally Posted by buchanan

It will be surprisingly difficult to make the machine drive in a straight line, especially in directions other than the three where two wheels are turning the same speed and the third is stopped. The problem is that the beautiful theoretical math only works as expected with beautiful theoretical hardware. In particular, the math solves for wheel speeds, and unless you have a good wheel/motor regulation arrangement, what you're actually setting is motor drive (e.g. PWM) level. How this translates to wheel speed depends on the voltage/speed linearity of the motor and even more on the load. While this problem in principle affects other drive systems, it's worse on 3-wheel omni because you're almost always at different speed/load points for each wheel. We dealt with it using gyro feedback. Another approach would be closed-loop PID on each wheel so you really do get the wheel speeds your program asks for.

[EthanMiller]

Originally Posted by Ether

Buchanan addressed this in an earlier post in this thread:

Yeah, I saw that. I was more wondering if there was an obvious fix I was missing. Will using the encoders help?

[Ether]

Originally Posted by EthanMiller

Okay... bear with me here.

Not a problem.

The 157 was a typo. It should have been 1.57


As for your other concerns, I should have explained more fully.

The maximum speed of a Kiwi drive is different in different directions. If the max speed that a Kiwi can drive in the sideways direction is, say, 6 feet per second, then the max speed in the forward direction will be (6)(2/sqrt(3)) = 6.93 feet per second... 15% faster.

When doing an inverse kinematic calculation to obtain wheel speeds, the inputs to the formulas (the joystick values) represent vehicle speed commands. The inverse kinematic formulas convert those into the individual wheel speeds necessary to achieve the desired overall vehicle speed.

The formulas I gave you were scaled to present the driver with a uniform vehicle speed response in all directions.

So for example, suppose your Xj and Yj joystick values were first scaled to a range of -6 to +6 (representing +/-6 feet per second vehicle speed) before feeding them to the inverse kinematic formulas.


To command the vehicle to go forward at 6 feet per second, you would issue joystick commands Xj=0 and Yj=-6.

Calculating the wheel speeds for Xj=0 and Yj=-6, you would get:

W1 = 0

W2 = -6*0.866 = -5.2 feet per second tangential wheel velocity

W3 = -6*(-0.866) = +5.2 feet per second tangential wheel velocity

... which would cause the vehicle to go forward at 6 feet per second, as commanded.


Now look at what happens if you command the vehicle to go sideways to the right at 6 feet per second. To do this, you would have Xj=6, Yj=0.

Putting Xj=6 and Yj=0 into the inverse kinematic formulas would give:

W1 = 6

W2 = -3

W3 = -3

... which would cause the vehicle to go straight to the right at 6 feet per second, as commanded.


Suppose you wanted to command the vehicle to go 6 feet per second diagonally. You would set Xj=6/sqrt(2) and Yj =-6/sqrt(2), so that sqrt(Xj^2 + Yj^2) would be 6:


W1 = 6/sqrt(2) = 4.24

W2 = -(6/sqrt(2))/2 +0.866*(-6/sqrt(2)) = -5.8

W3 = -(6/sqrt(2))/2 -0.866*(-6/sqrt(2)) = 1.55

... which would cause the vehicle to go 6 feet per second diagonally


But what if you give joystick commands Xj=6, Yj=-6. You would be commanding the vehicle to go 6*sqrt(2) = 8.5 feet per second diagonally. What would happen?

Plugging in Xj=6 and Yj=-6:

W1 = 6

W2 = -6/2 +0.866*(-6) = -8.2

W3 = -6/2 -0.866*(-6) = 2.2

Notice that the maximum absolute value exceeds 6. Multiplying all 3 wheel speeds by 6/(8.2) gives:

W1 = 4.4

W2 = -6

W3 = 1.6

... and the normalization gives wheel speeds pretty close to the previous example

[WizenedEE]

#include <AFMotor.h>
#include <math.h>

AF_DCMotor motor1 (1, MOTOR12_1KHZ);
AF_DCMotor motor3 (3, MOTOR12_1KHZ);
AF_DCMotor motor4 (4, MOTOR12_1KHZ);

long cosOneTwenty = -500;
long sinOneTwenty = 866;
long sinTwoForty = -866;
long cosTwoForty = -500;

struct motorspeeds {long speed4; long speed3; long speed1;};
struct axis {int x; int y;};
struct moveRobotOutputs{
  struct motorspeeds speeds;
  struct axis axis;
};

int runMtr(AF_DCMotor mtr, long spd) { //given a motor name and a speed -1,000,000 to 1,000,000, runs the motor at the speed
  uint8_t drctn;
  if      (spd < -1000000){spd = -1000000;}
  else if (spd >  1000000){spd =  1000000;}
  if (spd < 0) {drctn = BACKWARD;}
  else {drctn = FORWARD;}
  int fspd = fabs(spd) * .00018F + 75; //fspd is between 0 and 255
  if (fspd <= 75) {fspd = 0;}
  mtr.setSpeed(fspd); //set the speed
  mtr.run(drctn); //run the motor
  return fspd;
}

struct moveRobotOutputs moveRobot(int x, int y, float gyro) { //taking x, y, (-1000 to 1000), and gyro (in milliradians, 0 to 360,000) values, runs the motors at speeds to move robot in correct direction
  struct motorspeeds speeds;
  struct axis axis;
  struct moveRobotOutputs Outputs;
  gyro /= 1000;
  float cosgyro = cos(gyro);
  float singyro = sin (gyro);
  long xp = x * cosgyro - y * singyro; // rotating vectors
  long yp = x * singyro + y * cosgyro; // with respect to gyro angle
  long speed4 = sinTwoForty * yp + cosTwoForty * xp;
  long speed3 = sinOneTwenty * yp + cosOneTwenty * xp;
  long speed1 = xp * 1000; //normally, it would just be xp, but the trig values are all fractions of 1000
  axis.x = xp;
  axis.y = yp;
  
  long highSpeed = fabs(fmax(fmax(fabs(speed1),fabs(speed3)),fabs(speed4)));

  float speedDivisor = float(fmax(1000000, highSpeed))/1000000;
  
  speed4 /= speedDivisor;
//  speed4 *= 1000000;
  speed3 /= speedDivisor;
//  speed3 *= 1000000;
  speed1 /= speedDivisor;
//  speed1 *= 1000000;
  
  speeds.speed4 = runMtr(motor4, speed4);
  speeds.speed3 = runMtr(motor3, -speed3);
  speeds.speed1 = runMtr(motor1, speed1);
//  speeds.speed4 = speed4;
//  speeds.speed3 = speed3;
//  speeds.speed1 = speed1;
  Outputs.speeds = speeds;
  Outputs.axis = axis;
  return Outputs;
}

This was written for Arduino, using a motor shield (the one from adafruit if you're into that). The interesting part is probably the function "moveRobotOutputs" which takes values -1000 to 1000 for joysticks and 0 to 360,000 for gyroscope, and spits out a value from -1,000,000 to 1,000,000 for each motor. I tried to make it efficient to run without much floating point. It's written in some sort of pseudo C.