|
program tuker;
uses crt, graph;
{Caracteristicas de cada motor}
type pasoapaso = record
motor : shortint;
cod : shortint;
pasoactual : shortint;
numerosec : shortint;
app : real;
anguloactual : real;
end;
type vector = array[1..5, 1..2] of integer;
type longs = array[1..4] of real;
type archivo = FILE of longs;
var
option : char; {Opcion del menu principal}
matriz : array[1..4, 1..4] of integer; {Para el puerto}
steper : array[1..4] of pasoapaso; {Los cuatro motores}
puntos : vector; {Para el dibujo del brazo}
i : shortint; {Para los FOR}
{En estas variables se guardan las coordenadas, los angulos}
{y el largo de los segmentos}
x,y,z,f,xn,yn,zn,fn,fa,f1,f23,f34,l1,l2,l3,l4,l1n,l2n,l3n,l4n : real;
data,status,control : word; {Las direccion de los registros del puerto}
Gd, Gm : integer; {Nesesarias para el modo grafico}
escaladibujo1 : real;
escaladibujo2 : real;
{Variables relacioneadas con el archivo de configuracion}
largos : longs;
archlargos : archivo;
{Colores correxpondientes a cada motor-segmanto}
colores : array[1..4] of integer;
{Valores de referencia para el dibujo del brazo}
const
ref1_x = 80;
ref1_y = 370;
ref2_x = 409;
ref2_y = 270;
{Carga los los ultimos valores de los largos o si no existe el archivo
los valores predefinidos}
procedure loadlongs;
begin
assign(archlargos, 'largos.dta');
{$i-}
reset(archlargos);
{$i+}
if (IOResult <> 0) then
begin
rewrite(archlargos);
largos[1]:=8 ; largos[2]:=12.1 ; largos[3]:=5.8 ; largos[4]:=2.9;
write(archlargos,largos);
reset(archlargos);
end;
read(archlargos,largos);
l1:=largos[1] ; l2:=largos[2] ; l3:=largos[3] ; l4:= largos[4];
close(archlargos);
end;
procedure savelongs;
begin
assign(archlargos, 'largos.dta');
{$i-}
reset(archlargos);
{$i+}
if (IOResult <> 0) then
begin
rewrite(archlargos);
largos[1]:=8 ; largos[2]:=12.1 ; largos[3]:=5.8 ; largos[4]:=2.9;
write(archlargos,largos);
reset(archlargos);
end;
largos[1]:=l1 ; largos[2]:=l2 ; largos[3]:=l3 ; largos[4]:=l4;
write(archlargos, largos);
close(archlargos);
end;
{ Dibuja la pantalla con los valores actualizados }
procedure generarpantalla;
type data = record
motor,angulo,paso,secuencia : string; {Modo grafico solo texto}
end;
var
{motor, angulo : string;}
cuadro : array[1..4] of integer;
datos : array[1..4] of data;
xa,ya,za,fa : string;
l1a,l2a,l3a,l4a : string;
direcc : string;
begin
setbkcolor(15); {Color blanco de fondo}
setcolor(9); {Cambio a color azul}
ClearDevice; {Borra la pntalla}
{Transformo las Variables reales a string para poder imprimirlas}
str(x:2:2, xa); str(y:2:2, ya); str(z:2:2, za); str(f:3:2, fa);
{Dibujo las separaciones de la pantalla}
Rectangle(0, 0, GetMaxX, GetMaxY);
line(0,75,GetMaxX,75);
line((GetMaxX - 90),75,(GetMaxX - 90),GetMaxY);
line(159,0,159,75);
line(318,0,318,75);
line(477,0,477,75);
line(0,97,GetMaxX-90,97);
line(0,115,GetMaxX-90,115);
rectangle(5,120,270,420);
rectangle(274,120,544,420);
rectangle(5,425,544,445);
line(0,450,549,450);
cuadro[1] := 7; cuadro[2] := 166; cuadro[3] := 325; cuadro[4] := 484;
{Imprimo los datos de los motores}
for i := 1 to 4 do
begin
str(steper[i].motor, datos[i].motor);
str(steper[i].anguloactual:3:2, datos[i].angulo);
str(steper[i].pasoactual, datos[i].paso);
str(steper[i].numerosec, datos[i].secuencia);
setcolor(colores[i]);
moveto(cuadro[i],8); outtext('motor: '); setcolor(1);outtext(datos[i].motor);
setcolor(9);
moveto(cuadro[i],23); outtext('angulo: ');
setcolor(1);outtext(datos[i].angulo); outtext(' ø');
setcolor(9);
moveto(cuadro[i],38); outtext('paso: '); setcolor(1);outtext(datos[i].paso);
setcolor(9);
moveto(cuadro[i],53); outtext('secuencia: ');
setcolor(1);outtext(datos[i].secuencia);
setcolor(9);
end;
{Imprimo las coordenadas actuales}
moveto(559,85); outtext('Actual:');
moveto(553,105);outtext('x:'); setcolor(1); outtext(xa);
setcolor(9);
moveto(553,117);outtext('y:'); setcolor(1); outtext(ya);
setcolor(9);
moveto(553,129);outtext('z:'); setcolor(1); outtext(za);
setcolor(9);
moveto(553,141);outtext('f:'); setcolor(1); outtext(fa);outtext('ø');
setcolor(9);
{Las longitudes de los segmentos del brazo}
SetTextStyle(2, HorizDir, 4);
str(l1:2:2,l1a); str(l2:2:2,l2a); str(l3:2:2,l3a); str(l4:2:2,l4a);
moveto(10,430);
setcolor(colores[1]); outtext('base-hombro(l1):'); outtext(l1a);
setcolor(colores[2]); outtext(' hombro-codo(l2):'); outtext(l2a);
setcolor(colores[3]); outtext(' codo-mu¤eca(l3):'); outtext(l3a);
setcolor(colores[4]); outtext(' mu¤eca-mano(l4):'); outtext(l4a);
setcolor(9);
SetTextStyle(0{DefaultFont}, HorizDir, 1);
{Eje de coordenadas}
line(25,400,110,400); outtextxy(115,390,'(y)');
line(25,400,25,315); outtextxy(30,305,'(z)');
line(289,400,374,400); outtextxy(379,390,'(x)');
line(289,400,289,315); outtextxy(294,305,'(y)');
end;
{Procedimiento que calcula los angulos de los segmentos}
procedure calculo(var xn,yn,zn,fn : real; var fa,f1,f23,f34 : real);
var
r1,r2,r3,r4,r5,r6,zmuneca,a,a1,ycodo,zcodo: real;
f2,f3,f4 : real;
m : char;
ok : boolean;
xt,yt,zt,ft : string;
pr1,pr2,pr3,pr4: string;
begin
gotoxy(1,1);
ok := true;
zmuneca := zn - l1 + (sin((fn*pi)/180)*l4);
if ((zmuneca > 0) and (zmuneca < 0.0001)) {or ((zmuneca < 0) and (zmuneca >
-0.0001))}
or (zmuneca = 0) then zmuneca := 0.0001;
if ((zmuneca < 0) and (zmuneca > -0.0001)) then zmuneca := -0.0001;
a1 := cos((fn*pi)/180)*l4;
a := (sqrt((xn*xn)+(yn*yn))) - a1;
{Calculos auxiliares}
r1 := (l3*l3)-(l2*l2)-(a*a)-(zmuneca*zmuneca);
r2 := 2*a;
r3 := 2*zmuneca;
r4 := 1+((r2/r3)*(r2/r3));
r5 := (2*r1*r2)/(r3*r3);
r6 := ((r1/r3)*(r1/r3))-(l2*l2);
if ((r5*r5) < (4*r4*r6)) then
begin
moveto(554,355); outtext('Valores ');
moveto(554,365); outtext('invalidos');
readkey;
exit;
end;
x := xn; y := yn; z := zn; f := fn;
{Coordenadas de interseccion entre brazo y antebrazo}
if (zmuneca > 0) then
begin
ycodo := (-r5 - (sqrt((r5*r5)-(4*r4*r6)))) / (2*r4);
zcodo := (r1+(2*a*ycodo))/(-(2*zmuneca));
end;
if (zmuneca < 0) then
begin
ycodo := (-r5 + (sqrt((r5*r5)-(4*r4*r6)))) / (2*r4);
zcodo := (r1+(2*a*ycodo))/(-(2*zmuneca));
end;
{Los angulos: }
if x <> 0 then
{Angulo del motor 1 (base) (fa)}
begin
if (x > 0) and (y > 0) then
fa := ((arctan(x/y)/pi)*360)/2;
if (x > 0) and (y < 0) then
fa := (((arctan(-y/x)/pi)*360)/2)+90;
if (x < 0) and (y > 0) then
fa := ((arctan(y/x)/pi)*360)/2;
if (x < 0) and (y < 0) then
fa := (((arctan(-y/x)/pi)*360)/2)-90;
end;
if x = 0 then
begin
if y >= 0 then
fa := 0;
if y < 0 then
fa := 180;
end;
if y = 0 then
begin
if x > 0 then
fa := 90;
if x < 0 then
fa := -90;
end;
{Angulo del motor 2(hombro) con respecto a la base (f1)}
if zcodo = 0 then
begin
if ycodo >= 0 then f1 := 90 else
f1 := -90;
end
else
f1 := ((arctan(ycodo/zcodo)/pi)*360)/2;
if zcodo < 0 then
begin
f1 := 180 + f1;
end;
{Angulo intermedio}
f2 := 90 - f1;
{Angulo del motor 3(codo) con respecto al antebrazo (f23)}
if (zcodo > zmuneca) and (a > ycodo) then
begin
f3 := ((arctan((zcodo-zmuneca)/(a-ycodo))/pi)*360)/2;
f23 := f2 +f3;
if f23 < 0 then f23 := f23 + 180;
end;
if (zcodo > zmuneca) and (a < ycodo) then
begin
f3 := ((arctan((ycodo-a)/(zcodo-zmuneca))/pi)*180)+90;
f23 := f2 + f3;
if f23 < 0 then f23 := f23 + 180;
end;
if (zmuneca > zcodo) then
begin
f3 := ((arctan((zmuneca-zcodo)/(a-ycodo))/pi)*360)/2;
f23 := f2 - f3;
if f23 < 0 then f23 := f23 + 180;
end;
if (zmuneca = zcodo) then f23 := f2;
{Angulo de ataque}
f4 := f;
{Angulo del motor 4(mu¤aca) con respecto al brazo (f34)}
if (zmuneca > (z-l1)) and (zmuneca > zcodo) then
begin
f34 := f3 +f4;
end;
if (zmuneca < (z-l1)) and (zcodo > zmuneca) then
begin
if f3 > f4 then
f34 := -(f3 - f4);
if f3 < f4 then
f34 := f4 - f3;
end;
if ((z-l1) > zmuneca) and (zmuneca > zcodo) then
begin
if f3 > f4 then
begin
f34 := f3 + f4;
end;
if f3 < f4 then
begin
f34 := -(f4 - f3);
end;
end;
if ((z-l1) < zmuneca) and (zcodo > zmuneca) then
begin
if f3 > f4 then
f34 := -(f3 - f4);
if f3 < f4 then
begin
f34 := f4 - f3;
end;
end;
if ((z-l1) = zmuneca) then f34 := f3;
{Puntos necesarios para dibujar el brazo}
puntos[1,1] := trunc(0) ; puntos[1,2] := trunc(l1*escaladibujo1);
puntos[2,1] := trunc(ycodo*escaladibujo1); puntos[2,2] := trunc((l1+
zcodo)*escaladibujo1);
puntos[3,1] := trunc(a*escaladibujo1) ; puntos[3,2] := trunc((l1+
zmuneca)*escaladibujo1);
puntos[4,1] := trunc((a+a1)*escaladibujo1) ; puntos[4,2] :=
trunc(z*escaladibujo1);
puntos[5,1] := trunc(x * escaladibujo2); puntos[5,2] := trunc(y *
escaladibujo2);
end;
{encuentra las direcciones del puerto paralelo}
function findport(var data,status,control : word): boolean;
begin
data := MemW[$0040:8]; {No se que hace, lo copie}
status := data +1;
control := status +1;
end;
{Modifica el registro del motor segun el movimiento que tenga que hacer}
procedure avanzar (dir : integer ; var steper : pasoapaso);
var
pasoinicial : byte;
begin
with steper do
begin
pasoinicial := pasoactual;
if (pasoinicial < 4) and (dir = +1) then {Si tiene que
incrementar un paso y no es el ultimo (4)}
begin
pasoactual := pasoactual + dir; {Incrementa el paso}
anguloactual := anguloactual + (app * dir); {Incrementa el angulo}
exit;
end;
if (pasoinicial > 1) and (dir = -1) then {Si tiene que
decrementar un paso y no es el primero (1)}
begin
pasoactual := pasoactual + dir; {Decrementa el paso}
anguloactual := anguloactual + (app * dir); {Decrementa el angulo}
exit;
end;
if (pasoinicial = 4) and (dir = 1) then begin {Si tiene que
incrementar un paso y es el ultimo(4)}
pasoactual := 1; {Vuelve a empesar
la secuencia de pasos}
numerosec := numerosec + dir;
{Incrementa el numero de secuencia}
anguloactual := anguloactual + (app *
dir); {Incrementa el angulo}
exit;
end;
if (pasoinicial = 1) and (dir =-1) then begin {Si tiene que
decrementar un paso y es el primero(1)}
pasoactual := 4; {Vuelve al final de
la secuencia anterior}
numerosec := numerosec + dir;
{Decrementa el numero de secuencia}
anguloactual := anguloactual + (app *
dir); {Decrementa el angulo}
exit;
end;
end;
end;
{Calcula los pasos de cada motor segun los angulos de cada uno
y mueve los motores}
procedure mover(fa,f1,f23,f34: real);
var
ang : array[1..4] of real;
dir : array[1..4] of shortint;
pasos_calculados : array[1..4] of real;
pasos_reales, secuencia_tope, pasos_tope : array[1..4] of integer;
i : integer;
ok : array[1..4] of boolean;
pt3 : string;
begin
{Paso los angulos a un array para usarlos en un FOR}
ang[1] := fa; ang[2] := f1; ang[3] := f23; ang[4] := f34;
for i := 1 to 4 do
begin
{Determino la direccion del movimiento(giro) nesesario
para ir de la posision actual a la nueva posision}
if ang[i] > steper[i].anguloactual then dir[i] := +1;
if ang[i] = steper[i].anguloactual then dir[i] := 0;
if ang[i] < steper[i].anguloactual then dir[i] := -1;
{Calculo la cantidad de pasos reales del motor segun su angulo por paso}
{para llegar a la nueva posision desde la ultima posision}
if dir[i] > 0 then pasos_calculados[i] := (ang[i] -
steper[i].anguloactual)/steper[i].app;
if dir[i] < 0 then pasos_calculados[i] := (steper[i].anguloactual -
ang[i])/steper[i].app;
if dir[i] = 0 then pasos_calculados[i] := 0;
{Convierto el valor anterior en valores enteros, que son
los que sirven para mover el motor}
if (pasos_calculados[i] - (trunc(pasos_calculados[i])) < 0.5)
then pasos_reales[i] := trunc(pasos_calculados[i])
else pasos_reales[i] := trunc(pasos_calculados[i]) +1;
{Sabiendo el paso inicial y la cantidad de pasos a realizar, calculo
la secuencia y el paso en el que tendria que estar en la posison final}
secuencia_tope[i] := steper[i].numerosec + ((pasos_reales[i] div 4) *
dir[i]); {Calcula el numero de la secuencia }
pasos_tope[i] := steper[i].pasoactual + ((pasos_reales[i] mod 4) * dir[i]);
{y el paso correcto}
if (pasos_tope[i] > 4) then
begin
inc(secuencia_tope[i]); { secuencia_tope[i] +1}
pasos_tope[i] := pasos_tope[i] - 4;
end;
if (pasos_tope[i] < 1) then
begin
dec(secuencia_tope[i]); { secuencia_tope[i] -1}
pasos_tope[i] := pasos_tope[i] + 4;
end;
end;
{A continuacion se mueven los motores de a un paso y en forma secuencial
para simular un movimiento homogeneo.En cada pasada se actualizan los
valores en pantalla}
repeat
for i := 1 to 4 do
begin
ok[i] := false;
if (steper[i].pasoactual <> pasos_tope[i]) or {Si no llego a su
posision final se sigue moviendo}
(steper[i].numerosec <> secuencia_tope[i]) then
begin
avanzar(dir[i],steper[i]);
end
else
ok[i] := true; {Si llego esta "ok"}
end;
generarpantalla; {Actualiza valores en pantalla}
until ok[1] and ok[2] and ok[3] and ok[4];
end;
{Dibuja el brazo en la posision actual uniendo los puntos de
inteseccion entre los segmentos dados por el procedimiento de
calculo}
procedure grafico ( p: vector);
var i,j : integer;
ff : word;
begin
setfillstyle(1,9);
bar (ref1_x+p[1,1]-20, ref1_y, ref1_x+p[1,1]+20, ref1_y-5);
setfillstyle(1,colores[1]);
bar (ref1_x+p[1,1]-2, ref1_y, ref1_x+p[1,1]+2, ref1_y-p[1,2]);
setcolor(colores[2]);line ( ref1_x+p[1,1], ref1_y-p[1,2], ref1_x+p[2,1],
ref1_y-p[2,2]);
setcolor(colores[3]);line ( ref1_x+p[2,1], ref1_y-p[2,2], ref1_x+p[3,1],
ref1_y-p[3,2]);
setcolor(colores[4]);line ( ref1_x+p[3,1], ref1_y-p[3,2], ref1_x+p[4,1],
ref1_y-p[4,2]);
setcolor(1);
for i:= 1 to 4 do
circle (ref1_x+p[i,1], ref1_y-p[i,2], 2);
circle (ref2_x , ref2_y, 20);
circle (ref2_x , ref2_y, 5);
line (ref2_x, ref2_y, ref2_x + p[5,1],ref2_y - p[5,2]);
end;
{Pide la nueva coordenada (x,y,z,angulo de ataque)}
procedure pedirdatos(var xn,yn,zn,fn: real);
begin
generarpantalla;
grafico(puntos);
textcolor(1);
{$i-}
moveto(0,(GetMaxy -20));
outtext(' Ingrese cualquier letra en cualquier campo para salir');
moveto(5,80); outtext('Ingrese Nuevas coordenadas');
moveto(559,255);outtext('Nuevo: ');
moveto(559,275);outtext('x: ');
gotoxy(73,18); readln(xn); {Nueva coordenada en x}
moveto(559,291);outtext('y: ');
gotoxy(73,19); readln(yn); {Nueva coordenada en y}
moveto(559,307);outtext('z: ');
gotoxy(73,20); readln(zn); {Nueva coordenada en z}
moveto(559,323);outtext('Angulo: ');
gotoxy(72,22); readln(fn); {Angulo de ataque}
{$i+}
end;
procedure pedirlargos(var l1,l2,l3,l4: real);
begin
generarpantalla;
grafico(puntos);
textcolor(1);
{$i-}
moveto(0,(GetMaxy -20));
outtext(' Ingrese cualquier letra en cualquier campo para salir');
moveto(180,82); outtext('Ingrese Nuevas dimensiones');
moveto(559,255);outtext('Nuevo: ');
moveto(559,275);outtext('l1: ');
gotoxy(74,18); readln(l1); {Nueva coordenada en x}
moveto(559,291);outtext('l2: ');
gotoxy(74,19); readln(l2); {Nueva coordenada en y}
moveto(559,307);outtext('l3: ');
gotoxy(74,20); readln(l3); {Nueva coordenada en z}
moveto(559,323);outtext('l4: ');
gotoxy(74,21); readln(l4); {Angulo de ataque}
{$i+}
escaladibujo1 := 240/(l1+l2+l3+l4);
escaladibujo2 := 175/(l1+l2+l3+l4);
end;
procedure moverX;
var key : char;
exit : boolean;
const paso = 0.05;
begin
repeat
generarpantalla;
grafico(puntos);
setcolor(9);
moveto(250,80); outtext('Modo manual');
moveto(5,100); outtext('Modificando eje x (+) : aumenta (-) : decrementa ');
moveto(0,GetMaxY-20); outtext(' Presione [ q ] para salir');
exit := false;
key := readkey;
case key of
#43 : begin
xn := x+paso;
calculo(xn,y,z,f,fa,f1,f23,f34);
mover(fa,f1,f23,f34);
end;
#45 : begin
xn := x-paso;
calculo(xn,y,z,f,fa,f1,f23,f34);
mover(fa,f1,f23,f34);
end;
'q' : exit := true;
end;
until exit;
end;
procedure moverY;
var key : char;
exit : boolean;
const paso = 0.05;
begin
repeat
generarpantalla;
grafico(puntos);
setcolor(9);
moveto(250,80); outtext('Modo manual');
moveto(5,100); outtext('Modificando eje y (+) : aumenta (-) : decrementa ');
moveto(0,GetMaxY-20); outtext(' Presione [ q ] para salir');
exit := false;
key := readkey;
case key of
#43 : begin
yn := y+paso;
calculo(x,yn,z,f,fa,f1,f23,f34);
mover(fa,f1,f23,f34);
end;
#45 : begin
yn := y-paso;
calculo(x,yn,z,f,fa,f1,f23,f34);
mover(fa,f1,f23,f34);
end;
'q' : exit := true;
end;
until exit;
end;
procedure moverZ;
var key : char;
exit : boolean;
const paso = 0.05;
begin
repeat
generarpantalla;
grafico(puntos);
setcolor(9);
moveto(250,80); outtext('Modo manual');
moveto(5,100); outtext('Modificando eje z (+) : aumenta (-) : decrementa ');
moveto(0,GetMaxY-20); outtext(' Presione [ q ] para salir');
exit := false;
key := readkey;
case key of
#43 : begin
zn := z+paso;
calculo(x,y,zn,f,fa,f1,f23,f34);
mover(fa,f1,f23,f34);
end;
#45 : begin
zn := z-paso;
calculo(x,y,zn,f,fa,f1,f23,f34);
mover(fa,f1,f23,f34);
end;
'q' : exit := true;
end;
until exit;
end;
procedure moverF;
var key : char;
exit : boolean;
const paso = 0.05;
begin
repeat
generarpantalla;
grafico(puntos);
setcolor(9);
moveto(250,80); outtext('Modo manual');
moveto(5,100); outtext('Modificando Angulo (+) : aumenta (-) : decrementa');
moveto(0,GetMaxY-20); outtext(' Presione [ q ] para salir');
exit := false;
key := readkey;
case key of
#43 : begin
fn := f+paso;
calculo(x,y,z,fn,fa,f1,f23,f34);
mover(fa,f1,f23,f34);
end;
#45 : begin
fn := f-paso;
calculo(x,y,z,fn,fa,f1,f23,f34);
mover(fa,f1,f23,f34);
end;
'q' : exit := true;
end;
until exit;
end;
procedure manual;
var
key : char;
exit : boolean;
begin
repeat
generarpantalla;
setcolor(9);
moveto(250,80); outtext('Modo manual');
moveto(5,102); outtext('x - Mueve(x) y - Mueve(y) z - Mueve(z) a - Mueve(f)');
moveto(0,GetMaxY-20); outtext(' Presione [ q ] para salir');
grafico(puntos);
exit := false;
key := readkey;
case key of
'x' : moverX;
'y' : moverY;
'z' : moverZ;
'a' : moverF;
'q' : exit := true;
end;
until exit ;
end;
procedure reset;
begin
{inicializa variables}
steper[1].cod := 1;
steper[2].cod := 2;
steper[3].cod := 4;
steper[4].cod := 8;
matriz[1,1] := 17; matriz[1,2] := 33; matriz[1,3] := 65; matriz[1,4] := 129;
matriz[2,1] := 18; matriz[2,2] := 34; matriz[2,3] := 66; matriz[2,4] := 130;
matriz[3,1] := 20; matriz[3,2] := 36; matriz[3,3] := 68; matriz[3,4] := 132;
matriz[4,1] := 24; matriz[4,2] := 40; matriz[4,3] := 72; matriz[4,4] := 136;
{Estos valores dependen de la posision en
la que se encuentra el brazo en la posision
inicial, despues del reset.Solo se podran
completar correctamente una vez terminada la
estructura de brazo}
for i :=1 to 4 do
begin
steper[i].pasoactual:= 1;
steper[i].numerosec:= 0;
steper[i].motor:= i;
steper[i].app := 1;
steper[i].anguloactual := 0;
end;
x :=0 ;
y:=0 ;
z:=0 ;
xn:=0 ;
yn:=0 ;
zn:=0 ;
f:=0 ;
fn:=0 ;
fa:=0 ;
f1:=0 ;
f23:=0 ;
f34:=0;
end;
begin
clrscr;
{inicia modo grafico}
Gd := Detect;
InitGraph(Gd, gm, 'c:\tp\bgi');
if GraphResult <> grOk then
Halt(1);
SetTextStyle(0, HorizDir, 1);
findport(data,status,control); {Encuentra las direcciones base,status y
control del puerto paralelo}
{Limpia la salida de datos}
port[data] := 0;
port[data] := 15;
port[data] := 0;
reset;
loadlongs;
escaladibujo1 := 240/(l1+l2+l3+l4);
escaladibujo2 := 175/(l1+l2+l3+l4);
{Estos son los valores de las coordenadas de la posision inicial}
x := 0; y := 0; z := l1+l2+l3+l4-0.00000000004378 ;f := -90;
{Inicializacion del programa con los valores
determinados como iniciales}
calculo(x,y,z,f,fa,f1,f23,f34);
mover(fa,f1,f23,f34);
colores[1] := 1 ; colores[2] := 2 ; colores[3] := 4 ; colores[4] := 7;
{Programa principal}
repeat
generarpantalla; {Dibuja la pantalla de fondo}
grafico(puntos); {Dibujo aproximado del brazo robot}
setcolor(9);
{Dibuja el men£ principal}
moveto(260,80); outtext('Opciones');
moveto(5,102); outtext(' (1) -Nueva posicion (2) -Forma Manual (3) -Nuevas
Longitudes');
SetTextStyle(0, HorizDir, 1);
moveto(0,GetMaxY-20); outtext(' Presione [ q ] para salir');
option := '0';
option := readkey;
case option of
'1':
begin
pedirdatos(xn,yn,zn,fn); {Pide las nuevas coordenadas}
if IOResult = 0 then
begin
calculo(xn,yn,zn,fn,fa,f1,f23,f34); {Calcula los angulos}
mover(fa,f1,f23,f34); {Mueve el brazo segun los angulos calculados}
end;
end;
'2': manual;
'3' :
begin
pedirlargos(l1n,l2n,l3n,l4n);
if (IOResult = 0) then
begin
reset;
l1:=l1n ; l2:=l2n ; l3:=l3n ; l4:=l4n;
x := 0; y := 0; z := l1+l2+l3+l4-0.0001 ;f := -90;
calculo(x,y,z,f,fa,f1,f23,f34); {Calcula los angulos}
mover(fa,f1,f23,f34); {Mueve el brazo segun los angulos calculados}
savelongs;
end;
end;
end;
until (option = 'q'); {Con la tecla "q" se sale del programa}
closegraph; {Se desactiva el modo grafico}
{Limpia la salida}
port[data] := 15;
port[data] := 0;
end.
|
¿Buscás un auto?
Sin conexión 
Enviar mensaje