Motion Control

[Graphics:HTMLFiles/index_1.gif]

Radius = Linear Velocity/Angular Velocity

Goniometer = AngVel * Timer = (SR - SL)/2

Odometer = LinVel * Timer = (SL + SR)/2<br />

SL = r * Gonio = (LinVel - d/2 * AngVel) * t

SR = (r + d) * Gonio = (LinVel + d/2 * AngVel) * t

SR - SL = d * Gonio = (d * AngVel) * t<br />

Odo = (SL + SR)/2 = (r + d)/2 * Gonio = LinVel * t

Gonio = (SR - SL)/d = AngVel * t

Radius = r + d/2 = LinVel / AngVel<br />

Velocity

Dir = +1 (Left) or - 1 (Right)

AngVel = Dir * DEG2RAD (Degree)

LinVel = Dir * Radius * AngVel<br />

AngVel = Dir * LinVel / Radius

Summary

SL : Left Wheel Distance

SR : Right Wheel Distance

Dir : Direction

d : Wheel_Base

r : Radius of Rotation

R : Radius = r + d/2

LinVel : Linear Velocity

AngVel : Angular Velocity

t : Timer

Gonio : Goniometer = AngVel * t

Odo : Odometer = LinVel * t = (SL + SR)/2

Radius = LinVel/AngVel

r = LinVel/AngVel - d/2

Velocity

SL = (LinVel - d/2 * AngVel) * t = r * Gonio

SR = (LinVel + d/2 * AngVel) * t = (r + d) * Gonio

SR - SL = d * Gonio = (d * AngVel) * t

Odo = (SL + SR)/2 = (r + d)/2 * Gonio = LinVel * t

Gonio = (SR - SL)/d = AngVel * t

Radius = r + d/2 = (LinVel * t)/Gonio = LinVel/AngVel

Dir = +1 (Left) or - 1 (Right) ;

AngVel = Dir * Deg2Rad (Degree) ;

LinVel = Dir * Radius * AngVel ;

AngVel = Dir * LinVel/Radius ; (Not Radians Just LinVel/Radius)

How To

Q = ArcTan[ax - bx, ay - by] ;

R = ((bx - ax)^2 + (by - ay)^2)^(1/2) = (bx - ax) Cos[Q] + (by - ay) Sin[Q] ;

ax = bx + R Cos[Q] ;

ay = by + R Sin[Q] ;

({{ax}, {ay}, {1}}) = ({{1, 0, R Cos[Q]}, {0, 1, R Sin[Q]}, {0, 0, 1}}) · ({{bx}, {by}, {1}})

({{ax}, {ay}, {1}}) = ({{Cos[Q], -Sin[Q], bx}, {Sin[Q], Cos[Q], by}, {0, 0, 1}}) · ({{R}, {0}, {1}})

Inverse[({{Cos[Q], -Sin[Q], bx}, {Sin[Q], Cos[Q], by}, {0, 0, 1}})]

({{R}, {0}, {1}}) = ( {{(ax - bx) Cos[Q] + (ay - by) Sin[Q]}, {(ay - by) Cos[Q] - (ax - bx) Sin[Q]}, {1}} )

R = Cos[Q] * (ax - bx) + Sin[Q] * (ay - by) ;

0 = -Sin[Q] * (ax - bx) + Cos[Q] * (ay - by) ;

Sin[Q] * (ax - bx) = Cos[Q] * (ay - by) ;

Tan[Q] = Sin[Q]/Cos[Q] = (ay - by)/(ax - bx) ;

Q = ArcTan[(ay - by)/(ax - bx)] ;

(x - ax)/(bx - ax) = (y - ay)/(by - ay)

y = (by - ay)/(bx - ax) (x - ax) + ay ;

y = Tan[Q] (x - ax) + ay ;

Det[({{ax, ay, 1}, {bx, by, 1}, {cx, cy, 1}})] = ax * (by - cy) + bx * (cy - ay) + cx * (ay - by)

SignalAng - RobotQ - Turn - Motion - Gonio

[Graphics:HTMLFiles/index_56.gif]

Setting

[Graphics:HTMLFiles/index_57.gif]

MotionDir     MotionQ = SignalAng - RobotQ

Arrive Angle

[Graphics:HTMLFiles/index_61.gif]

MotionDir     Gonio += DiffQ

+(L)                  Gonio>MotionQ

-(R)                  Gonio<MotionQ

                          Abs[Gonio] >360

[Graphics:HTMLFiles/index_66.gif]