Re: Holonomic Control System
Quote:
Originally Posted by SudoSammich
(Post 1001009)
I didn't set out to re-invent the wheel, but it came to that after a small series of events. Basically, our cRio is stuck on last year's bot, so we're using the '07 robot controller and player station mounted to a piece of plywood. As such, I'm writing in C, not LabVIEW (our normal programming language at this point is Java anyway, but I assume there's a class for that as well). And I'm sure it's capable of two dimensional movement while spinning, I just haven't put forth the energy into performing those calculations yet. Good to know that there's a class waiting for me when we transition to this year's real bot though.
|
Hey, if you'd mentioned that earlier, I would have posted this for you:
Code:
// 3-axis joystick interface to an omni drive
// with robot-centric driver interface
// first define your driver interface,
// in this case a 3-axis joystick:
forward = -Y; // push joystick forward to go forward
right = X; // push joystick to the right to strafe right
clockwise = Z; // twist joystick clockwise turn clockwise
// here is where you would put any special shaping of the joystick
// response curve, such as deadband or gain adjustment
// now apply the inverse kinematic tranformation
// to convert your vehicle motion command
// to 4 wheel speed commands:
front_left = forward + clockwise + right;
front_right = forward - clockwise - right;
rear_left = forward + clockwise - right;
rear_right = forward - clockwise + right;
// finally, normalize the wheel speed commands
// so that no wheel speed command exceeds magnitude of 1:
max = abs(front_left);
if (abs(front_right)>max) max = abs(front_right);
if (abs(rear_left)>max) max=abs(rear_left);
if (abs(rear_right)>max) max=abs(rear_right);
if (max>1)
{front_left/=max; front_right/=max; rear_left/=max; rear_right/=max;}
// you're done. send these four wheel commands to their respective wheels
... it's C code for fully holonomic operation of your omni bot.
If you want to add the gyro angle for field-centric control, it only adds a small amount of complexity:
Code:
// 3-axis joystick interface to an omni drive
// with gyro (field-centric driver interface)
// first define your driver interface,
// in this case a 3-axis joystick:
forward = -Y; // push joystick forward to go forward
right = X; // push joystick to the right to strafe right
clockwise = Z; // twist joystick clockwise turn clockwise
// here is where you would put any special shaping of the joystick
// response curve, such as deadband or gain adjustment
// use the gyro angle for field-centric control
// "theta" is the gyro angle, measured CCW from the zero reference
forward' = forward*cos(theta) - right*sin(theta);
right' = forward*sin(theta) + right*cos(theta);
// now apply the inverse kinematic tranformation
// to convert your vehicle motion command
// to 4 wheel speed commands:
front_left = forward' + clockwise + right';
front_right = forward' - clockwise - right';
rear_left = forward' + clockwise - right';
rear_right = forward' - clockwise + right';
// finally, normalize the wheel speed commands
// so that no wheel speed command exceeds magnitude of 1:
max = abs(front_left);
if (abs(front_right)>max) max = abs(front_right);
if (abs(rear_left)>max) max=abs(rear_left);
if (abs(rear_right)>max) max=abs(rear_right);
if (max>1)
{front_left/=max; front_right/=max; rear_left/=max; rear_right/=max;}
// you're done. send these four wheel commands to their respective wheels
|
