Offseason Project: Holonomic Kiwi Drive Robot

[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.

[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

Will using the encoders help?

Encoders allow you to turn open-loop voltage commands into closed-loop wheel speed commands. Read this post before proceeding.

If your wheels are turning at the proper speeds the vehicle should go in the desired direction, assuming that the wheels are aligned at the correct angle and the rollers are free-spinning and have little or no axial free play.

[EthanMiller]

Okay, thanks. We're using the FTC stuff - an NXT programmed with RobotC - Do the FPGA issues apply to that, as well?

Okay - I think I may have found the issue. I was watching the variables and noticed at position (127,0) on the joystick, W1 and W2 were being sent 0's. The Joystick was scaled by dividing by 1.28 (Because 128 was the maximum negative value)

The code I used is:

 W1 = (- 1/2 * 99) - (sqrt(3)/2 * 0)

To test if I was off in my order of operations, I parenthesized more fully the code:

 W1 = ((-1 * (1/2)) * 99) - (((sqrt(3))/2) * 0)

And ran those calculations on RobotC - both returned 0. On a TI-89, they returned -49.5, which is what I expected.

I dove a bit deeper - I tried each individual expression.

  float test = -1/2;
  
  writeDebugStreamLine("%d", test);

Returned 0.

Then,

  float test = -.5;
  
  writeDebugStreamLine("%d", test);

Returned -1090519040.

sqrt(3)/2 returned 1063105496 - Any idea what the problem is?

EDIT 2:

Tested the sample math program, everything worked. It printed to the NXT screen, so I told it to print to the debug stream as well - lo and behold, something mucked up the numbers in between. I changed the code on the NXT to be -.5 instead of -1/2 and it works fine now - Why is that?

[Alan Anderson]

Originally Posted by EthanMiller

I changed the code on the NXT to be -.5 instead of -1/2 and it works fine now - Why is that?

1 is an integer. 2 is an integer. When C sees "1/2", it does an integer division and produces an integer result. The integer result of 1/2 is 0.

I assume writeDebugStreamLine() uses the same format strings as the C standard library? %d specifies an int. If you give it a double (a floating point value), the number will be interpreted (and printed) incorrectly. Use %f instead.