Dan,
This is part of a program use to control a mimimover robot witch looks
a lot like the teachmover.
It was written by my father centuries ago ... =)
I think it's written in Modula-2 and contains the kinematic equations
you'll need.
Regards, Jan
DEFINITION MODULE Trans;
(*
CCRC Cartesisian coordinates to Robot coordinates
Cartesian: (millimeters and radians)
x cc[0]
y cc[1]
z cc[2]
pich cc[3]
roll cc[4]
Robot: r1,r2,r3,r4,r5 (radians)
RCCC Robot to Cartesian coordinates
Robot: rc[0] ... rc[4]
Cartesian: x,y,z,p,r
TWRC: steps to robot coordinates
t : steps
rc : robot
RCTW Robot coordinates to steps
t : steps
rc : robot
*)
EXPORT QUALIFIED CCRC,RCCC,TWRC,RCTW,IntToReals;
PROCEDURE CCRC(cc:ARRAY OF REAL;VAR r1,r2,r3,r4,r5:REAL;VAR ok:BOOLEAN);
(*Carthesian to robot coordinates
Deze procedure zet cart. coordinaten om naar robotcoordinaten.
Cart.coord. :x,y en z waarden in mm.
pitch en roll in radialen
x,y,z,p en r resp. cc[0] tot cc[4].
Robotcoord. :Dit zijn de hoeken in radialen van de gewrichten
r1,r2,r3,r4,r5.
ok :Geeft aan of er fouten waren. *)
PROCEDURE RCCC(rc:ARRAY OF REAL;VAR x,y,z,p,r:REAL);
(*Robot coordinates to carthesian coordinates
Robotcoordinaten worden omgezet naar cart. coordinaten.
Robotcoord. :rc[0] tot rc[4].
Cart. coord. :x,y,z,p en r. *)
PROCEDURE TWRC(t:ARRAY OF INTEGER;VAR rc:ARRAY OF REAL);
(*Steps to robotcoordinates (scaling?)
Tellerwaarden die verkregen zijn door de procedue Read kunnen hier
omgezet worden in Robotcoordinaten.
Tellerwaarden :aantal stappen dat elke motor reeds verplaatst is
t[0] tot t[5].
Robotcoord. :rc[0] tot rc[5]
rc[5] = handopening in mm. *)
PROCEDURE RCTW(rc:ARRAY OF REAL;VAR t:ARRAY OF INTEGER);
(* robotcoordinates to steps
De robotcoordinaten worden omgezet in tellerwaarden.
Robotcoord. :rc[0] tot rc[5].
Tellerwaarden :t[0] tot t[5]
*)
PROCEDURE IntToReals(i:INTEGER):REAL;
(* Functie-procedure die een INTEGER i omzet naar een REAL. *)
END Trans.
___
IMPLEMENTATION MODULE Trans;
FROM InOut IMPORT WriteString;
FROM FloatingUtilities IMPORT Round;
FROM MathLib0 IMPORT sin,cos,arctan,sqrt;
PROCEDURE Error(n: CARDINAL);
BEGIN
CASE n OF
1 : WriteString(' Locatie buiten werkgebied van de robot ');|
2 : WriteString(' Te grote rotatie van de pols '); |
3 : WriteString(' Te grote rotatie van de arm ');
ELSE ;
END
END Error;
PROCEDURE Sign (r:REAL):REAL;
VAR Teken : REAL;
BEGIN
IF r=0.0 THEN Teken := 0.0
ELSIF r0.0 THEN Teken := -1.0
ELSE Teken := 1.0
END;
RETURN Teken
END Sign;
PROCEDURE RCCC(rc:ARRAY OF REAL;VAR x,y,z,p,r:REAL);
(* Zet robotcoordinaten om naar cart. coord. *)
CONST el = 177.8;
ll = 89.0;
h = 195.0;
VAR rr : REAL;
BEGIN
r:=((rc[4]-rc[3])/2.0)-rc[0];
p:=((rc[4]+rc[3])/2.0);
rr:=el*cos(rc[1])+el*cos(rc[2])+ll*cos(p);
x:=rr*cos(rc[0]);
y:=rr*sin(rc[0]);
z:=h+el*sin(rc[1])+el*sin(rc[2])+ll*sin(p);
END RCCC;
PROCEDURE CCRC(cc:ARRAY OF REAL;VAR r1,r2,r3,r4,r5:REAL;VAR ok:BOOLEAN);
(* Zet cart. coord. om naar robotcoord. *)
CONST el = 177.8;
pi = 3.141592654;
ll = 89.0;
h = 195.0;
VAR d,g,a,ro,b,rr,zo,v1,v2,v3,v4 : REAL;
notok : BOOLEAN;
BEGIN
ok:=TRUE;
d:=180.0/pi;
v1:=144.0/d;
v2:=127.0/d;
v3:=180.0/d;
v4:=270.0/d;
b:=4.0*el*el;
rr:=sqrt(cc[0]*cc[0]+cc[1]*cc[1]);
IF(cc[0]0.0)OR(rr60.0)
THEN ok:=FALSE;
Error(1)
ELSE IF cc[0]=0.0
THEN r1:=Sign(cc[1])*pi/2.0
ELSE r1:=arctan(cc[1]/cc[0])
END;
r5:=cc[3]+cc[4]+r1;
r4:=cc[3]-cc[4]-r1;
IF(ABS(r4)v4)OR(ABS(r5)v4)
THEN ok:=FALSE;
Error(2)
ELSE ro:=rr-ll*cos(cc[3]);
zo:=cc[2]-ll*sin(cc[3])-h;
IF ro=0.0
THEN g:=Sign(zo)*pi/2.0
ELSE g:=arctan(zo/ro)
END;
a:=ro*ro+zo*zo;
a:=b/a-1.0;
IF (a0.0)OR(ro60.0)
THEN ok:=FALSE;
Error(1)
ELSE a:= arctan(sqrt(a));
r2:=a+g;
r3:=g-a;
notok := (r2v1) OR (r2 -v2) ;
notok := notok OR (r2+ r3 -v3) OR (r2+r3v1);
IF notok THEN
ok:=FALSE;
Error(3)
END
END
END
END
END CCRC;
PROCEDURE TWRC(t:ARRAY OF INTEGER;VAR rc:ARRAY OF REAL);
VAR b:ARRAY[0..5]OF BOOLEAN;
i:INTEGER;
BEGIN
FOR i:=0 TO 5 DO b[i]:=FALSE;
IF t[i]0 THEN b[i]:=TRUE;
t[i]:=-t[i] END;
END;
rc[0]:=-FLOAT(t[0])/1125.0; rc[1]:=-FLOAT(t[1])/1125.0;
rc[2]:= FLOAT(t[2])/672.0; rc[3]:=-FLOAT(t[3])/241.0;
rc[4]:= FLOAT(t[4])/241.0; rc[5]:= FLOAT(t[5])/13.4;
FOR i:=0 TO 5 DO IF b[i] THEN rc[i]:=-rc[i] END END;
rc[1]:=0.47694+rc[1];rc[2]:=-1.6155+rc[2];
rc[3]:=-1.57079+rc[3];rc[4]:=-1.57079+rc[4];
END TWRC;
PROCEDURE RCTW(rc:ARRAY OF REAL;VAR t:ARRAY OF INTEGER);
BEGIN
rc[1]:=rc[1]-0.47694;
rc[2]:=rc[2]+1.6155;
rc[3]:=rc[3]+1.57079;
rc[4]:=rc[4]+1.57079;
t[0]:=-Round(1125.0*rc[0]);
t[1]:=-Round(1125.0*rc[1]);
t[2]:= Round(672.0*rc[2]);
t[3]:=-Round(241.0*rc[3]);
t[4]:= Round(241.0*rc[4]);
t[5]:= Round(13.4*rc[5])
END RCTW;
PROCEDURE IntToReals(i:INTEGER):REAL;
VAR r:REAL;
