ROBOLIST.DOC The printed listing of the Robot Simulator, dated January 15, 1990: 3,360 lines, shown as in his file
*******************************************
* *
* Robot Simulator software package *
* *
* Author: Andrew B. Conru *
* *
* Date: January 15, 1990 *
* *
* No reproduction of this software or *
* report may be copied an any medium *
* without the written permission of *
* the above stated author. *
* *
*******************************************{this is the main menu system along with some general procedures}
program menu_driver(input,output);
{$F+}
USES DOS,RoboSess,Drivers,Fonts,LISTTTT5,CRT,FASTTTT5, WINTTT5,RoboDir,
MenuTTT5,Graph,IOTTT5, KeyTTT5,Strnttt5,RoboDbas,MiscTTT5,ReadTTT5,
RoboCov,RoboAid;
Const
ListWidth = 60;
Type
StrList = string[ListWidth];
Var BackFromSession,PickedFile : integer;
RevList : array[1..50] of StrList;
FirstName,LastName,Dir,IDnum : string;
{************************************************************************}
Procedure Menus;
var
M100,M140,M170,M180 : menu_record;
Back_col,Bar_col,Cap_col,Low_col,Top_col : integer;
Choice, Retcode,Choice1,Retcode1,Choice2,Retcode2 : integer;
Ch : char;
Dummy : StrScreen;
DummyRetcode : integer;
{************************************************************************}
Procedure Abort(Msg:string);{Graphics Error Trapping Procedure.}
Begin
Writeln(Msg,':',GraphErrorMsg(GraphResult));
Halt(1);
End;
{************************************************************************}
Procedure Register;
Begin
if RegisterBGIdriver(@EGAVGADriverProc) < 0 then Abort('EGA');
if RegisterBGIfont(@SmallFontProc) < 0 then Abort('Small');
if RegisterBGIfont(@TriplexFontProc) < 0 then Abort('Triplex');
End;
{***********************************************************************}
Procedure Define_colors;
Begin
Back_col := lightgray;
Bar_col := cyan;
Cap_col := black;
Low_col := black;
Top_col := blue;
Register;
End;
{***********************************************************************}
Procedure SetScrollOptions;
Begin
with LTTT do
begin
Lines := 15;
Y := 5;
BacCol := cyan;
BoxFCol := cyan;
BoxBCol := cyan;
CapFCol := blue;
NorFCol := blue;
HiBCol := blue;
HiFCol := lightblue;
LeftChar := chr(026);
RightChar := ' ';
AllowToggle := false;
end;
End;
{***********************************************************************}
Procedure Define_Menus;
begin
Menu_Set(M100); { Main Menu }
With M100 do
begin
Heading1 := 'Main Menu';
Heading2 := '';
Topic[1] := ' Make/Get Robot';
Topic[2] := ' Play Robot';
Topic[3] := ' Teach Robot';
Topic[4] := ' Run Robot';
Topic[5] := ' RRR Rev.Kin.';
Topic[6] := ' Help Screens';
Topic[7] := ' Output Screen';
Topic[8] := ' File Manager';
Topic[9] := ' Exit Program';
TotalPicks := 9;
TopleftXY[1] := 3; {auto center}
TopLeftXY[2] := 3;
PicksPerLine := 1;
AddPrefix := 4;
BoxType := 5;
Margins := 1;
Colors[1] := Cap_col;
Colors[2] := Bar_col;
Colors[3] := Low_col;
Colors[4] := Back_col;
Colors[5] := Top_col;
AllowEsc := false;
end;
Menu_Set(M140); {Review Menu }
With M140 do
begin
Heading1 := 'Topic 4';
Heading2 := '';
Topic[1] := ' Graph Only';
Topic[2] := ' Graph&Stats';
Topic[3] := ' Stats Only';
Topic[4] := ' Help';
Topic[5] := ' Quit';
TotalPicks := 5;
TopleftXY[1] := 23;
TopLeftXY[2] := 6;
PicksPerLine := 1;
AddPrefix := 4;
BoxType := 5;
Margins := 1;
Colors[1] := Cap_col;
Colors[2] := Bar_col;
Colors[3] := Low_col;
Colors[4] := Back_col;
Colors[5] := Top_col;
AllowEsc := false;
end;
Menu_Set(M170); { Screen Type }
With M170 do
begin
Heading1 := 'Screen Type';
Heading2 := '';
Topic[1] := ' 3-projections';
Topic[2] := ' 1-projection';
Topic[3] := ' Quit';
TotalPicks := 3;
TopleftXY[1] := 32;
TopLeftXY[2] := 7;
PicksPerLine := 1;
AddPrefix := 4;
BoxType := 5;
Margins := 1;
Colors[1] := Cap_col;
Colors[2] := Bar_col;
Colors[3] := Low_col;
Colors[4] := Back_col;
Colors[5] := Top_col;
AllowEsc := false;
end;
Menu_Set(M180); { Screen Type }
With M180 do
begin
Heading1 := 'Help Screens';
Heading2 := '';
Topic[1] := ' Robot Database';
Topic[2] := ' Play Robot ';
Topic[3] := ' Teach Robot';
Topic[4] := ' Run Robot ';
Topic[5] := ' Reverse Kin.';
Topic[6] := ' Output Screens';
Topic[7] := ' Quit';
TotalPicks := 7;
TopleftXY[1] := 32;
TopLeftXY[2] := 7;
PicksPerLine := 1;
AddPrefix := 4;
BoxType := 5;
Margins := 1;
Colors[1] := Cap_col;
Colors[2] := Bar_col;
Colors[3] := Low_col;
Colors[4] := Back_col;
Colors[5] := Top_col;
AllowEsc := false;
end;
end;
{*********************************************************************}
Procedure InfoUpdate(RoboName:string; DOF,ScreenMode:integer; FileName:string);
Const
Hi = cyan;
Con = black;
Back = blue;
Begin
FBox(1,20,80,24,back,back,0);
WriteAt(24,21,con,back,'Robot Name:');
WriteAt(52,21,con,back,'DOF:');
WriteAt(29,22,con,back,'Run File:');
WriteAt(29,23,con,back,'Screen Type:');
WriteAt(36,21,hi,back,RoboName);
WriteAt(57,21,hi,back,int_to_str(DOF));
WriteAt(39,22,hi,back,FileName);
if ScreenMode = 0 then
begin
WriteAt(43,23,hi,back,'3-Projections');
end
else
begin
WriteAt(43,23,hi,back,'1-Projection');
end;
End;
{**************************************************************************}
Procedure Fill_RevList(VAR NumFound:integer);
Var
Index : integer;
DateTimeStr,TestFile,month,hour,AMPM,Spaces,Size,DateStr,TimeStr : string[60];
FileTime : longint;
DT : DateTime;
DirInfo : SearchRec;
Begin
Index:=0;
Spaces := ' ';{if you change this change call to readyrev}
FindFirst('*.PTP',AnyFile,DirInfo);
While DosError = 0 do
Begin
inc(Index);
UnPackTime(DirInfo.Time, DT);
Case DT.month of
1: Month:='Jan';
2: Month:='Feb';
3: Month:='Mar';
4: Month:='Apr';
5: Month:='May';
6: Month:='Jun';
7: Month:='Jul';
8: Month:='Aug';
9: Month:='Sep';
10: Month:='Oct';
11: Month:='Nov';
12: Month:='Dec';
end;
Case DT.hour of
0..11: Begin
Hour := int_to_str(DT.hour);
AMPM := 'AM';
End;
12..23: Begin
Hour := int_to_str(DT.hour-12);
AMPM := 'PM';
End;
end;
TimeStr:=concat(hour+':'+int_to_str(DT.min)+' '+AMPM);
TimeStr:=copy(concat(TimeStr+' '),1,10);
DateStr:=concat(month+' '+int_to_str(DT.day)+', '+int_to_str(DT.year));
DateStr:=copy(concat(DateStr+' '),1,15);
DirInfo.Name:=copy(concat(DirInfo.Name+' '),1,17);
Size:=copy(concat(int_to_str(DirInfo.Size)+' '),1,9);
RevList[index]:=concat(Spaces+DirInfo.Name+Size+DateStr+TimeStr);
FindNext(DirInfo);
End;
inc(index);
RevList[index]:=' <<<< SELECT THIS TO RETURN TO MAIN MENU >>>>';
NumFound := Index;
End;
{**************************************************************************}
Procedure NoFindPTPfiles;
Begin
MkWin(43,5,77,14,yellow,lightgray,2);
WriteAt(53,6,blue,lightgray,'<<< PROBLEM >>>');
WriteAT(45,8,blue,lightgray,'Can not find *.PTP files in the');
WriteAT(53,10,blue,lightgray,'Current Directory');
WriteAT(46,12,blue,lightgray,' Type any key to continue');
Repeat
begin
delay(200);
end
Until KeyPressed;
RmWin;
End;
{**************************************************************************}
Procedure NoRobotDefined;
Begin
MkWin(43,5,77,14,yellow,lightgray,2);
WriteAt(53,6,blue,lightgray,'<<< PROBLEM >>>');
WriteAT(50,8,blue,lightgray,'Robot not defined yet.');
WriteAT(52,10,blue,lightgray,'Use Database Routine');
WriteAT(46,12,blue,lightgray,' Type any key to continue');
Repeat
begin
delay(200);
end
Until KeyPressed;
RmWin;
End;
{**************************************************************************}
Procedure GetRRR3D;
Begin
MkWin(43,5,77,14,yellow,lightgray,2);
WriteAt(53,6,blue,lightgray,'<<< PROBLEM >>>');
WriteAT(50,8,blue,lightgray,'Define RRR-3D Robot');
WriteAT(52,10,blue,lightgray,'In Database Routine');
WriteAT(46,12,blue,lightgray,' Type any key to continue');
Repeat
begin
delay(200);
end
Until KeyPressed;
RmWin;
End;
{**************************************************************************}
Procedure NotSameDOF(TestDOF:integer);
Begin
MkWin(43,5,77,14,yellow,lightgray,2);
WriteAt(53,6,blue,lightgray,'<<< PROBLEM >>>');
WriteAT(50,8,blue,lightgray,'Robot''s DOF not same as file''s');
WriteAT(52,10,blue,lightgray,concat('File DOF = '+Int_to_str(TestDOF)));
WriteAT(46,12,blue,lightgray,' Type any key to continue');
Repeat
begin
delay(200);
end
Until KeyPressed;
RmWin;
End;
{**************************************************************************}
{Main Program}
VAR
ReturnCode,SesDur,index,NumFound: integer;
TopLine : string[80];
RoboName,InPutFile : string;
ScreenMode,DOF,TestDOF : integer;
alpha0,a0,d1,theta1,alpha1,a1,d2,theta2 : real;
alpha2,a2,d3,theta3,alpha3,a3,d4,theta4 : real;
alpha4,a4,d5,theta5,alpha5,a5,d6,theta6 : real;
alpha6,a6,d7,theta7 : real;
OutFileStr : strscreen;
InFile,OutFile : string;
OverWrite,NewFile : boolean;
DataFile : Text;
tstring:string;
Begin
ClrScr;
{MenuBack;}
Choice := 1;{Defaults to where you want cursor sent in menus}
Choice1 := 1;
Choice2 := 1;
InFile := 'Not Defined';
OutFile := 'Not Defined';
RoboName := 'Run Database';
DOF := 0;
ScreenMode:=0;
Define_colors;
InitializeGraphics;
CoverPage;
Define_menus;
WriteAt(1,1,yellow,lightgray,' Robot Simulation Program Ver. 1.0 (c) 1990 ');
Repeat
FillScreen(1,2,80,24,black,blue,char(177));
FillScreen(1,1,80,1,cyan,cyan,char(219));
FillScreen(1,25,80,25,cyan,cyan,char(177));
InfoUpdate(RoboName,DOF,ScreenMode,InFile);
TopLine := concat('Robot Simulation Program '+Date);
WriteCenter(1,black,cyan,TopLine);
TopLine := concat('Custom Software by A.B.Conru - Ver 1.0 (c) 1990');
WriteCenter(25,black,cyan,TopLine);
DisplayMenu(M100,true,Choice,Retcode);{Main Menu}
InfoUpdate(RoboName,DOF,ScreenMode,InFile);
if RetCode >= 0 Then Begin
Case Choice of
1 : Begin{1}
DODbase(RoboName,DOF,alpha0,a0,d1,theta1,alpha1,a1,d2,theta2,
alpha2,a2,d3,theta3,alpha3,a3,d4,theta4,alpha4,a4,d5,theta5,
alpha5,a5,d6,theta6,alpha6,a6,d7,theta7);
InfoUpdate(RoboName,DOF,ScreenMode,InFile);
End;{1}
2 : Begin{2 - Play With Robot}
RoboStuff(RoboName,OutFile,InFile,1,ScreenMode,DOF,alpha0,a0,d1,theta1,alpha1,a1,d2,theta2,
alpha2,a2,d3,theta3,alpha3,a3,d4,theta4,alpha4,a4,d5,theta5,
alpha5,a5,d6,theta6,alpha6,a6,d7,theta7);
End;{2}
3 : Begin{3 - Define Points With Robot}
if DOF>0 then
begin
NewFile:=False;
OverWrite:=False;
OutFile:='';
MkWin(33,5,75,14,yellow,lightgray,2);
WriteAT(45,6,yellow,lightgray,'Motion File Creator');
Read_String_Upper(37,10,8,'Enter File To Create ==>',1,OutFileStr);
OutFile:=Concat(OutFileStr+'.PTP');
if Exist(OutFile) then
begin
WriteAt(40,12,cyan,lightgray,'File Already Exists');
WriteAt(40,13,cyan,lightgray,'Want to overwrite it? Yes/No');
WriteAt(62,13,Lightcyan,cyan,'Y');
WriteAt(66,13,Lightcyan,cyan,'N');
Read_YN(69,13,'',0,OverWrite);
end
else
begin
NewFile:=true;
end;
RmWin;
if OverWrite=true then NewFile:=True;
if NewFile=true then
begin
InfoUpdate(RoboName,DOF,ScreenMode,OutFile);
RoboStuff(RoboName,OutFile,InFile,2,ScreenMode,DOF,alpha0,a0,d1,theta1,alpha1,a1,d2,theta2,
alpha2,a2,d3,theta3,alpha3,a3,d4,theta4,alpha4,a4,d5,theta5,
alpha5,a5,d6,theta6,alpha6,a6,d7,theta7);
end;
end
else
begin
NoRobotDefined;
end;
End;{3}
4 : Begin{4 - Run Robot Simulation using data files}
if DOF>0 then
begin
Fill_RevList(NumFound);
if NumFound>1 then begin
MkWin(1,1,80,25,cyan,cyan,2);
SetScrollOptions;
WriteCenter(2,yellow,cyan,'Pick The File To Review');
WriteAT(16,4,black,cyan,'FileName Size Date Time');
WriteCenter(24,black,cyan,'Use the arrow keys to select file');
WriteCenter(25,black,cyan,'Press <Return> to pick file to use');
Show_List(RevList,ListWidth,NumFound);
PickedFile:=L_Pick;
InPutFile:=RevList[L_Pick];
InFile:=copy(InputFile,9,13);
RmWin;
if PickedFile<NumFound then
begin
Assign(DataFile,InFile);
ReSet(DataFile);
read(DataFile,TestDOF);
Close(DataFile);
if TestDOF=DOF then
begin
InfoUpdate(RoboName,DOF,ScreenMode,InFile);
RoboStuff(RoboName,OutFile,InFile,3,ScreenMode,DOF,alpha0,a0,d1,theta1,alpha1,a1,d2,theta2,
alpha2,a2,d3,theta3,alpha3,a3,d4,theta4,alpha4,a4,d5,theta5,
alpha5,a5,d6,theta6,alpha6,a6,d7,theta7);
end
else
begin
NotSameDOF(TestDOF);
end;
end;{if pickedfile<numfound}
end{if numfound>1}
else
begin
NoFindPTPfiles;
end;
end
else
begin
NoRobotDefined;
end;
End;{4}
5 : Begin{5=reverse kinematics on rrr-3d robot}
if RoboName='RRR-3D' then
Begin
RoboStuff(RoboName,OutFile,InFile,4,ScreenMode,DOF,alpha0,a0,d1,theta1,alpha1,a1,d2,theta2,
alpha2,a2,d3,theta3,alpha3,a3,d4,theta4,alpha4,a4,d5,theta5,
alpha5,a5,d6,theta6,alpha6,a6,d7,theta7);
End
Else
Begin
GetRRR3D;
End;
End;{5}
6 : Begin{6=help screens}
DisplayMenu(M180,true,Choice2,Retcode2);
if RetCode2 >= 0 then begin
Case Choice2 of
1 : Begin
Help(1);{database}
End;
2 : Begin
Help(2);{play mode}
End;
3 : Begin
Help(3);{teach mode}
End;
4 : Begin
Help(4);{run mode}
End;
5 : Begin
Help(5);{reverse kinematics}
End;
6 : Begin
Help(6);{screen modes}
End;
end; {case}
end;
End;{6}
7 : Begin{7=output screen}
DisplayMenu(M170,true,Choice2,Retcode2);
if RetCode2 >= 0 then begin
Case Choice2 of
1 : Begin
ScreenMode:=0;
End;
2 : Begin
ScreenMode:=1;
End;
end; {case}
end;
End;{7}
8 : Begin{8}
Dummy := Display_Directory('*.*',DummyRetcode);
End;{8}
9 : Begin{9}
End;{9}
End;{case}
End;
Until (Retcode <> 0)or(Choice IN [9]);
ClrScr;
LowVideo;
end;
Begin
Repeat
Menus;
BackFromSession :=0;
Until BackFromSession = 0;
End.UNIT ROBOSESS;
INTERFACE
Uses CRT,DOS,Graph,FastTTT5,WinTTT5,StrnTTT5,KeyTTT5,
ReadTTT5,MiscTTT5;
var
GrMode,GrDriver,GrError : integer;
ErrCode : integer;
Procedure InitializeGraphics;
Procedure RoboStuff(RoboName,OutFile,InFile:string;RunMode,ScreenMode,
DOF:integer;alpha0,a0,d1,theta1,
alpha1,a1,d2,theta2,alpha2,a2,d3,theta3,
alpha3,a3,d4,theta4,alpha4,a4,d5,theta5,
alpha5,a5,d6,theta6,alpha6,a6,d7,theta7:real);
(********************************************************************)
IMPLEMENTATION
Procedure InitializeGraphics;
Var
GraphDriver,GraphMode,Error:integer;
Begin
GrDriver := Ega;
GrMode := EgaHi;
InitGraph(GrDriver,GrMode,'');{'' was 'C:\tp\graph'}
if GraphResult <> GrOK then
begin
WriteLn('Graphics Init Error: ',
GraphErrorMsg(GraphDriver));
Halt(1);
end;
end;
(************************************************************************)
Procedure DrawScreen1;
Begin
SetColor(yellow);
MoveTo(1,1);
LineTo(640,1);
LineTo(640,325);
LineTo(1,325);
LineTo(1,1);
Line(315,1,315,325);
Line(315,158,640,158);
SetColor(Red);
Circle(158,158,2);
Circle(473,128,2);
Circle(473,295,2);
OutTextXY(4,70,'y');
OutTextXY(210,315,'x');
OutTextXY(318,35,'z');
OutTextXY(535,148,'x');
OutTextXY(318,193,'z');
OutTextXY(535,315,'y');
SetColor(Cyan);
OutTextXY(208,328,'1-( , , ) 2-( , , ) 3-( , , )');
OutTextXY(208,339,'4-( , , ) 5-( , , ) 6-( , , )');
OutTextXY(11,328,'Variables:');
SetColor(4);
OutTextXY(100,328,char(24));
End;
{************************************************************************}
Procedure DrawScreen2;
Begin
SetColor(Cyan);
OutTextXY(208,328,'1-( , , ) 2-( , , ) 3-( , , )');
OutTextXY(208,339,'4-( , , ) 5-( , , ) 6-( , , )');
OutTextXY(11,328,'Variables:');
SetColor(4);
OutTextXY(100,328,char(24));
SetColor(yellow);
OutTextXY(50,300,'Pitch Axis:ctrl-p');
SetColor(cyan);
OutTextXY(225,300,'Roll Axis:ctrl-r');
SetColor(lightred);
OutTextXY(400,300,'Yaw Axis:ctrl-y');
End;
{**********************************************************************}
Procedure Atan(ty,tx:real;var atang:real);
var tstring : string;
begin
if abs(tx)<0.00001 then tx:=0.00001;
atang:=abs(arctan(ty/tx));
{ str(atang:8:4,tstring);
outtextxy(340,10,concat('atang t1 '+tstring));}
if (ty>0) and (tx<0) then atang:=3.14159265-atang;
if (ty<0) and (tx<0) then atang:=3.14159265+atang;
if (ty<0) and (tx>0) then atang:=-atang;
{ str(atang:8:4,tstring);
outtextxy(340,30,concat('atang t1 later '+tstring));
delay(1000);}
end;
{*********************************************************************}
Procedure RoboStuff(RoboName,OutFile,InFile:string;RunMode,ScreenMode,
DOF:integer;alpha0,a0,d1,theta1,
alpha1,a1,d2,theta2,alpha2,a2,d3,theta3,
alpha3,a3,d4,theta4,alpha4,a4,d5,theta5,
alpha5,a5,d6,theta6,alpha6,a6,d7,theta7:real);
type
RealArrayType7 = ARRAY[1..7] of real;
RealArrayType6 = ARRAY[0..7] of real;
Real3DArrayType733 = ARRAY[1..7,1..3,1..3] of real;
Real2DArrayType73 = ARRAY[1..7,1..3] of real;
RealArrayType3 = ARRAY[1..3] of real;
IntArrayType7 = ARRAY[1..7] of integer;
Int2DArrayType73 = ARRAY[1..7,1..3] of integer;
ByteArrayType7 = ARRAY[1..7] of byte;
var
DriverF: file;
DriverP: pointer;
TR2org,TR3org,TR21,TR22,TR31,TR32 : RealArrayType7;
alpha,a : RealArrayType6;
d,theta,dres,thetares,DorT : RealArrayType7;
Rorg,R0org,P0,OldP0 : Real2DArrayType73;
R,R0 : Real3DArrayType733;
x,y,z : RealArrayType3;
DTsign,Reps : IntArrayType7;
NumLoc : Int2DArrayType73;
LinkType,ColorArray : ByteArrayType7;
ctheta,stheta,calpha,salpha,m,re : real;
Yaw,Roll,Pitch,sYaw,sRoll,sPitch,cYaw,cRoll,cPitch,xa,ya,za : real;
xp,yp,zp : real;
Angle,OutofRange,InverseKin,Tracer : boolean;
t1,t21,t22,t23,t24,t31,t32,LL,LLc,Ls : real;
c21,c22,s21,s22,c3,s3,delt,testdelt,L1,L2 : real;
st2,ct2,st3,ct3,denom,L : real;
sign,code,t : integer;
xyz,i,imo,j,k : byte;
tstring,BigBar : string[10];
block : string[3];
GetOut,FreezeNumbers,Pauseit,ReadyForNextPt,FirstTimeRun : boolean;
RunReady,RunIt,Projection:boolean;
Ch : char;
DataFile : Text;
Begin
InitializeGraphics;
Yaw := 0;
Roll := 0;
Pitch := 0;
RunReady:=false;
GetOut := false;
FreezeNumbers := false;
sign := 1;
block := concat(char(219)+char(219)+char(219));
BigBar := concat(block+block+block);
if ScreenMode = 0 then
begin {page for 3 separate projections}
DrawScreen1;
end
else
begin {page for 1 projection}
DrawScreen2;
{x=right/left, y=in/out, z=up/down}
x[1] := 280; {x component of x axis}
x[2] := 0; {y component of x axis}
x[3] := 0; {z component of x axis}
y[1] := 0; {x component of y axis}
y[2] := 110; {y component of y axis}
y[3] := 0; {z component of y axis}
z[1] := 0; {x component of z axis}
z[2] := 0; {y component of z axis}
z[3] := 110; {z component of z axis}
end;
SetColor(cyan);
OutTextXY(5,339,'Robot: ');
SetColor(white);
OutTextXY(61,339,RoboName);
if RunMode=2 then
begin
Assign(DataFile,OutFile);
ReWrite(DataFile);
Writeln(DataFile,DOF);
SetColor(yellow);
OutTextXY(62+8*length(RoboName),339,concat(':'+copy(OutFile,1,8)));
end;
if RunMode=3 then
begin
setcolor(white);
Assign(DataFile,InFile);
ReSet(DataFile);
Readln(DataFile,t);
SetColor(yellow);
OutTextXY(62+8*length(RoboName),339,concat(':'+copy(InFile,1,8)));
FirstTimeRun:=true;
RunReady:=true;
RunIt:=false;
end;
if RunMode = 4 then
begin
FirstTimeRun:=true;
RunReady:=true;
RunIt:=false;
xp := 40;
yp := 40;
zp := 40;
L1 := 100;
L2 := 80;
angle := false;
end;
{define color array for arm output}
ColorArray[1]:=magenta;
ColorArray[2]:=blue;
ColorArray[3]:=cyan;
ColorArray[4]:=green;
ColorArray[5]:=yellow;
ColorArray[6]:=red;
ColorArray[7]:=lightred;
{define resolution for d and theta}
dres[1]:=1;
dres[2]:=1;
dres[3]:=1;
dres[4]:=1;
dres[5]:=1;
dres[6]:=1;
dres[7]:=1;
thetares[1]:=0.05;
thetares[2]:=0.05;
thetares[3]:=0.05;
thetares[4]:=0.05;
thetares[5]:=0.05;
thetares[6]:=0.05;
thetares[7]:=0.05;
{defines locations where to put x,y,z data on screen}
NumLoc[1,1]:=255;
NumLoc[2,1]:=400;
NumLoc[3,1]:=544;
NumLoc[4,1]:=255;
NumLoc[5,1]:=400;
NumLoc[6,1]:=544;
for i:=1 to 6 do
begin
NumLoc[i,2]:=NumLoc[i,1]+32;
NumLoc[i,3]:=NumLoc[i,1]+64;
end;
{initializes OldP0's}
for i:=1 to DOF+1 do
begin
OldP0[i,1]:=0;
OldP0[i,2]:=0;
OldP0[i,3]:=0;
end;
{sets up link parameter arrays}
alpha[0]:=alpha0*0.017453293;
alpha[1]:=alpha1*0.017453293;
alpha[2]:=alpha2*0.017453293;
alpha[3]:=alpha3*0.017453293;
alpha[4]:=alpha4*0.017453293;
alpha[5]:=alpha5*0.017453293;
alpha[6]:=alpha6*0.017453293;
a[0]:=a0;
a[1]:=a1;
a[2]:=a2;
a[3]:=a3;
a[4]:=a4;
a[5]:=a5;
a[6]:=a6;
d[1]:=d1;
d[2]:=d2;
d[3]:=d3;
d[4]:=d4;
d[5]:=d5;
d[6]:=d6;
d[7]:=d7;
theta[1]:=theta1*0.017453293;
theta[2]:=theta2*0.017453293;
theta[3]:=theta3*0.017453293;
theta[4]:=theta4*0.017453293;
theta[5]:=theta5*0.017453293;
theta[6]:=theta6*0.017453293;
theta[7]:=theta7*0.017453293;
alpha[DOF+1]:=0;
a[DOF+1]:=0;
{p 1}
for i:=1 to DOF do
begin
if d[i] = 999 then
begin
LinkType[i]:=1; {Prismatic}
if RunMode=3 then
begin
readln(DataFile,d[i]);
end
else
begin
d[i] := 40; {for now only}
end;
end
else
begin
LinkType[i]:=2; {Revolute}
if RunMode=3 then
begin
readln(DataFile,theta[i]);
end
else
begin
theta[i] := 30*0.017453293; {for now only}
end;
end;
end;
ReadyForNextPt:=true;
{p 2}
{defines transformation matrices with constants only}
for i:=1 to DOF+1 do
begin
if LinkType[i]=1 then
begin {Prismatic}
ctheta:=cos(theta[i]);
stheta:=sin(theta[i]);
calpha:=cos(alpha[i-1]);
salpha:=sin(alpha[i-1]);
R[i,1,1]:=ctheta;
R[i,1,2]:=-stheta;
R[i,1,3]:=0;
R[i,2,1]:=stheta*calpha;
R[i,2,2]:=ctheta*calpha;
R[i,2,3]:=-salpha;
R[i,3,1]:=stheta*salpha;
R[i,3,2]:=ctheta*salpha;
R[i,3,3]:=calpha;
Rorg[i,1]:=a[i-1];
TR2org[i]:=-salpha;
TR3org[i]:=calpha;
end {Prismatic}
else
begin {Revolute}
calpha:=cos(alpha[i-1]);
salpha:=sin(alpha[i-1]);
R[i,1,3]:=0;
TR21[i]:=calpha;
TR22[i]:=calpha;
R[i,2,3]:=-salpha;
TR31[i]:=salpha;
TR32[i]:=salpha;
R[i,3,3]:=calpha;
Rorg[i,1]:=a[i-1];
Rorg[i,2]:=-salpha*d[i];
Rorg[i,3]:=calpha*d[i];
end; {Revolute}
end;
{solve for initial theta's if in inverse kinematics mode}
if RunMode = 4 then
begin
theta[1] := 0.5236;
theta[2] := 1.05;
theta[3] := -0.5236;
atan(yp,xp,t1);
LL:=xp*xp+yp*yp;
c3:=(LL+zp*zp-L1*L1-L2*L2)/(2*L1*L2);
s3:=sqrt(1-c3*c3);
atan(s3,c3,t31);
atan(-s3,c3,t32);
LLc:=(L1+L2*c3);
Ls:=L2*s3;
c21:=(sqrt(LL)*LLc+Ls*zp)/(LLc*LLc+Ls*Ls); {+s3}
c22:=(sqrt(LL)*LLc-Ls*zp)/(LLc*LLc+Ls*Ls); {-s3}
s21:=sqrt(1-c21*c21); {+s3}
s22:=sqrt(1-c22*c22); {-s3}
atan(s21,c21,t21);
atan(-s21,c21,t22);
atan(s22,c22,t23);
atan(-s22,c22,t24);
delt := 9999;
if abs(L1*sin(t21)+L2*sin(t21+t31)-zp)<0.03 then
begin
if abs(L1*cos(t21)+L2*cos(t21+t31)-sqrt(LL))<0.03 then
begin
testdelt := abs(theta[2]-t21)+abs(theta[3]-t31);
if testdelt<delt then
begin
delt:=testdelt;
theta[1]:=t1;
theta[2]:=t21;
theta[3]:=t31;
end;
end;
end;
if abs(L1*sin(t22)+L2*sin(t22+t31)-zp)<0.03 then
begin
if abs(L1*cos(t22)+L2*cos(t22+t31)-sqrt(LL))<0.03 then
begin
testdelt := abs(theta[2]-t22)+abs(theta[3]-t31);
if testdelt<delt then
begin
delt:=testdelt;
theta[1]:=t1;
theta[2]:=t22;
theta[3]:=t31;
end;
end;
end;
if abs(L1*sin(t23)+L2*sin(t23+t32)-zp)<0.03 then
begin
if abs(L1*cos(t23)+L2*cos(t23+t32)-sqrt(LL))<0.03 then
begin
testdelt := abs(theta[2]-t23)+abs(theta[3]-t32);
if testdelt<delt then
begin
delt:=testdelt;
theta[1]:=t1;
theta[2]:=t23;
theta[3]:=t32;
end;
end;
end;
if abs(L1*sin(t24)+L2*sin(t24+t32)-zp)<0.03 then
begin
if abs(L1*cos(t24)+L2*cos(t24+t32)-sqrt(LL))<0.03 then
begin
testdelt := abs(theta[2]-t24)+abs(theta[3]-t32);
if testdelt<delt then
begin
delt:=testdelt;
theta[1]:=t1;
theta[2]:=t24;
theta[3]:=t32;
end;
end;
end;
end;
{****** This is the begining of the variable work ******}
{modify transformation matricies with variables}
REPEAT
for i:=1 to DOF do
begin
if LinkType[i]=1 then
begin {Prismatic}
Rorg[i,2]:=TR2org[i]*d[i];
Rorg[i,3]:=TR3org[i]*d[i];
end
else
begin {Revolute}
ctheta:=cos(theta[i]);
stheta:=sin(theta[i]);
R[i,1,1]:=ctheta;
R[i,1,2]:=-stheta;
R[i,2,1]:=TR21[i]*stheta;
R[i,2,2]:=TR22[i]*ctheta;
R[i,3,1]:=TR31[i]*stheta;
R[i,3,2]:=TR32[i]*ctheta;
end;
end;
i:=DOF+1;
if LinkType[i]=1 then
begin {Prismatic}
Rorg[i,2]:=TR2org[i]*d[i];
Rorg[i,3]:=TR3org[i]*d[i];
end;
{p 3}
{solve for P0[1]}
p0[1,1]:=Rorg[1,1]+0.5;
p0[1,2]:=Rorg[1,2]+0.5;
p0[1,3]:=Rorg[1,3]+0.5;
{solve for P0[2]}
for j:=1 to 3 do
begin
P0[2,j]:=R[1,j,1]*Rorg[2,1]+R[1,j,2]*Rorg[2,2]+
R[1,j,3]*Rorg[2,3]+Rorg[1,j]+0.5;
end;
{solve for 0T2 = 0T1 * 1T2}
for j:=1 to 3 do
begin
for k:=1 to 3 do
begin
R0[2,j,k]:=R[1,j,1]*R[2,1,k]+R[1,j,2]*R[2,2,k]+R[1,j,3]*R[2,3,k];
end;
R0org[2,j]:=R[1,j,1]*Rorg[2,1]+R[1,j,2]*Rorg[2,2]+
R[1,j,3]*Rorg[2,3]+Rorg[1,j];
end;
{p4}
if DOF > 2 then
begin
for i:=3 to DOF do
begin
{solve for P0[i,1-2-3], i=3,4,5,6,7...}
imo := i-1;
for j:=1 to 3 do
begin
P0[i,j]:=R0[imo,j,1]*Rorg[i,1]+R0[imo,j,2]*Rorg[i,2]+
R0[imo,j,3]*Rorg[i,3]+R0org[imo,j]+0.5;
end;
{solve for 0T(n) from 0T(n-1) * (n-1)T(n) when n=3,4,5..}
imo := i-1;
for j:=1 to 3 do
begin
for k:=1 to 3 do
begin
R0[i,j,k]:=R0[imo,j,1]*R[i,1,k]+R0[imo,j,2]*R[i,2,k]+
R0[imo,j,3]*R[i,3,k];
end;
R0org[i,j]:=R0[imo,j,1]*Rorg[i,1]+R0[imo,j,2]*Rorg[i,2]+
R0[imo,j,3]*Rorg[i,3]+R0org[imo,j];
end;
end;
end;
{p 5}
{solves for end effectors location}
for j:=1 to 3 do
begin
P0[DOF+1,j]:=R0[DOF,j,1]*Rorg[DOF+1,1]+R0[DOF,j,2]*Rorg[DOF+1,2]+
R0[DOF,j,3]*Rorg[DOF+1,3]+R0org[DOF,j];
end;
if ScreenMode=0 then {3-projections}
begin
{Erases Old robot arm in xy,xz,yz directions}
SetColor(black);
MoveTo(158,158);{x-y}
for i:=1 to DOF+1 do
begin
LineTo(158+trunc(OldP0[i,1]),158-trunc(OldP0[i,2]*0.75));
end;
if tracer=true then putpixel(trunc(158+OldP0[DOF+1,1]),
trunc(158-OldP0[DOF+1,2]*0.75),yellow);
MoveTo(473,128);{x-z}
for i:=1 to DOF+1 do
begin
LineTo(473+trunc(OldP0[i,1]),128-trunc(OldP0[i,3]*0.75));
end;
if tracer=true then putpixel(trunc(473+OldP0[DOF+1,1]),
trunc(128-OldP0[DOF+1,3]*0.75),yellow);
MoveTo(473,295);{y-z}
for i:=1 to DOF+1 do
begin
LineTo(473+trunc(OldP0[i,2]),295-trunc(OldP0[i,3]*0.75));
end;
if tracer=true then putpixel(trunc(473+OldP0[DOF+1,2]),
trunc(295-OldP0[DOF+1,3]*0.75),yellow);
{Plots out robot arm in xy,xz,yz directions}
MoveTo(158,158);{x-y}
for i:=1 to DOF+1 do
begin
SetColor(ColorArray[i]);
LineTo(158+trunc(P0[i,1]),158-trunc(P0[i,2]*0.75));
end;
MoveTo(473,128);{x-z}
for i:=1 to DOF+1 do
begin
SetColor(ColorArray[i]);
LineTo(473+trunc(P0[i,1]),128-trunc(P0[i,3]*0.75));
end;
MoveTo(473,295);{y-z}
for i:=1 to DOF+1 do
begin
SetColor(ColorArray[i]);
LineTo(473+trunc(P0[i,2]),295-trunc(P0[i,3]*0.75));
end;
end
else
begin {begin the projection of arm on one plane}
{erase axes}
SetColor(black);
circle(320+trunc(x[1]),150-trunc(x[3]*0.75),2);
circle(320+trunc(y[1]),150-trunc(y[3]*0.75),2);
circle(320+trunc(z[1]),150-trunc(z[3]*0.75),2);
MoveTo(320-trunc(x[1]),150+trunc(x[3]*0.75));
LineTo(320+trunc(x[1]),150-trunc(x[3]*0.75));
x[1] := 280; {x component of x axis}
x[2] := 0; {y component of x axis}
x[3] := 0; {z component of x axis}
MoveTo(320-trunc(y[1]),150+trunc(y[3]*0.75));
LineTo(320+trunc(y[1]),150-trunc(y[3]*0.75));
y[1] := 0; {x component of y axis}
y[2] := 110; {y component of y axis}
y[3] := 0; {z component of y axis}
MoveTo(320-trunc(z[1]),150+trunc(z[3]*0.75));
LineTo(320+trunc(z[1]),150-trunc(z[3]*0.75));
z[1] := 0; {x component of z axis}
z[2] := 0; {y component of z axis}
z[3] := 110; {z component of z axis}
{draw new axes}
SetColor(Darkgray);
xa := cyaw*x[1]-syaw*x[2];
ya := syaw*x[1]+cyaw*x[2];
x[1] := cRoll*xa+sRoll*x[3]; {x component of axis}
za := cRoll*x[3]-sRoll*xa;
x[2] := cPitch*ya-sPitch*za;
x[3] := sPitch*ya+cPitch*za;
{ xint := trunc(x[1]);
zint := trunc(x[3]);}
MoveTo(320-trunc(x[1]),150+trunc(x[3]*0.75));
LineTo(320+trunc(x[1]),150-trunc(x[3]*0.75));
xa := cyaw*y[1]-syaw*y[2];
ya := syaw*y[1]+cyaw*y[2];
y[1] := cRoll*xa+sRoll*y[3]; {x component of axis}
za := cRoll*x[3]-sRoll*xa;
y[2] := cPitch*ya-sPitch*za;
y[3] := sPitch*ya+cPitch*za;
MoveTo(320-trunc(y[1]),150+trunc(y[3]*0.75));
LineTo(320+trunc(y[1]),150-trunc(y[3]*0.75));
xa := cyaw*z[1]-syaw*z[2];
ya := syaw*z[1]+cyaw*z[2];
z[1] := cRoll*xa+sRoll*z[3]; {x component of axis}
za := cRoll*z[3]-sRoll*xa;
z[2] := cPitch*ya-sPitch*za;
z[3] := sPitch*ya+cPitch*za;
MoveTo(320-trunc(z[1]),150+trunc(z[3]*0.75));
LineTo(320+trunc(z[1]),150-trunc(z[3]*0.75));
setcolor(yellow);
circle(320+trunc(x[1]),150-trunc(x[3]*0.75),2);
setcolor(cyan);
circle(320+trunc(y[1]),150-trunc(y[3]*0.75),2);
setcolor(lightred);
circle(320+trunc(z[1]),150-trunc(z[3]*0.75),2);
cYaw:=cos(Yaw); {convert x,z coordinates}
sYaw:=sin(Yaw); {using Yaw,Roll,Pitch angles}
cRoll:=cos(Roll);
sRoll:=sin(Roll);
cPitch:=cos(Pitch);
sPitch:=sin(Pitch);
{erase old arm}
SetColor(black);
MoveTo(320,150);{x-y}
for i:=1 to DOF+1 do
begin
LineTo(320+trunc(OldP0[i,1]),150-trunc(OldP0[i,3]*0.75));
end;
if tracer=true then putpixel(trunc(320+OldP0[DOF+1,1]),
trunc(150-OldP0[DOF+1,3]*0.75),yellow);
{draw new arm}
MoveTo(320,150);
for i:=1 to DOF+1 do
begin
SetColor(ColorArray[i]);
xa := cyaw*P0[i,1]-syaw*P0[i,2];
ya := syaw*P0[i,1]+cyaw*P0[i,2];
P0[i,1] := cRoll*xa+sRoll*P0[i,3];
za := cRoll*P0[i,3]-sRoll*xa;
P0[i,2] := cPitch*ya-sPitch*za;
P0[i,3] := sPitch*ya+cPitch*za;
LineTo(320+trunc(P0[i,1]),150-trunc(P0[i,3]*0.75));
end;
end;
{transfers P0's to OldP0's so they can be deleted later}
for i:=1 to DOF+1 do
begin
OldP0[i,1]:=P0[i,1];
OldP0[i,2]:=P0[i,2];
OldP0[i,3]:=P0[i,3];
end;
if (RunMode=4) and (Angle=true) then
begin
xp := P0[DOF+1,1];
yp := P0[DOF+1,2];
zp := P0[DOF+1,3];
angle := false;
end;
If KeyPressed = true then
Begin
Ch := GetKey;
if Ch = #187 then{F1}
Begin
if LinkType[1] = 1 then
begin{prismatic}
d[1]:=d[1]+sign*dres[1];
end
else
begin{revolute}
theta[1]:=theta[1]+sign*thetares[1];
end;
angle := true;
End;
if Ch = #188 then{F2}
Begin
if LinkType[2] = 1 then
begin{prismatic}
d[2]:=d[2]+sign*dres[2];
end
else
begin{revolute}
theta[2]:=theta[2]+sign*thetares[2];
end;
angle := true;
End;
if Ch = #189 then{F3}
Begin
if LinkType[3] = 1 then
begin{prismatic}
d[3]:=d[3]+sign*dres[3];
end
else
begin{revolute}
theta[3]:=theta[3]+sign*thetares[3];
end;
angle := true;
End;
If Ch = #190 then{F4}
Begin
if LinkType[4] = 1 then
begin{prismatic}
d[4]:=d[4]+sign*dres[4];
end
else
begin{revolute}
theta[4]:=theta[4]+sign*thetares[4];
end;
angle := true;
End;
if Ch = #191 then{F5}
Begin
if LinkType[5] = 1 then
begin{prismatic}
d[5]:=d[5]+sign*dres[5];
end
else
begin{revolute}
theta[5]:=theta[5]+sign*thetares[5];
end;
angle := true;
End;
If Ch = #192 then{F6}
Begin
if LinkType[6] = 1 then
begin{prismatic}
d[6]:=d[6]+sign*dres[6];
end
else
begin{revolute}
theta[6]:=theta[6]+sign*thetares[6];
end;
angle := true;
End;
if Ch = #193 then{F7 - Change Direction of Links}
Begin
sign:=sign * -1;
SetColor(black);
OutTextXY(100,328,char(219));
SetColor(3+sign);
if sign = 1 then OutTextXY(100,328,char(24))
else OutTextXY(100,328,char(25));
End;
If Ch = #194 then{F8 - Freeze Numbers}
Begin
if FreezeNumbers = true then FreezeNumbers := false
else FreezeNumbers := true;
End;
If (Ch=#195)and(RunMode=2) then{F9 - Store d,theta in data file}
Begin
for i:=1 to DOF do
begin
if LinkType[i]=1 then Writeln(DataFile,d[i])
else Writeln(DataFile,theta[i]);
end;
End;
If Ch = #196 then{F10}
Begin
SetColor(black);
if Pauseit = true then
begin
Pauseit := false;
OutTextXY(124,328,BigBar);
SetColor(cyan);
OutTextXY(124,328,'Running');
end
else
begin
Pauseit := true;
OutTextXY(124,328,BigBar);
SetColor(yellow);
OutTextXY(124,328,'Paused');
end;
End;
{this should be Ch = #139}
If (Ch = #13)and(RunMode=3)and(RunReady=true) then{F11}
begin
if FirstTimeRun = false then
begin
Close(DataFile);
Assign(DataFile,InFile);
ReSet(DataFile);
Readln(DataFile,t);
RunIt:=true;
PauseIt:=false;
end
else
begin
RunIt:=true;
PauseIt:=false;
end;
end;
if Ch = #25 then {ctrl-y = alter yaw}
Begin
Yaw := Yaw+sign*0.034906; {alter yaw by 5 degrees}
end;
if Ch = #18 then {ctrl-r = alter roll}
Begin
Roll := Roll+sign*0.034906; {alter pitch by 5 degrees}
end;
if Ch = #16 then {ctrl-p = alter pitch}
Begin
Pitch := Pitch+sign*0.034906; {alter pitch by 5 degrees}
end;
if Ch = #173 then {alt-x = alter xp}
Begin
xp := xp+sign*2; {alter xp by 2}
InverseKin := true;
xyz :=1;
end;
if Ch = #149 then {alt-y = alter yp}
Begin
yp := yp+sign*2; {alter yp by 2}
InverseKin := true;
xyz :=2;
end;
if Ch = #172 then {alt-z = alter zp}
Begin
zp := zp+sign*2; {alter zp by 2}
InverseKin := true;
xyz :=3;
end;
if Ch = #20 then {ctrl-t = toggle tracer}
Begin
SetColor(black);
if Tracer = true then
begin
Tracer := false;
OutTextXY(124,328,BigBar);
SetColor(cyan);
OutTextXY(124,328,'Trace Off');
end
else
begin
Tracer := true;
OutTextXY(124,328,BigBar);
SetColor(yellow);
OutTextXY(124,328,'Trace On');
end;
End;
If Ch = #27 then{escape}
Begin
GetOut := true;
End;
End;
if (RunMode=4) and (InverseKin = true) then
Begin
atan(yp,xp,t1);
LL:=xp*xp+yp*yp;
{ c3:=(LL+zp*zp-L1*L1-L2*L2)/(2*L1*L2);}
c3:=(LL+zp*zp-16400)/(16000);
if (abs(c3)<1) or (abs(c3)=1) then
begin
s3:=sqrt(1-c3*c3);
atan(s3,c3,t31);
atan(-s3,c3,t32);
{ LLc:=(L1+L2*c3);
Ls:=L2*s3;}
LLc:=(100+80*c3);
Ls:=80*s3;
denom := LLc*LLc+Ls*Ls;
if denom<0.0001 then denom:=0.0001;
L := sqrt(LL);
c21:=(L*LLc+Ls*zp)/(denom); {+s3}
c22:=(L*LLc-Ls*zp)/(denom); {-s3}
if (abs(c21)<1)and(abs(c22)<1) then
begin
if outofrange=true then
begin
SetColor(black);
OutTextXY(124,328,BigBar);
SetColor(yellow);
OutTextXY(124,328,'In Range');
outofrange:=false;
end;
s21:=sqrt(1-c21*c21); {+s3}
s22:=sqrt(1-c22*c22); {-s3}
atan(s21,c21,t21);
atan(-s21,c21,t22);
atan(s22,c22,t23);
atan(-s22,c22,t24);
delt := 9999;
ct2 := cos(theta[2]);
st2 := sin(theta[2]);
ct3 := cos(theta[3]);
st3 := sin(theta[3]);
if abs(L1*s21+L2*sin(t21+t31)-zp)<0.3 then
begin
if abs(L1*c21+L2*cos(t21+t31)-L)<0.3 then
begin
testdelt := abs(ct2-c21)+
abs(st2-s21)+
abs(ct3-c3)+
abs(st3-s3);
{ SetColor(black);
OutTextXY(20,20,BigBar);
SetColor(yellow);
str(testdelt:8:4,tstring);
outtextxy(20,20,concat('delt1'+tstring));}
if testdelt<delt then
begin
delt:=testdelt;
theta[1]:=t1;
theta[2]:=t21;
theta[3]:=t31;
end;
end;
end;
if abs(-L1*s21+L2*sin(t22+t31)-zp)<0.3 then
begin
if abs(L1*c21+L2*cos(t22+t31)-L)<0.3 then
begin
testdelt := abs(ct2-c21)+
abs(st2+s21)+
abs(ct3-c3)+
abs(st3-s3);
if testdelt<delt then
begin
delt:=testdelt;
theta[1]:=t1;
theta[2]:=t22;
theta[3]:=t31;
end;
end;
end;
if abs(L1*s22+L2*sin(t23+t32)-zp)<0.3 then
begin
if abs(L1*c22+L2*cos(t23+t32)-L)<0.3 then
begin
testdelt := abs(ct2-c22)+
abs(st2-s22)+
abs(ct3-c3)+
abs(st3+s3);
{ SetColor(black);
OutTextXY(20,40,BigBar);
SetColor(yellow);
str(testdelt:8:4,tstring);
outtextxy(20,40,concat('delt3'+tstring));}
if testdelt<delt then
begin
delt:=testdelt;
theta[1]:=t1;
theta[2]:=t23;
theta[3]:=t32;
end;
end;
end;
if abs(-L1*s22+L2*sin(t24+t32)-zp)<0.3 then
begin
if abs(L1*c22+L2*cos(t24+t32)-L)<0.3 then
begin
{ SetColor(black);
OutTextXY(20,50,BigBar);
SetColor(yellow);
str(testdelt:8:4,tstring);
outtextxy(20,50,concat('delt4'+tstring));
delay(400);}
testdelt := abs(ct2-c22)+
abs(st2+s22)+
abs(ct3-c3)+
abs(st3+s3);
testdelt := abs(theta[2]-t24)+abs(theta[3]-t32);
if testdelt<delt then
begin
delt:=testdelt;
theta[1]:=t1;
theta[2]:=t24;
theta[3]:=t32;
end;
end;
end;
InverseKin := false;
end
else
begin
outofrange:=true;
end;
end
else
begin
outofrange:=true;
end;
if outofrange=true then
begin
SetColor(black);
OutTextXY(124,328,BigBar);
SetColor(yellow);
OutTextXY(124,328,'Too Far!!');
if xyz=1 then xp:=xp-sign*2;
if xyz=2 then yp:=yp-sign*2;
if xyz=3 then zp:=zp-sign*2;
xyz:=4;
end;
End;
if FreezeNumbers = false then
begin
for i:=1 to DOF+1 do
begin
for j:=1 to 3 do
begin
str(trunc(P0[i,j]),tstring);
SetColor(black);
if i<4 then
begin
OutTextXY(NumLoc[i,j]-24,328,block);
SetColor(yellow);
OutTextXY(NumLoc[i,j]-length(tstring)*8,328,tstring);
end
else
begin
OutTextXY(NumLoc[i,j]-24,339,block);
SetColor(yellow);
OutTextXY(NumLoc[i,j]-length(tstring)*8,339,tstring);
end;
end;
end;
end;
if (RunMode=3)and(Pauseit=false)and(RunIt=true) then
begin
if ReadyForNextPt=true then
begin
if not EOF(DataFile) then
begin
for i:=1 to DOF do
begin
Readln(DataFile,DorT[i]);
if LinkType[i]=1 then
begin{prismatic}
m:=DorT[i]-d[i];
if m>0 then DTsign[i]:=1 else DTsign[i]:=-1;
Reps[i]:=trunc(abs(m)/dres[i]);
if Reps[i]>0 then ReadyForNextPt:=false;
end
else
begin{revolute}
m:=DorT[i]-theta[i];
if m>0 then DTsign[i]:=1 else DTsign[i]:=-1;
Reps[i]:=trunc(abs(m)/thetares[i]);
if Reps[i]>0 then ReadyForNextPt:=false;
end;
end;
end
else
begin
RunReady:=true;
FirstTimeRun:=false;
RunIt:=false;
end;
end
else
begin
ReadyForNextPt:=true;
for i:=1 to DOF do
begin
if LinkType[i]=1 then
begin
if Reps[i]>0 then
begin
d[i]:=d[i]+DTsign[i]*dres[i];
dec(Reps[i]);
ReadyForNextPt:=false;
end
else
begin
d[i]:=DorT[i];
end;
end
else
begin
if Reps[i]>0 then
begin
theta[i]:=theta[i]+DTsign[i]*thetares[i];
dec(Reps[i]);
ReadyForNextPt:=false;
end
else
begin
theta[i]:=DorT[i];
end;
end;
end;
end;
end;
UNTIL GETOUT;
if (RunMode=2)or(RunMode=3) then
begin
{$I-}
close(DataFile);
{$I+}
end;
RestoreCrtMode;
End;
BEGIN
END.
{ the following are stored print routines which printout
calculations at various points in the program.}
{p1 for i:= 1 to 7 do
begin
setcolor(yellow);
str(alpha[i-1]:8:4,tstring);
outtextxy(20,20+30*i,tstring);
str(a[i-1]:8:4,tstring);
outtextxy(220,20+30*i,tstring);
str(d[i]:8:4,tstring);
outtextxy(320,20+30*i,tstring);
str(theta[i]:8:4,tstring);
outtextxy(460,20+30*i,tstring);
end;
repeat until keypressed;
setfillstyle(solidfill,0);
bar(0,0,640,320);}
{p2 for i:= 1 to 7 do
begin
setcolor(yellow);
str(alpha[i-1]:8:4,tstring);
outtextxy(20,20+30*i,tstring);
str(a[i-1]:8:4,tstring);
outtextxy(220,20+30*i,tstring);
str(d[i]:8:4,tstring);
outtextxy(320,20+30*i,tstring);
str(theta[i]:8:4,tstring);
outtextxy(460,20+30*i,tstring);
end;
repeat
begin
delay(500);
end;
until keypressed;
setfillstyle(solidfill,0);
bar(0,0,640,320);
outtextxy(600,1,'linktype');}
{p3 outtextxy(200,1,'mod matrix');
for i:= 1 to DOF+1 do
begin
setcolor(yellow);
str(i,tstring);
outtextxy(10,10,tstring);
for k:=1 to 3 do
begin
for j:= 1 to 3 do
begin
str(R[i,j,k]:8:4,tstring);
outtextxy(2+140*k,20+30*j,tstring);
end;
str(Rorg[i,k]:8:4,tstring);
outtextxy(575,20+30*k,tstring);
end;
repeat
begin
delay(1000);
end;
until keypressed;
setfillstyle(solidfill,0);
bar(0,0,640,320);
end;}
{p4 setcolor(yellow);
str(i,tstring);
outtextxy(10,1,concat('T0[2]'+tstring));
for k:=1 to 3 do
begin
for j:= 1 to 3 do
begin
str(R0[2,j,k]:8:4,tstring);
outtextxy(2+140*k,20+30*j,tstring);
end;
str(R0org[2,k]:8:4,tstring);
outtextxy(575,20+30*k,tstring);
end;
repeat
begin
delay(1000);
end;
until keypressed;
setfillstyle(solidfill,0);
bar(0,0,640,320);}
{p5 delay(1000);
setcolor(yellow);
str(i,tstring);
outtextxy(10,1,concat('T0[3]'+tstring));
for k:=1 to 3 do
begin
for j:= 1 to 3 do
begin
str(R0[3,j,k]:8:4,tstring);
outtextxy(2+140*k,20+30*j,tstring);
end;
str(R0org[3,k]:8:4,tstring);
outtextxy(575,20+30*k,tstring);
end;
repeat
begin
delay(1000);
end;
until keypressed;
setfillstyle(solidfill,0);
bar(0,0,640,320);}{this contains the procedure for the help screens}
UNIT RoboAid;
INTERFACE
USES DOS,RoboDir, LISTTTT5, CRT, FASTTTT5, WINTTT5,MenuTTT5,Graph, IOTTT5,
KeyTTT5, Strnttt5, MiscTTT5;
Type LongIntArrayType = ARRAY[1..9] of longint;
IntArrayType = ARRAY[1..9] of integer;
RealArrayType = ARRAY[1..9] of real;
StrArrayType = ARRAY[1..15] of string[80];
Procedure Help(HelpNum:integer);
{==========================================================================
HELP\ERROR PART OF THE PROGRAM
===========================================================================}
IMPLEMENTATION
{********************************************************************}
Procedure Help(HelpNum:integer);
Type StrArrayType = ARRAY[1..15] of string;
Var HelpList: StrArrayType;
Y,MessageMax : integer;
begin
if HelpNum = 1 then begin
HelpList[1]:='Help Screen for Robot DataBase';
HelpList[2]:='A. To Enter New Robot : Enter information, then press F4';
HelpList[3]:='';
HelpList[4]:='B. To Find Existing Robot''s File: Enter any part of robot''s File';
HelpList[5]:=' Press F8';
HelpList[6]:='';
HelpList[7]:='C. To Select Robot for Session Run: Find Robot File via B.';
HelpList[8]:=' Press F10';
HelpList[9]:='';
HelpList[10]:='D. To Delete Robot''s File: Find Robot''s File via B.';
HelpList[11]:=' Press F6';
HelpList[12]:='';
HelpList[13]:='E. To Exit DataBase Without Files : Press the <Esc> key';
HelpList[14]:=' ';
HelpList[15]:='F. To Clear Screen : Press F2';
MessageMax:= 14;
end;
if HelpNum = 2 then begin
HelpList[1]:='Help Screen for Play Robot Mode';
HelpList[2]:='A. Call up robot using database';
HelpList[3]:='';
HelpList[4]:='B. Enter <Play Robot>';
HelpList[5]:='';
HelpList[6]:='C. Function Keys: F1-F6 alter variable link parameters';
HelpList[7]:=' F7 allow F1-F6 to in/decrease parameters';
HelpList[8]:=' F8 toggles (x,y,z) display on/off';
HelpList[9]:='';
HelpList[10]:='D. To exit -- type the <esc> key';
HelpList[11]:='';
HelpList[12]:='E. Note: ctrl-t puts a tracer on the robot hand';
HelpList[13]:='';
HelpList[14]:='';
HelpList[15]:='';
MessageMax:= 14;
end;
if HelpNum = 3 then begin
HelpList[1]:='Help Screen for Teach Robot Mode';
HelpList[2]:='A. Call up robot using database';
HelpList[3]:='';
HelpList[4]:='B. Enter <Teach Robot> - type in output file name';
HelpList[5]:='';
HelpList[6]:='C. Function Keys: F1-F6 alter variable link parameters';
HelpList[7]:=' F7 allow F1-F6 to in/decrease parameters';
HelpList[8]:=' F8 toggles (x,y,z) display on/off';
HelpList[9]:=' F9 saves variable parameters for current pos.';
HelpList[10]:='';
HelpList[11]:='D. To exit -- type the <esc> key';
HelpList[12]:='';
HelpList[13]:='E. Note: ctrl-t puts a tracer on the robot hand';
HelpList[14]:='';
HelpList[15]:='';
MessageMax:= 14;
end;
if HelpNum = 4 then begin
HelpList[1]:='Help Screen for Run Robot Mode';
HelpList[2]:='A. Call up robot using database';
HelpList[3]:='';
HelpList[4]:='B. Enter <Run Robot> - find a input file';
HelpList[5]:='';
HelpList[6]:='C. Type <enter> to "run" robot';
HelpList[7]:='';
HelpList[8]:='D. Type <esc> to exit ';
HelpList[9]:='';
HelpList[10]:='E. Note: ctrl-t puts a tracer on the robot hand';
HelpList[11]:='';
HelpList[12]:='';
HelpList[13]:='';
HelpList[14]:='';
HelpList[15]:='';
MessageMax:= 14;
end;
if HelpNum = 5 then begin
HelpList[1]:='Help Screen for Reverse Kinematics of RRR-3D Robot';
HelpList[2]:='A. Call up robot named "RRR-3D" using database';
HelpList[3]:='';
HelpList[4]:='B. Enter <RRR Rev.Kin.>';
HelpList[5]:='';
HelpList[6]:='C. Alt Keys: Alt-x moves robot hand in x direction';
HelpList[7]:=' Alt-y moves robot hand in y direction';
HelpList[8]:=' Alt-z moves robot hand in z direction';
HelpList[9]:=' Note: direction specified by F7';
HelpList[10]:='';
HelpList[11]:='D. Note: F1-F6 also work - see help for play robot';
HelpList[12]:='';
HelpList[13]:='E. Type <esc> to exit.';
HelpList[14]:='';
HelpList[15]:='F. Note: ctrl-t put a tracer on the robot hand';
MessageMax:= 15;
end;
if HelpNum = 6 then begin
HelpList[1]:='Help Screen for Screen Mode';
HelpList[2]:='A. 3 projections show robot in xy,xz,and yz planes.';
HelpList[3]:='';
HelpList[4]:='B. 1 projection shows robot as seen by viewer';
HelpList[5]:='';
HelpList[6]:=' - ctrl-p rotates pitch axis ';
HelpList[7]:=' - ctrl-y rotates yaw axis';
HelpList[8]:=' - ctrl-r rotates roll axis';
HelpList[9]:=' Note: direction specified by F7';
HelpList[10]:='';
HelpList[11]:='C. Other function and Alt keys work in both modes';
HelpList[12]:='';
HelpList[13]:='D. Type <esc> to exit.';
HelpList[14]:='';
HelpList[15]:='';
MessageMax:= 14;
end;
MkWin(1,2,80,24,blue,lightgray,2);
WriteBetween(5,75,4,black,Lightgray,HelpList[1]);
For Y:=2 to MessageMax do begin
WriteAt(10,Y+5,blue,Lightgray,HelpList[Y]);
end;
WriteBetween(5,75,21,black,Lightgray,'Press the <Return> or <Enter> to return');
Repeat
Begin
End;
Until KeyPressed;
RmWin;
end;
Begin
End.{this unit creates the cover page}
UNIT ROBOCOV;
INTERFACE
Uses CRT,DOS,Graph,FastTTT5,WinTTT5,StrnTTT5,KeyTTT5,
ReadTTT5,MiscTTT5;
var
GrMode,GrDriver,GrError : integer;
{ Procedure InitializeGraphics;}
Procedure CoverPage;
(********************************************************************)
IMPLEMENTATION
Procedure CoverPage;
Begin
SetVisualPage(1);
SetActivePage(0);
Setfillstyle(1,DarkGray);
Bar(0,0,639,479);
SetTextStyle(1,HorizDir,5);
SetUserCharSize(1,1,6,4);
SetColor(Yellow);
OutTextXY(trunc(320-TextWidth('Robot Simulator')/2),60,'Robot Simulator');
SetUserCharSize(1,1,1,1);
SetColor(Cyan);
SetTextStyle(1,HorizDir,1);
OutTextXY(trunc(320-TextWidth('Type any key to start')/2),300,'Type any key to start');
SetTextStyle(0,HorizDir,1);
SetLineStyle(SolidLn,0,ThickWidth);
setcolor(magenta);
Moveto(320,280);
Lineto(450,200);
setcolor(blue);
Lineto(400,170);
setcolor(cyan);
Lineto(270,130);
setcolor(green);
Lineto(240,160);
SetVisualPage(0);
repeat
Begin
End;
until KeyPressed;
RestoreCrtMode;
End;
BEGIN
END.{this unit takes care of the manipulators database files}UNIT RoboDbas;
INTERFACE
USES DOS{,BioDir}, LISTTTT5, CRT, FASTTTT5, WINTTT5,MenuTTT5,Graph,
{BIOaid,} IOTTT5, KeyTTT5, Strnttt5, MiscTTT5, ReadTTT5{,Printer};
Const
MaxClients = 999;
NumOfFields = 30;
DbaseFileName = 'RoboFile.DBF';
TemporaryName = 'tempor.dbf';
Type
DbaseRecordType = Record
dRoboName : string;
dDOF : string;
dalpha1 : string;
da1 : string;
dd1 : string;
dtheta1 : string;
dalpha2 : string;
da2 : string;
dd2 : string;
dtheta2 : string;
dalpha3 : string;
da3 : string;
dd3 : string;
dtheta3 : string;
dalpha4 : string;
da4 : string;
dd4 : string;
dtheta4 : string;
dalpha5 : string;
da5 : string;
dd5 : string;
dtheta5 : string;
dalpha6 : string;
da6 : string;
dd6 : string;
dtheta6 : string;
dalpha7 : string;
da7 : string;
dd7 : string;
dtheta7 : string;
End;
DisplayArrayType = Array[1..100] of string[70];
FileRecordSeekIndexType = Array[0..100] of integer;
SearchType = (sRoboName,sDOF);
Display_Cols = record
TextF : Byte;
Back : Byte;
MsgF : Byte;
MsgB : Byte;
FieldHiF : Byte;
FieldHiB : Byte;
FieldLoF : Byte;
FieldLoB : Byte;
BoxF : Byte;
HeadF : Byte;
end;
DbaseFileType = File of DbaseRecordType;
DbaseArrayType = Array [1..MaxClients] of string[40];
DbaseIntArrayType = Array [1..999] of integer;
VAR
Dir,LongDir : string[150];
NumofFiles,SesNum,i : integer;
SearchFor : string[80];
PrintArray : DbaseIntArrayType;
SearchField : SearchType;
Position : integer;
Rec,ExistRec,Selection : DbaseRecordType;
DbaseFile : DbaseFileType;
rndm : integer;
loop : longint;
Screen_Cols : Display_Cols;
ReturnCodeDB1 : Integer;
EscapeFound,continue,continue2 : boolean;
LastChar : Char;
NoneSelect,NewRec,FilesClosed : Boolean;
KeepPosition : integer;
MemStart : pointer;
DBCode : byte;
Procedure Set_Screen_General;
Procedure Set_Fields;
Procedure ZeroRec;
Procedure GetRecord(DBcode : byte; Var Rec: DbaseRecordType;Var Position : integer);
Function Same(Rec, DeadRec : DbaseRecordType) : Boolean;
Procedure SaveRecord;
Procedure DeleteRecord(DeadRec:DbaseRecordType);
Procedure EasyDeleteRecord(DeadRec:DbaseRecordType);
Procedure MenuBack;
Procedure DODbase(VAR RoboName:string; VAR DOF:integer; VAR alpha1,a1,d1,theta1,alpha2,a2,d2,theta2,
alpha3,a3,d3,theta3,alpha4,a4,d4,theta4,alpha5,a5,d5,theta5,
alpha6,a6,d6,theta6,alpha7,a7,d7,theta7:real);
Procedure DBPutBottomLine(DBcode : byte);
Procedure Init_Vars;
Procedure SetDisplayRecord;
{==========================================================================
DATABASE PART OF THE PROGRAM
===========================================================================}
IMPLEMENTATION
Procedure SetDisplayRecord;
BEGIN
With LTTT DO BEGIN
X := 3;
Y := 6;
LeftSide := true;
BoxType := 2;
Lines := 15;
TopicWidth := 71;
BoxFCol := cyan;{black}
BoxBCol := cyan;{black}
CapFCol := blue;
BacCol := cyan;{black}
NorFcol := Blue;
HiFCol := lightblue;
HiBCol := Blue;
AllowToggle := False;
LeftChar := Char(026);
RightChar := Char(027);
End;
End;{SetDisplayRecord;}
{**************************************************************************}
Procedure Set_Screen_General;
begin
ClearText(1,1,80,25,white,black);
With Screen_Cols do
begin
FBox(1,1,80,21,BoxF,Back,2);
WriteCenter(1,TextF,back,'Robot Simulation Database');
WriteAt(3,4,TextF,back,'Robot Name');
WriteAt(31,4,TextF,back,'DOF');
WriteAt(39,6,TextF,back,'Link Parameters');
WriteAT(3,8,TextF,back,'i');
WriteAT(7,8,TextF,back,'alpha[i-1]');
WriteAT(21,8,TextF,back,'a[i-1]');
WriteAT(31,8,TextF,back,'d[i]');
WriteAT(39,8,TextF,back,'theta[i]');
WriteAT(3,9,TextF,back,'1');
WriteAT(3,10,TextF,back,'2');
WriteAT(3,11,TextF,back,'3');
WriteAT(3,12,TextF,back,'4');
WriteAT(3,13,TextF,back,'5');
WriteAT(3,14,TextF,back,'6');
WriteAT(3,15,TextF,back,'7');
end;
end; {of proc Set_Screen}
{************************************************************************}
Procedure Set_Fields;
var I : Integer;
begin
Create_Tables(1);
Activate_Table(1);
With Screen_Cols do
Define_Colors(FieldHiF, FieldHiB, FieldLoF, FieldLoB, MsgF, MsgB);
Allow_Esc(True);
Create_Fields(30);
Add_Field(1, 30, 2,30, 2, 14,4);{robot name}
Add_Field(2, 1, 3, 1, 3, 37,4);{DOF}
Add_Field(3, 2, 4, 2, 4, 9,9);{alpha1}
Add_Field(4, 3, 5, 3, 5, 22,9);{a1}
Add_Field(5, 4, 6, 4, 6, 31,9);{d1}
Add_Field(6, 5, 7, 5, 7, 40,9);{theta1}
Add_Field(7, 6, 8, 6, 8, 9,10);{alpha2}
Add_Field(8, 7, 9, 7, 9, 22,10);{a2}
Add_Field(9, 8,10, 8,10, 31,10);{d2}
Add_Field(10, 9,11, 9,11, 40,10);{theta2}
Add_Field(11, 10,12,10,12, 9,11);{alpha3}
Add_Field(12, 11,13,11,13, 22,11);{a3}
Add_Field(13, 12,14,12,14, 31,11);{d3}
Add_Field(14, 13,15,13,15, 40,11);{theta3}
Add_Field(15, 14,16,14,16, 9,12);{alpha4}
Add_Field(16, 15,17,15,17, 22,12);{a4}
Add_Field(17, 16,18,16,18, 31,12);{d4}
Add_Field(18, 17,19,17,19, 40,12);{theta4}
Add_Field(19, 18,20,18,20, 9,13);{alpha5}
Add_Field(20, 19,21,19,21, 22,13);{a5}
Add_Field(21, 20,22,20,22, 31,13);{d5}
Add_Field(22, 21,23,21,23, 40,13);{theta5}
Add_Field(23, 22,24,22,24, 9,14);{alpha6}
Add_Field(24, 23,25,23,25, 22,14);{a6}
Add_Field(25, 24,26,24,26, 31,14);{d6}
Add_Field(26, 25,27,25,27, 40,14);{theta6}
Add_Field(27, 26,28,26,28, 9,15);{alpha7}
Add_Field(28, 27,29,27,29, 22,15);{a7}
Add_Field(29, 28,30,28,30, 31,15);{d7}
Add_Field(30, 29, 1,29, 1, 40,15);{theta7}
String_Field(1, rec.dRoboName,'!!!!!!!!');
String_Field(2, rec.dDOF,'#');
String_Field(3, rec.dalpha1,'######');
String_Field(4, rec.da1,'###');
String_Field(5, rec.dd1,'###');
String_Field(6, rec.dtheta1,'######');
String_Field(7, rec.dalpha2,'######');
String_Field(8, rec.da2,'###');
String_Field(9, rec.dd2,'###');
String_Field(10, rec.dtheta2,'######');
String_Field(11, rec.dalpha3,'######');
String_Field(12, rec.da3,'###');
String_Field(13, rec.dd3,'###');
String_Field(14, rec.dtheta3,'######');
String_Field(15, rec.dalpha4,'######');
String_Field(16, rec.da4,'###');
String_Field(17, rec.dd4,'###');
String_Field(18, rec.dtheta4,'######');
String_Field(19, rec.dalpha5,'######');
String_Field(20, rec.da5,'###');
String_Field(21, rec.dd5,'###');
String_Field(22, rec.dtheta5,'######');
String_Field(23, rec.dalpha6,'######');
String_Field(24, rec.da6,'###');
String_Field(25, rec.dd6,'###');
String_Field(26, rec.dtheta6,'######');
String_Field(27, rec.dalpha7,'######');
String_Field(28, rec.da7,'###');
String_Field(29, rec.dd7,'###');
String_Field(30, rec.dtheta7,'######');
Add_Message(1,0,20,'A file will be created under this name');
Add_Message(2,0,20,'Number of Degrees of Freedom for the system');
Add_Message(3,0,20,'Link Twist [+/- 180 deg]');
Add_Message(4,0,20,'Link Length [cm]');
Add_Message(5,0,20,'Link Offset [cm], 999 for variable');
Add_Message(6,0,20,'Joint Angle [+/- 180 deg], 999 for variable');
Add_Message(7,0,20,'Link Twist [+/- 180 deg]');
Add_Message(8,0,20,'Link Length [cm]');
Add_Message(9,0,20,'Link Offset [cm], 999 for variable');
Add_Message(10,0,20,'Joint Angle [+/- 180 deg], 999 for variable');
Add_Message(11,0,20,'Link Twist [+/- 180 deg]');
Add_Message(12,0,20,'Link Length [cm]');
Add_Message(13,0,20,'Link Offset [cm], 999 for variable');
Add_Message(14,0,20,'Joint Angle [+/- 180 deg], 999 for variable');
Add_Message(15,0,20,'Link Twist [+/- 180 deg]');
Add_Message(16,0,20,'Link Length [cm]');
Add_Message(17,0,20,'Link Offset [cm], 999 for variable');
Add_Message(18,0,20,'Joint Angle [+/- 180 deg], 999 for variable');
Add_Message(19,0,20,'Link Twist [+/- 180 deg]');
Add_Message(20,0,20,'Link Length [cm]');
Add_Message(21,0,20,'Link Offset [cm], 999 for variable');
Add_Message(22,0,20,'Joint Angle [+/- 180 deg], 999 for variable');
Add_Message(23,0,20,'Link Twist [+/- 180 deg]');
Add_Message(24,0,20,'Link Length [cm]');
Add_Message(25,0,20,'Link Offset [cm], 999 for variable');
Add_Message(26,0,20,'Joint Angle [+/- 180 deg], 999 for variable');
Add_Message(27,0,20,'Link Twist [+/- 180 deg]');
Add_Message(28,0,20,'Link Length [cm]');
Add_Message(29,0,20,'Link Offset [cm], 999 for variable');
Add_Message(30,0,20,'Joint Angle [+/- 180 deg], 999 for variable');
end; {of proc Set_Fields}
Procedure ZeroRec;
BEGIN
WITH Rec DO BEGIN
dRoboName := '';
dDOF := '';
dalpha1 := ' 0';
da1 := ' 0';
dd1 := ' 0';
dtheta1 := ' 0';
dalpha2 := ' 0';
da2 := ' 0';
dd2 := ' 0';
dtheta2 := ' 0';
dalpha3 := ' 0';
da3 := ' 0';
dd3 := ' 0';
dtheta3 := ' 0';
dalpha4 := ' 0';
da4 := ' 0';
dd4 := ' 0';
dtheta4 := ' 0';
dalpha5 := ' 0';
da5 := ' 0';
dd5 := ' 0';
dtheta5 := ' 0';
dalpha6 := ' 0';
da6 := ' 0';
dd6 := ' 0';
dtheta6 := ' 0';
dalpha7 := ' 0';
da7 := ' 0';
dd7 := ' 0';
dtheta7 := ' 0';
END;
END;
{************************************************************************}
Procedure Init_Vars;
{}
begin
With Screen_Cols do
If BaseOfScreen = $B800 then {color}
begin
TextF := Black;
Back := LightGray;
MsgF := Black;
MsgB := LightGray;
FieldHiF := Brown;
FieldHiB := Cyan;
FieldLoF := Blue;
FieldLoB := LightGray;
BoxF := Blue;
HeadF := Blue;
end
else
begin
TextF := lightgray;
Back := black;
MsgF := white;
MsgB := black;
FieldHiF := black;
FieldHiB := lightgray;
FieldLoF := white;
FieldLoB := black;
BoxF := white;
HeadF := white;
end;
end; {of proc Init}
{***********************************************************************}
Procedure InputSearchString(Var Rec:DbaseRecordType);
{ This procedure uses the database Entry Screen for entry of search data }
{$F+}
begin
With Table[CurrentTable]^.ITTT do
ErrorLine := 21;
Process_Input(1,ReturnCodeDB1,LastChar);
end;{ of InputSearchStringEdit }
{*************************************************************************}
{ The following procedure selects a particular record from the DB-file }
Procedure GetRecord(DBcode : byte;
Var Rec: DbaseRecordType;
Var Position : integer);
Type
SearchArgumentRecordType = Record
Field : SearchType;
Txt : String[80];
End;
VAR
FileRecordSeekIndex : FileRecordSeekIndexType;
DbaseFile : DbaseFileType;
loop,recnum : longint;
DisplayArray : DisplayArrayType; { Holds the display Window strings. }
LastRecordAdded,n : Integer; { The last record Added to the Display Array }
SearchArguments : Array[1..30] of SearchArgumentRecordType;
ArgumentNumber : Integer;
SearchFor : string[80];
SearchField : SearchType;
TemporaryRec : DbaseRecordType;
NotFound : boolean;
{***************************************************************************}
Procedure AddRec(loop: integer);
Var Istr,Lstr,Fstr,Mstr,ASstr,Acstr,SSNstr : string[100];
BEGIN
Inc(LastRecordAdded);
PrintArray[LastRecordAdded]:=loop;
With Rec Do Begin
DisplayArray[LastRecordAdded] := Concat(' Robot Name: '+dRoboName+
' Degrees of Freedom: '+dDOF);
End;
End;{AddRec(loop)}
{***************************************************************************}
Procedure SelectMade;
VAR LongDir : string[150];
continue,continue2 : boolean;
SesNum : integer;
BEGIN
If L_Pick < n then
begin
Position := FileRecordSeekIndex[L_pick];
Seek(DbaseFile,Position);
Read(DbaseFile,rec);
end
else
begin
IF L_Pick = n THEN
BEGIN
ZeroRec;
END;
end;
END;{Select Made}
{**************************************************************************}
Procedure SelectFromList;
BEGIN
MkWin(1,1,80,25,white,cyan,2);
n:=LastRecordAdded+1;
DisplayArray[n]:=' <<<< SELECT THIS FOR NONE OF THE ABOVE >>>>';
WriteCenter(23,black,cyan,'Use the arrow keys to select record');
WriteCenter(24,black,cyan,'Press <Return> to pick record');
WriteCenter(2,yellow,cyan,'Matching Records');
Show_List(DisplayArray,70,n);
RmWin;
End;{Select From List}
{**************************************************************************}
Procedure ReadRecords;
VAR
CurPlace,NextPlace,RecNum,Temp : longint;
Temp1 : real;
loop,loop1 : integer;
BEGIN
For loop := 1 to 100 DO Begin
DisplayArray[loop] := ''; {Empty the display Array }
{ FileRecordSeekIndex[loop] := -1;}
End;
FileRecordSeekIndex[0] := -1;
LastRecordAdded := 0; { Zero the last record Added }
Reset(DbaseFile);
RecNum := FileSize(DbaseFile);
WriteAT(15,24,cyan,black,'Total Records: ');
WriteAT(30,24,yellow,black,int_to_str(RecNum));
For loop := 0 to RecNum-1 Do Begin
WriteAt(55,24,cyan,black,'Records Read: ');
WriteAt(69,24,yellow,black,int_to_str(loop+1));
Seek(DbaseFile,loop);
Read(DbaseFile,rec);
NotFound:=true;
For loop1 := 1 to ArgumentNumber Do Begin
SearchFor := SearchArguments[loop1].Txt;
SearchField := SearchArguments[loop1].Field;
With Rec DO BEGIN
CASE SearchField OF
sRoboName : Begin
IF (SearchFor = '*')and(NotFound) Then Begin
FileRecordSeekIndex[LastRecordAdded+1] := loop;
AddRec(loop);
NotFound:=false;
END ELSE
IF (dRoboName = SearchFor)and(NotFound) Then BEGIN
FileRecordSeekIndex[LastRecordAdded+1] := loop;
AddRec(loop);
NotFound:=false;
END;
End;
sDOF : Begin
IF (dDOF = SearchFor)and(NotFound) Then BEGIN
FileRecordSeekIndex[LastRecordAdded+1] := loop;
AddRec(loop);
NotFound:=false;
END;
End;
End;{Case}
End;{With}
End;{For}
End;{For loop}
WriteAt(5,24,black,black,' ');
End;{Read Records}
{*************************************************************************}
Procedure AssembleSearchArguments;
VAR
loop,LastArgument : integer;
BEGIN
LastArgument := 1;
For Loop := 1 to 17 DO
Begin
SearchArguments[loop].Txt := '';
SearchArguments[loop].Field := sRoboName;
End;
With Rec DO Begin
IF dRoboName <> '' THEN
Begin
SearchArguments[LastArgument].Txt := dRoboName;
SearchArguments[LastArgument].Field := sRoboName;
Inc(LastArgument);
End;
IF dDOF <> '' THEN
Begin
SearchArguments[LastArgument].Txt := dDOF;
SearchArguments[LastArgument].Field := sDOF;
Inc(LastArgument);
End;
ArgumentNumber := LastArgument - 1;
End;{With}
End;{Assign Search Arguments }
Begin {Main part of GetRecord}
IF (FSearch(DbaseFileName,'c:\')<>'') Then Begin{ if the file exists }
AssembleSearchArguments; { get all the search arguments }
Assign(DbaseFile,DbaseFileName); { assign file name }
ReadRecords; { read in the new records }
If LastRecordAdded > 0 Then
BEGIN { if there are matches }
SelectFromList; { display scroll list and get selection }
SelectMade; { place the selection in the global var }
Position := FileRecordSeekIndex[L_pick];
END
ELSE
BEGIN
MkWin(20,8,60,15,white,cyan,2);
WriteCenter(10,yellow,cyan,'No Matching Robot Files');
WriteCenter(13,black,cyan,'Type Any Key to Continue');
Repeat
begin
end;
Until KeyPressed;
RmWin;
ZeroRec;
END;
Close(DbaseFile); { close the file }
End else begin
MkWin(20,8,60,15,white,cyan,2);
WriteCenter(10,yellow,cyan,'DataBase File Not Found');
WriteCenter(11,yellow,cyan,'Program will create an empty file');
WriteCenter(13,black,cyan,'Type Any Key to Continue');
Repeat
begin
end;
Until KeyPressed;
RmWin;
Assign(DbaseFile,DbaseFileName);
Rewrite(DbaseFile);
Close(DbaseFile);
END;
End; { of procedure Get Record }
{***************************************************************************}
Function Same(Rec, DeadRec : DbaseRecordType) : Boolean;
Begin
Same := (Rec.dRoboName=DeadRec.dRoboName)
AND (Rec.dDOF=DeadRec.dDOF);
End;
{***************************************************************************}
Procedure DeleteRecord(DeadRec : DbaseRecordType);
Var
RecNum : Integer;
loop : integer;
OutFile : DbaseFileType;
Results : Integer;
ch : string[80];
{---------------------------------------------------------------------------}
Procedure ReadRecordsIn;
Var
Rec : DbaseRecordType;
loop,OutCounter : integer;
Begin
OutCounter := 0;
WriteCenter(24,yellow,black,'Deleting Record - Please Wait ');
For loop := 0 to RecNum-1 Do Begin
SEEK(DbaseFile,loop);
Read(DbaseFile,Rec);
If Not Same(Rec,DeadRec) Then Begin
Seek(OutFile,OutCounter);
Write(OutFile,Rec);
Inc(OutCounter);
End;
End;
End;{ of procedure write records out }
{-------------------------------------------------------------------------}
Begin { Main Part of delete record }
Assign(DbaseFile,DbaseFileName);
Assign(OutFile,TemporaryName);
Reset(DbaseFile);
Rewrite(OutFile);
FilesClosed:=false;
RecNum := FileSize(DbaseFile);
If ReturnCodeDB1 >= 0 Then
Begin
Position := KeepPosition;
If Position >= 0 Then
Begin { If the record is found }
DeadRec := Rec;
GotoXY(15,24);TextColor(Yellow);Write('Do You Really Want To Delete This Robot File? <Y> <N> ');
Ch := GetKey;
If Ch = #027 Then exit;
WriteAt(5,24,black,black,' ');
If ((ch[1]='y')OR(ch[1]='Y')) Then
Begin
ReadRecordsIn;
Close(DbaseFile);
Close(OutFile);
Erase(DbaseFile);
Rename(OutFile,DbaseFileName);
FilesClosed := true;
End;
End Else Begin
MkWin(20,8,60,15,white,cyan,2);
WriteCenter(10,yellow,cyan,'Robot File is not in current dir.');
WriteCenter(13,black,cyan,'Type Any Key to Continue');
Repeat
begin
end;
Until KeyPressed;
RmWin;
End;
End;{ Check escape in selection }
if FilesClosed = false then
begin
Close(DbaseFile);
Close(OutFile);
end;
FilesClosed := true;
End;{ of procedure Delete Record }
{***************************************************************************}
Procedure EasyDeleteRecord(DeadRec : DbaseRecordType);
Var
RecNum : Integer;
loop : integer;
OutFile : DbaseFileType;
Results : Integer;
ch : string[80];
Procedure EasyReadRecordsIn;
Var
Rec : DbaseRecordType;
loop,OutCounter : integer;
Begin
OutCounter := 0;
WriteCenter(24,yellow,black,'Deleting Old Record - Please Wait ');
For loop := 0 to RecNum-1 Do Begin
SEEK(DbaseFile,loop);
Read(DbaseFile,Rec);
if (Rec.dRoboName<>DeadRec.dRoboName) then
Begin
Seek(OutFile,OutCounter);
Write(OutFile,Rec);
Inc(OutCounter);
End;
End;
WriteCenter(24,black,black,' ');
End;{ of procedure write records out }
Begin { Main Part of easy delete record }
Assign(DbaseFile,DbaseFileName);
Assign(OutFile,TemporaryName);
Reset(DbaseFile);
Rewrite(OutFile);
FilesClosed:=false;
RecNum := FileSize(DbaseFile);
If ReturnCodeDB1 >= 0 Then Begin
Position := KeepPosition;
If Position >= 0 Then Begin { If the record is found }
DeadRec := Rec;
Begin
EasyReadRecordsIn;
Close(DbaseFile);
Close(OutFile);
FilesClosed:=true;
Erase(DbaseFile);
Rename(OutFile,DbaseFileName);
End;
End Else Begin
MkWin(20,8,60,15,white,cyan,2);
WriteCenter(10,yellow,cyan,'Old Record is not in the File');
WriteCenter(13,black,cyan,'Type Any Key to Continue');
Repeat
begin
end;
Until KeyPressed;
RmWin;
End;
End;{ Check escape in selection }
if FilesClosed = false then
begin
Close(DbaseFile);
Close(OutFile);
end;
FilesClosed := true;
End;{ of procedure Easy Delete Record }
{*************************************************************************}
Procedure SaveRecord;
Var Place,loop,OneTheSamePos:integer;
OneTheSame,OverWrite:boolean;
DeadRec : DbaseRecordType;
SamesName : string[80];
Begin
if (Rec.dRoboName<>'')and(Rec.dDOF<>'') then
Begin
OneTheSame := false;
Assign(DbaseFile,DbaseFileName);
Reset(DbaseFile);
Place := FileSize(DbaseFile);
for loop:=0 to (Place-1) Do
Begin
Seek(DbaseFile,loop);
Read(DbaseFile,ExistRec);
if Rec.dRoboName=ExistRec.dRoboName then
Begin
OneTheSame := true;
OneTheSamePos := Loop;
SamesName := concat(' Robot Name: '+ExistRec.dRoboName+
' DOF: '+ExistRec.dDOF);
End;
End;
Close(DbaseFile);
OverWrite:=false;
If OneTheSame then
Begin
MkWin(19,8,61,15,white,cyan,2);
WriteCenter(10,yellow,cyan,'New record is similar to');
WriteCenter(11,yellow,cyan,SamesName);
WriteCenter(13,yellow,cyan,'Do you want to overwrite it? Yes/No');
WriteAt(52,13,Lightcyan,cyan,'Y');
WriteAt(56,13,Lightcyan,cyan,'N');
Read_YN(59,13,'',0,OverWrite);
RmWin;
End;
if OneTheSame=false then
Begin
Assign(DbaseFile,DbaseFileName);
Reset(DbaseFile);
Place := FileSize(DbaseFile);
Seek(DbaseFile,Place);
Write(DbaseFile,Rec);
Close(DbaseFile);
End;
if OverWrite=true then
Begin
Assign(DbaseFile,DbaseFileName);
Reset(DbaseFile);
Seek(DbaseFile,OneTheSamePos);
Read(DbaseFile,DeadRec);
Close(DbaseFile);
EasyDeleteRecord(DeadRec);
WriteCenter(24,yellow,black,'Over-Writing Old Robot File');
Assign(DbaseFile,DbaseFileName);
Reset(DbaseFile);
Place := FileSize(DbaseFile);
Seek(DbaseFile,Place);
Write(DbaseFile,Rec);
Close(DbaseFile);
WriteCenter(24,black,black,' ');
End
End;
End;
(***************************************************************************)
Procedure CheckForDataFileExist;
Begin
{$I-}
Assign(DbaseFile,DbaseFileName);
Reset(DbaseFile);
Close(DbaseFile);
{$I+}
if (IOResult<>0)and(DbaseFileName<>'') then
Begin
Rewrite(DbaseFile);
End;
End;
(***************************************************************************)
Procedure DBPutBottomLine(DBCode : byte);
Begin
WriteAt(1,25,cyan,cyan,' ');
WriteAT(1,25,Black,Cyan,' -Help -Clear -Save -Delete -Find -Use -Escape ');
WriteAT(3,25,Red,cyan,'F1');
WriteAT(13,25,Red,cyan,'F2');
WriteAT(24,25,Red,cyan,'F4');
WriteAT(34,25,Red,cyan,'F6');
WriteAT(46,25,Red,cyan,'F8');
WriteAT(57,25,Red,cyan,'F10');
WriteAt(68,25,Red,cyan,'Esc');
End;
{**************************************************************************}
Procedure MenuBack;
begin
Fillscreen(1,1,80,25,Blue,black,chr(176));
ClearText(1,24,80,25,white,blue);
End;
{**************************************************************************}
Procedure DODbase(VAR RoboName:string;VAR DOF:integer;VAR alpha1,a1,d1,theta1,alpha2,a2,d2,theta2,
alpha3,a3,d3,theta3,alpha4,a4,d4,theta4,alpha5,a5,d5,theta5,
alpha6,a6,d6,theta6,alpha7,a7,d7,theta7:real);
Var mem1 : pointer;
Var
Ch : Char;
DefinedDirectory : boolean;
code : integer;
Begin
clrscr;
ZeroRec;
Init_Vars;
Set_Fields;
ZeroRec;
SetDisplayRecord;
NewRec := True;
if Exist(DbaseFileName)=false then
begin
Assign(DbaseFile,DbaseFileName);
rewrite(DbaseFile);
close(DbaseFile);
end;
Assign(DbaseFile,DbaseFileName);
Reset(DbaseFile);
KeepPosition := FileSize(DbaseFile);
Repeat
Set_Screen_General;
DBPutBottomLine(DBCode);
Activate_Table(1);
Dispose_Fields;
Dispose_Tables;
SET_FIELDS;
EscapeFound := False;
With Table[CurrentTable]^.ITTT do ErrorLine := 21;
Process_Input(1,ReturnCodeDB1,LastChar);
Ch := LastChar;
Case CH of
{Alt-E} #146 : Begin
ZeroRec;
NewRec := True;
Reset(DbaseFIle);
KeepPosition := FileSize(DbaseFIle);
Activate_Table(1);
Dispose_Fields;
Dispose_Tables;
Set_Fields;
End;
{F1} #187 : Begin
End;
{F2} #188 : Begin
ZeroRec;
End;
{F4} #190 : Begin
SaveRecord;
Activate_Table(1);
Dispose_Fields;
Dispose_Tables;
Set_Fields;
End;
{F6} #192 : Begin
DeleteRecord(Rec);
ZeroRec;
Assign(DbaseFile,DbaseFileName);
Reset(DbaseFile);
KeepPosition := FileSize(DbaseFile);
Activate_Table(1);
Dispose_Fields;
Dispose_Tables;
Set_Fields;
End;
{F8} #194 : Begin
NewRec:=false;
GetRecord(DBCode,Rec,Position);
KeepPosition := Position;
Activate_Table(1);
Dispose_Fields;
Dispose_Tables;
Set_Fields;
End;
{F10}#196 : Begin
Selection := Rec;
With Rec do Begin{rec}
if (dRoboName<>'')or(dDOF<>'') then
BEGIN{num1}
RoboName := dRoboName;
val(dDOF,DOF,code);
val(dalpha1,alpha1,code);
val(da1,a1,code);
val(dd1,d1,code);
val(dtheta1,theta1,code);
val(dalpha2,alpha2,code);
val(da2,a2,code);
val(dd2,d2,code);
val(dtheta2,theta2,code);
val(dalpha3,alpha3,code);
val(da3,a3,code);
val(dd3,d3,code);
val(dtheta3,theta3,code);
val(dalpha4,alpha4,code);
val(da4,a4,code);
val(dd4,d4,code);
val(dtheta4,theta4,code);
val(dalpha5,alpha5,code);
val(da5,a5,code);
val(dd5,d5,code);
val(dtheta5,theta5,code);
val(dalpha6,alpha6,code);
val(da6,a6,code);
val(dd6,d6,code);
val(dtheta6,theta6,code);
val(dalpha7,alpha7,code);
val(da7,a7,code);
val(dd7,d7,code);
val(dtheta7,theta7,code);
END;
end;{rec}
EscapeFound := true;
if RoboName=''then EscapeFound := false;
if DOF=0 then EscapeFound := false;
End;
#027 : exit;
End;{Case}
Until EscapeFound;
Activate_Table(1);
Dispose_Fields;
Dispose_Tables;
MenuBack;
End;
Begin
End. Please note that about 60 pages of support procedures are not listed here. They are an accumalation of both commercial pascal procedures and procedures which I have written.
Andrew B. Conru
67 S. 17 Street
Terre Haute, IN 47807
(812) 232-2965
February 2, 1990From ROBOLIST.DOC on the robot-simulator floppy. XyWrite page breaks are shown as separate sheets.