10 MODE 2:mdes=1:DEFINT x-z
15 MEMORY &6A03:LOAD"FILL":LOAD"DEPIMG":'Chargement routine LM (en &6A04 et &6F40)
20 PRINT"Robot a 6 degres de liberte a commande directe"
30 PRINT"options:-visualisation position fixe" 
40 PRINT"        -animation sur 7 images"
50 PRINT"Copyright Hebdogicie1 et 1'auteur:Hedi Kodja"
60 INPUT "Presentation(o/n)";k$
70 IF k$="o" oR k$="O" THEN GOSUB 2200
80 MODE 2:DEG
85 PRINT:PRINT"Nota: Pour la demo ROBOT3D Nb Seg=7; Nb sec/seg=4 et Pt/sec=8":PRINT:PRINT,"Puis, chargez le dossier 'xyzl'":PRINT
90 INPUT"Nombre de segments:";km
100 INPUT"Nombre de sections/segment:";im
110 INPUT"Nombre de Points/section:";jm 
120 km=km-1:im=im-1::jm=jm-1
130 DIM x(im,jm,km), y(im,jm,km),z(im,jm,km)
140 DIM xe(im,jm,km), ye(im,jm,km),ze(im,jm,km)
150 DIM l(km),a(km)
160 DIM t%(5),ord%(6),sens%(6),ctr%(im,jm)
170 m$="":PRINT:PRINT"desirez-vous:"
180 PRINT"1-Lire les segments sur cassette"
190 PRINT"2-Sauvegarder les segments sur cassette" 
200 PRINT"3-Editer/Modifier les segments"
210 PRINT"   ###"
220 PRINT"4-Representer le robot sur une position fixe"
230 PRINT"5-Representer le robot anime"
240 PRINT"6-La presentation"
250 PRINT"7-Vue eclatee(controle XYZL)"
260 PRINT"8-Changer le mode de dessin"
270 INPUT mu:IF mu<>8 GOTO 360  
280 PRINT"modes dessin:"
290 PRINT"1-Fil de fer"
300 PRINT"2-Faces vers observateur"
310 PRINT"3-Faces peintes en LM" 
320 PRINT"4-Faces peintes en BASIC"
330 INPUT mdes
340 IF mdes=3 THEN MODE 1:INK 0,10:INK 1,1:INK 2,24:INK 3,0,24:PAPER 1:PEN 2 ELSE INK 0,1:INK 1,24:PEN 1:PAPER 0:MODE 2
350 CLS:INPUT"Meme disposition?(o/n)";m$:IF m$="o" THEN GOSUB 410 ELSE m$="":GOTO 170
360 ON mu GOSUB 2120,2110,2070,400,450,2200,2550:GOTO 170
370 '
380 'mode une image
390 '
400 ec=1:GOSUB 1480:FOR k=1 TO km:PRINT"art.";k;:INPUT a(k):NEXT k:IF mdes=3 THEN MODE 1 ELSE MODE 2
410 orgy=100:ORIGIN 500,orgy:CLS:PRINT"10 secondes de calcul":GOSUB 670:SOUND 1,100,50:LOCATE 1,2:PRINT"menu ?":CALL &BB06:RETURN
420 '
430 'mode defilement 7 images
440 '
450 ec=0.5:GOSUB 1480:PRINT"entrez la valeur initiale et la valeur finale pour chaque articulation" 
460 FOR k=1 TO km:PRINT"art.";k;:INPUT dep(k),arr(k):dart(k)=(arr(k)-dep(k))/6:NEXT k
470 MODE 1-(mdes<>3):CLG -(mdes=3):orgy=250:ORIGIN 250,orgy,0,319,399,208 
480 DATA &c0,&28,&c4,&10,&c4,&38,&6b,&80,&6b,&a8,&6f,&90,&6f,&b8
490 RESTORE 480:'adresses images
500 LOCATE 1,13:PRINT"image no:" 
510 FOR kim=0 TO 6
520 FOR k=1 TO 6:a(k)=dep(k)+kim*dart(k):NEXT k:CLG -(mdes=3):LOCATE 11,13:PRINT kim+1:GOSUB 670:SOUND 1,100,50
530 READ a,b:POKE &6F89,a:POKE &6F88,b:POKE &6F87,&C0:POKE &6F86,&0:CALL(&6F40):NEXT kim:temp=0:'stockage images
540 DATA &c0,&28,&c4,&10,&c4,&38,&6b,&80,&6b,&a8,&6f,&90,&6f,&b8 
550 DATA &6f,&b8,&6f,&90,&6b,&a8,&6b,&80,&c4,&38,&c4,&10,&c0,&28  
560 'defilement
570 DATA &49,&4c,&50,&45,&4b,&42,&a5
580 RESTORE 570:FOR kim=1 TO 7:READ a:POKE 1+&7740,a:NEXT kim
590 LOCATE 1,13:PRINT"'s'=stopper, entrez temps (<0=fin)";
600 RESTORE 540
610 FOR kim=1 TO 14
620 READ a,b:POKE &6F89,&C0:POKE &6F88,&0:POKE &6F87,a:POKE &6F86,b
630 CALL(&6F40):FOR t=0 TO temp:NEXT t:SOUND 1,30,2:NEXT kim:'defilement
640 IF INKEY$="s" THEN INPUT temp:IF temp>=0 GOTO 560 ELSE RETURN
650 GOTO 600
660 '
670 'creation d'une image
680 '
690 IF m$="o" GOTO 750
700 m1=0:m2=0:m3=0:m1=t11*ec:m5=t21*ec:m6=t31*ec:m7=t12*ec:m8=t22*ec:m9=t32*ec:m10=t13*ec:m11=t23*ec:m12=t33*ec
710 k=0:IF disf>50 THEN GOSUB 1410 ELSE GOSUB 1320
720 sens%(0)=SGN(m10):FOR k=1 TO km:ON k GOSUB 1270,1220,1220,1270,1220,1270:sens%(k)=SGN(m10):IF disf>50 THEN GOSUB 1410 ELSE GOSUB 1320
730 NEXT k
740 '
750 ' affichage
760 '
770 ON mdes GOTO 1040,1090
780 GOSUB 1540
790 FOR ko=0 TO km:SOUND 1,50,10:k=ord%(ko):hp=4:IF sens%(k)>0 THEN ini=0:ifin=im-1:pas%=1 ELSE ini=im-1:ifin=0:pas%=-1
800 FOR i=ini TO ifin STEP pas%:is=i+1
810 FOR j=0 TO jm:js=j+1:IF js>jm THEN js=0
820 GOSUB 980:IF nxe<=0 GOTO 890
830 IF mdes=3 THEN PLOT 0,999,3:GOSUB 1970:POKE &6B7E,MIN(199,MAX(0,199-(YPOS+orgy)/2)):CALL &6A04:PLOT 0,999,2:GOSUB 1970:GOTO 890
840 xh(1)=ye(i,j,k):yh(1)=ze(i,j,k)
850 xh(2)=ye(is,j,k):yh(2)=ze(is,j,k)
860 xh(3)=ye(is,js,k):yh(3)=ze(is,js,k)
870 xh(4)=ye(i,js,k):yh(4)=ze(i,js,k)
880 GOSUB 1720:PLOT 0,1000,0:GOSUB 1950
890 NEXT j:NEXT i
900 hp=8:IF pas%=-1 THEN i=0
910 GOSUB 1010:IF nxe<=0 GOTO 950
920 IF mdes=3 THEN PLOT ye(i,jm,k),ze(i,jm,k),3:GOSUB 2050:POKE &6B7E,MIN(199,MAX(0,199-(YPOS+orgy)/2)):CALL &6A04:PLOT ye(i,jm,k),ze(i,jm,k),2:GOSUB 2050:GOTO 950
930 FOR jtra=0 TO Jm:xh(jtra+1)=ye(i,jtra,k):yh(jtra+1)=ze(i,jtra,k):NEXT jtra:GOSUB 1720
940 PLOT xh(8),yh(8),0:GOSUB 2040
950 NEXT ko:RETURN
960 '
970 'norm. ext. facette
980 nxe=(ze(is,j,k)-ze(i,j,k))*(ye(i,js,k)-ye(i,j,k))-(ye(is,j,k)-ye(i,j,k))*(ze(i,js,k)-ze(i,j,k)):RETURN
990 '
1000 'norm.ext.face extre.
1010 nxe=(ze(i,2,k)-ze(i,0,k))*(ye(i,jm,k)-ye(i,0,k))-(ye(i,2,k)-ye(i,0,k))*(ze(i,jm,k)-ze(i,0,k)):IF i<>0 THEN nxe=-nxe
1020 RETURN
1030 '
1040 'fil de fer
1050 '
1060 PLOT 0,0,1:FOR k=0 TO km:SOUND 1,50,5: FOR i=0 TO im:MOVE ye(i,jm,k),ze(i,jm,k): FOR j=0 TO jm:DRAW ye(i,j,k),ze(i,j,k):NEXT j:NEXT i
1070 FOR J=0 TO Jm : MOVE ye(0,j,k),ze(0,j,k):FOR i=1 TO im:DRAW ye(i,j,k),ze(i,j,k):NEXT i:NEXT j:NEXT k :RETURN
1080 '
1090 'faces ^ observ.
1100 '
1110 PLOT 0,0,1 : FOR k=0 TO km:SOUND 1,50,5:FOR i=0 TO im-1:is=i+1:FOR j=0 TO jm:js=j+1:IF js>jm THEN js=0
1120 GOSUB 980: IF nxe>0 THEN GOSUB 1970 
1130 NEXT j : NEXT i 
1140 FOR i=0 TO im STEP im
1150 GOSUB 1010:IF nxe<=0 GOTO 1170
1160 MOVE ye(i,jm,k),ze(i,jm,k):GOSUB 2050 
1170 NEXT i:NEXT k
1180 RETURN
1190 '
1200 ' rota/x
1210 ' 
1220 l=l(k-1):c=COS(a(k)):s=SIN(a(K)):m1=m1+m10*l:m2=m2+m11*l:m3=m3+m12*l:n7=m7:n8=m8:n9=m9:m7=c*m7+s*m10::m8=c*m8+s*m11
1230 m9=c*m9+s*m12:s=-s:m10=s*n7+c*m10:m11=s*n8+c*m11:m12=s*n9+c*m12:t%(k-1)=SGN(m4):RETURN
1240 '
1250 'rota/z
1260 ' .
1270  l=l(k-1):c=COS(a(k)):s=SIN(a(k)):m1=m1+m10*l:m2=m2+m11*l:m3=m3+m12*l:n4=m4:n5=m5:n6=m6:m4=c*m4+s*m7::m5=c*m5+s*m8
1280 m6=c*m6+s*m9:s=-s:m7=s*n4+c*m7:m8=s*n5+c*m8:m9=s*n6+c*m9:t%(k-1)=SGN(m10):RETURN

