View Single Post
  #19   Spotlight this post!  
Unread 22-03-2005, 09:18
seanwitte seanwitte is offline
Registered User
None #0116
Team Role: Engineer
 
Join Date: Nov 2002
Location: Herndon, VA
Posts: 378
seanwitte has a brilliant futureseanwitte has a brilliant futureseanwitte has a brilliant futureseanwitte has a brilliant futureseanwitte has a brilliant futureseanwitte has a brilliant futureseanwitte has a brilliant futureseanwitte has a brilliant futureseanwitte has a brilliant futureseanwitte has a brilliant futureseanwitte has a brilliant future
Send a message via AIM to seanwitte
Re: Holomonic Drive mathmatics discussion

This is based on information in the article I posted in a previous thread.

If you have four omni wheels at right-angles to one another, the following psuedo code will give you basic control of the robot. If you fix Phi, the robot's heading, at 0 then the movement will be relative to the robot. If you use a gyro and integrate the heading then the movement will be relative to the driver. Assume the wheels are labeled NORTH, EAST, SOUTH, WEST in the clockwise direction looking down on the robot. The X axis is perpendicular to the wheel labeled EAST and the Y axis is perpendicular to the wheel labeled NORTH. That means the Y axis is parallel to the axel for the NORTH/SOUTH wheels and the X axis is parallel to the axel for the EAST/WEST wheels. This is only a simplified version of the algorithm to get you started, you will have to work at it to get to to run really smoothly. Using feedback from encoders on the wheels or a gyro is almost a necessity.

A very simple way to apply the gyro feedback to remove unwanted rotation would be to compare the current gyro value to the commanded rotational velocity and add the difference to r. Not having tried this I don't know how well it will work. You can divide the error by 2 or 3 to damp is slightly without adding the differential and integral terms.

sint16 g = (sint16)((gyro >> 2) - 127);
sint16 error = r - g;
r += error;

(Ex. r = 0, g = 10. This means you want to go straight but are rotating slightly clockwise. error = (0 - 10) = -10; r += error = 0 - 10 = -10; You will be increasing the counter-clockwise rotation because you want to go straight but are rotating slightly in the clockwise direction. This is a proportional-only feedback loop with a gain of 1)

Assumptions:

1. Heading is in integer degrees in the range [0,359]
2. You have a math library that can calculate sine and cosine (I can give you one if you need it, using fixed-point decimals with 10 bits of precision).
3. You have a type library that defines sint16 as a signed int and sint32 as a signed long.

Inputs:

Vx - X axis component of the desired velocity, range [0,254]
Vy - Y axis component of the desired velocity, range [0,254]
Vr - Desired rotational velocity
Phi - Current heading

Outputs:

PWM_North - PWM value applied to the wheel labeled NORTH
PWM_South - PWM value applied to the wheel labeled SOUTH
PWM_East - PWM value applied to the wheel labeled EAST
PWM_West - PWM value applied to the wheel labeled WEST

Algorithm:

// convert inputs to signed integers
sint32 x = (sint32)pc_p1_x;
sint32 y = (sint32)pc_p1_y;
sint16 r = (sint16)pc_p2_x;
sint16 phi = robot.heading; // set phi to 0 to use robot's reference frame

// declare local working variables
sint32 sinPhi = sind(phi);
sint32 cosPhi = cosd(phi);
sint32 x2;
sint32 y2;
sint16 vNorth = 127;
sint16 vSouth = 127;
sint16 vEast = 127;
sint16 vWest = 127;

// transform velocity inputs to range [-127,127]
x -= 127;
y -= 127;
r -= 127;

// apply rotational transform to x and y velocity
x2 = (cosPhi * x) + (sinPhi * y);
y2 = -(sinPhi * x) + (cosPhi * y);

// drop the decimal from the new x and y components
x2 = ftoi(x2);
y2 = ftoi(y2);

// calculate wheel velocities
vNorth = x2 - r;
vSouth = -x2 - r;
vEast = -y2 - r ;
vWest = y2 - r;

// adjust values to match valid PWM range
pwm01 = AdjustPwm(vNorth);
pwm02 = AdjustPwm(vSouth);
pwm03 = AdjustPwm(vEast);
pwm04 = AdjustPwm(vWest);

END

// simple helper function to keep a signed PWM value inside
// the valid range

uchar AdjustPwm(sint32 val)
{
if (val > 127)
return 254;
else if (val < -127)
return 0;
else
return (uchar)(val + 127);
}

Last edited by seanwitte : 23-03-2005 at 08:11.