1290 '
1300 'Pos/repere ecran isometrique
1310 '
1320 FOR i=0 TO im:FOR j=0 TO jm
1330 x=x(i,j,k):y=y(i,j,k):z=z(i,j,k)
1340 xe(i,j,k)=m1+m4*x+m7*y+m10*z
1350 ye(i,j,k)=m2+m5*x+m8*y+m11*z
1360 ze(i,j,k)=m3+m6*x+m9*y+m12*z
1370 NEXT j:NEXT i:RETURN
1380 '
1390 'Pos/repere ecran avec Perspective 
1400 '
1410 FOR i=0 TO im:FOR j=0 TO jm
1420 x=x(i,j,k):y=y(i,j,k):z=z(i,j,k)
1430 xe(i,j,k)=m1+m4*x+m7*y+m10*z:koef=disf/(disf-xe(1,j,k))
1440 ye(i,j,k)=(m2+m5*x+m8*y+m11*z)*koef
1450 ze(i,j,k)=(m3+m6*x+m9*y+m12*z)*koef
1460 NEXT j:NEXT i:RETURN
1470 '
1480 'coefficients Euler
1490 '
1500 INPUT"Psi";Psi:INPUT"teta";teta:INPUT"distance focale Pour perspective (0 Si vue isometrique)";disf
1510 cp=COS(psi):sp=SIN(psi):ct=COS(teta):st=SIN(teta)
1520 t11=cp*ct:t12=sp*ct:t13=st:t21=-sp:t22=cp:t31=-st*cp:t32=-st*sp:t33=ct:RETURN
1530 '
1540 'Ordre segments
1560 FOR k=0 TO 6:ord%(k)=k:NEXT k
1570 FOR k=0 TO 5
1580 FOR ic1=0 TO 6
1590 IF ord%(ic1)=k GOTO 1610
1600 NEXT ic1
1610 FOR ic2=0 TO 6
1620 IF ord%(ic2)=k+1 GOTO 1640
1630 NEXT ic2
1640 IF SGN(ic2-ic1)=t%(k) GOTO 1690
1650 FOR ir=ic2 TO ic1+1 STEP-1
1660 ord%(ir)=ord%(ir-1)
1670 NEXT ir
1680 ord%(ic1)=k+1
1690 NEXT k
1700 RETURN
1710 '
1720 'peindre BASIC:peint contour polygonal convexe qqconque
1730 '
1740 PLOT 0,0,1:ihm=1:ymin=9999:FOR ih=1 TO hp:IF yh(ih)<yh(ihm) GOTO 1760
1750 ihm=ih
1760 IF yh(ih)>ymin GOTO 1780
1770 ymin=yh(ih)
1780 NEXT ih
1790 ymax=yh(ihm):IF (ymax-ymin)<dh THEN RETURN
1800 ag=ihm:ags=ihm+1:IF ags>hp THEN ags=ags-hp
1810 ad=ihm:ads=ihm-1:IF ads<1 THEN ads=ads+hp
1820 FOR yh=ymax-1 TO ymin+1 STEP -2
1830 IF yh>yh(ags) GOTO 1870
1840 ag=ag+1:IF ag>hp THEN ag=ag-hp
1850 ags=ags+1:IF ags>hp THEN ags=ags-hp
1860 GOTO 1830
1870 IF yh>yh(ads) GOTO 1910
1880 ad=ad-1:IF ad<1 THEN ad=ad+hp
1890 ads=ads-1:IF ads<1 THEN ads=ads+hp
1900 GOTO 1870
1910 xdeb=xh(ag)+(yh-yh(ag))*(xh(ags)-xh(ag))/(yh(ags)-yh(ag))
1920 xfin=xh(ad)+(yh-yh(ad))*(xh(ads)-xh(ad))/(yh(ads)-yh(ad))
1930 PLOT xdeb,yh:DRAW xfin,yh:NEXT yh:RETURN
1940 '
1950 ' une facette
1960 '
1970 MOVE ye(i,j,k),ze(i,j,k)
1980 DRAW ye(is,j,k),ze(is,j,k)
1990 DRAW ye(is,js,k),ze(is,js,k)
2000 DRAW ye(i,js,k),ze(i,js,k)
2010 DRAW ye(i,j,k),ze(i,j,k)
2020 RETURN 
2030 '
2040 ' face extreme
2050 FOR  j=0 TO jm:DRAW ye(i,j,k),ze(i,j,k):NEXT j: RETURN
2060 '
2070 PRINT"edition/modification de segments"
2080 INPUT"numero du segment (entre 0 et 6.Negatif pour retour menu)";k:IF k<0 THEN RETURN
2090 INPUT"entr-axe";l(k):FOR i=0 TO im:PRINT"i=",i:FOR j=0 TO jm:PRINT"j=";j;"/";:INPUT x(i,j,k),y(i,j,k),z(i,j,k): NExT j:INPUT"controle";ctr%(i,k):NEXT i:GOTO 2070
2100 '
2110 PRINT" Sauvegarde dossier: 'xyzl'":OPENOUT"xyzl":GOSUB 2140:CLOSEOUT:MODE 2:RETURN
2120 PRINT" Lecture dossier: 'xyzl'":OPENIN "xyzl":GOSUB 2140:CLOSEIN:MODE 2:RETURN
2130 '
2140 'module E/S     
2150 FOR k=0 TO km: IF mu=1 THEN INPUT #9,l(k) ELSE PRINT #9,l(k)
2160 FOR i=0 TO im:FOR j=0 TO jm:IF mu=1 THEN INPUT #9,x(i,j,k),y(i,j,k),z(i,j,k) ELSE PRINT #9,x(i,j,k),y(i,j,k),z(i,j,k)
2170 NEXT j:IF mu=1 THEN INPUT #9,ctr%(i,k) ELSE PRINT #9,ctr%(i,k)
2180 NEXT i:NEXT k:RETURN
2190 '
2200 MODE 2:PRINT"Presentation"
2210 PRINT"Ce programme est destine a visualiser un robot compose de 7 elements(segments):":PRINT
2220 PRINT"-le bati(0)est fixe" 
2230 PRINT"-le socle(1) pivote/bati(0)"
2240 PRINT"-le premier bras(2)"
2250 PRINT"-1e second bras(3)"
2260 PRINT"-les segments (4)et(5)"
2270 PRINT"-l'organe terminal=pince(6)":PRINT:PRINT"1'ensemble 456 est le poignet du  robot":PRINT
2280 PRINT"chacun,des segments est articule/a celui qui le precede,est"
2290 PRINT"sa position/au precedent est mesuree par un angle(degre) appelle variable articulaire.il y en a 6 en tout."
2300 PRINT :PRINT"Pour eviter les collisions entre segments,on doit avoir artic.2,3 et 5":PRINT"compris entre -135 et 135 degres":GOSUB 2540
2310 PRINT"Il existe 4 modes de dessin:":PRINT"-Fil de fer: toutes les arretes sont tracees":PRINT
2320 PRINT"-Faces vers observateur:les faces non orientees vers vous ne sont pas tracees(elimine certains traits caches,mais pas tous car objets non convexes)":PRINT
2330 PRINT"-Faces peintes en BASIC:tout les traits caches sont elimines":PRINT:PRINT"-Faces peintes en LM:idem mais tres rapide":GOSUB 2540
2340 PRINT"PARAMETRES OBSERVATEUR":PRINT"Pour visualiser le robot.vous devez Preciser d'ou vous le regardez et cela a 1'aitde des ang1es PSI et TETA":PRINT
2350 PRINT"----PSI:angle duquel vous tournez autour du robot(=0 donne de Profil.=-90"
2360 PRINT"donne de face=-45 donne en biais etc...)":PRINT
2370 PRINT"----TETA:ang1e de 'Plongee'(=0 vous restez au raz du sol.=30 vous surplombez legerement.=80 vous etes presque a la verticale.=90 vous voyez le robot du dessus."
2380 PRINT"Pour TETA negatif vous voyez le robot par desous(essayez teta=-30":PRINT:PRINT"(Position normale Psi=20,Tata=30)"
2390 PRINT:PRINT"Suivant la distance ROBOT-OBSERV. on aura un effet de PERSPECTIVE( avec 300(dist.focale<1000) ou une vue ISOMETRIOUE(entrer dist.foc.=0)":GOSUB 2540
2400 PRINT"choix POSITION FIXE":PRINT:PRINT"En Plus des Parametres observateur precedents,vous devez entrer la valeur des 6 variables articulaires(art.1 a 5)du robot":PRINT:PRINT"pour rester dans l'ecran:-60<art.1<60,30 <art.2<135,-90 art.3<135"
2410 GOSUB 2540
2420 PRINT"choix ANIMATION":PRINT:PRINT"Ce choix permet de faire un dessin anime du mouvement du robot":PRINT"vous devez entrer les para.obs.(Psi,teta,dist,Focale( et entrer pour chaque articulation 2 valeurs."
2430 PRINT:PRINT"-sa valeur initiale(degre)":PRINT"-sa valeur finale"
2440 PRINT"7 images initermediaires sont realisee.Entre la config.init. et la config.finale":PRINT:PRINT"Puis 1'effet de mouvement est fait par appel des images"
2450 PRINT"EXEMPLE:Psi=0/teta=30/dis.foc 0(=isom)/art.1:-40,40/art.2:45,135/art.3:45,-45/art.4:-45,45/art.5:20,60/art.6:20,-20.PuiS idem sauf:teta=-3,disf=300(montre effet perspec."
2460 PRINT:PRINT"Pour mettre en evidence l'effet de Chaque art.,prendre le mode anime,fixer toutes les art.en:45 degres(entrer:45,45)Sauf une, la mettre entre 0 et 30(entrer 0,30)"
2470 PRINT"Pour ralentir le mouvement tapez 's' et entrer un nombre>0.Si<0,retour menu":GOSUB 2540
2480 PRINT"IMPORTANT:Les 3 choix fil de de fer/peinture/etc.perspective/isometr.,et posi.fix/anime peuvent TOUS etre combines.":PRINT"donc 4*2*2=16 facons de visualiser le robot"
2490 PRINT"ATTENTION:SAUVEGARDEZ CE PRGRM AVANT TOUT ESSAI !!!":PRINT"peut sauter si erreurs de datas":PRINT"SAUVEGARDEZ ausi le DOSSIER XYZL(par l'option 2 du menu"
2500 PRINT"controle dossier xyzl:Prenez l'option controle(qui separe les  segt.)et comparez avec la vue fournie.":PRINT"Numerotatton des points.":PRINT"-indice 0<k<6,no segment"
2510 PRINT"-indice 0<j<7:no de section dans le segt.":PRINT"-indice 0<j<7:no du pt dans la section"
2528 PRINT"si un pt est faux lors du list,faire BREAK,entrer correction en mode direct,Puis CONT,Puis sauver le dossier"
2538 PRINT"-si vous utilisez les data (dossier xyzl) ci-joints.entrez au":PRINT"debut du Prg.:":PRINT"-nbre de segments=7":PRINT"-nbre de sections/segt=4":PRINT"-nbre de pts/sect=8"
2540 PRINT:INPUT"<enter>",k$:MODE 2:RETURN
2550 MODE 2:PRINT"Vue eclatee":mdes=1:INK 1,24:INK 0,1:ORIGIN 500,70: FOR k=0 TO km:a(k)=50:l(k)=l(k)+20:NEXT k:disf=900:psi=20:teta=20:GOSUB 1510
2560 ec=l:GOSUB 670:FOR k=0 TO km:l(k)=l(k)-20:NEXT k:SOUND 1,20,30:PRINT"<enter>:impression et controle dossier XYZL":CALL &BB06
2570 CLS:ip=0:FOR k=0 TO km:PRINT#ip,"segment k=";k;": entraxe=";l(k)
2580 FOR i=0 TO im:s%=0:PRINT#ip,"section i=";i:FOR j=0 TO jm:x=x(i,j,k):y=y(i,j,k):z=z(i,j,k):s%=s%+x+y+z:PRINT#ipP,"j=";j;"/";x;y;z:NEXT j:PRINT #ip,"controle=";ctr%(i,k)
2590 IF s%<>ctr%(i,k) THEN PRINT#ip,"erreur segt";k;"section";i;STRING$(37,"#")
2600 NEXT i:NEXT k:RETURN
