!**************************************************************************! ! ! ! ROBOT VISION ! ! ! ! IAN R GEBBIE (CS4) ! ! ! ! JUNE 1986 ! ! ! !**************************************************************************! %include "inc:util.imp" {Utilities package } %include "irg:imgraph.inc" {Graphics package } %include "irg:maths.inc" {Maths Package } %include "inc:maths.imp" {Constant Maths tables} %include "irg:roblib.inc" {Robot constants } %include "irg:region" {File handling package} %begin %recordformat vertice(%real x,y,z) %record(vertice)%array vert(1:8) %recordformat linef(%integer x1,y1,x2,y2,%real m) %record(linef) %array lines(0:1000) %real cx,cy,cz,m %constreal bigreal=9999999 %integer l,x,y, i,j,ch, numlines, lineswanted, bx,by,fx,fy, w,th,cth %integer frames=1, steps=21, exposure=10, outtofile=0, obx=0, cox=250, coy=250, arrowcolour=yellow, dohough=0, homeset=0, dothina=0, dothinb=0, useworm=0, minthresh=25, maxthresh=100, threshhold=50, px=86, py=1 %byte a,b,c,col %string (255) file,reply %integerarray pulses(0:5) %bytearray pic(0:65535) {Array for direct image} %bytearray image,copy1,copy2,orig(0:128*512) {Final image + copies } %constinteger base = 16_400FF0, tag = 16_C0, noalt=32, narrow=16, eightbit=8, twoarray=4, soak=2, nosend=1, alt=0, wide=0, sevenbit=0, onearray=0, refresh=0, send=0, lens=8, elbow=4, shoulder=2, turntable=1, cubeside=50, strobe = 128 %constreal xmult=1.264705882 %label quit,cont @base+16_00 %byte acia cont; @base+16_02 %byte acia data @base+16_08 %byte pia areg; @base+16_0A %byte pia acont @base+16_0C %byte pia breg; @base+16_0E %byte pia bcont !*****************************************************! ! WAIT ! !*****************************************************! %routine wait(%integer msec) msec = cputime + msec %while cputime <= msec %cycle %repeat %end !*****************************************************! ! SHOWOPTIONS ! ! Shows mouse button options at top of graphics ! ! screen. ! !*****************************************************! %routine showoptions(%string (15) ml,mm,mr,mlmr) setcolour(black) fill(42,490,162,500) fill(202,490,322,500) fill(362,490,482,500) fill(538,490,658,500) setcolour(white) textat(42,490);showstring(ml) textat(202,490);showstring(mm) textat(362,490);showstring(mr) textat(538,490);showstring(mlmr) %end !*****************************************************! ! YESorNO ! ! Prompts for a YES or NO answer. ! !*****************************************************! %string(255)%function YESorNO(%string(255) prmpt) %string(255) reply,res %integer ch prompt(prmpt);read(reply) %if reply->("y").reply %or reply->("Y").reply %then %c res="YES" %else res="NO" %cycle ch=testsymbol %repeatuntil ch=-1 %result=res %end !-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----! !-----=======<<<<<<<<<< ROBOT CONTROL >>>>>>>>>>=======-----! !-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----! !*****************************************************! ! PULSE ! ! Sends a single pulse to the robot. ! !*****************************************************! %routine pulse(%integer msec) pia bcont = 16_3C wait(msec) pia bcont = 16_34 wait(msec) %end !*****************************************************! ! MOVE AXES ! ! Moves all axes of robot according to contents of ! ! array PULSES. Each location in PULSES stores the ! ! number of pulses to be sent to the corresponding ! ! axis. ! !*****************************************************! %routine move axes(%integer mdp, %integerarray pulses(0:5)) %integer mep, i, pulsewdth pia breg = mdp pia breg = mdp ! strobe wait(1) pia breg = mdp & (\strobe) %if pulses(0) >0 %then pulsewdth = 8 %else pulsewdth = 5 %cycle mep = 0 %for i=0, 1, 5 %cycle %if pulses(i)>0 %then mep = mep ! (1<>>>>>>>>>>>>>>>>>>=======-----! !-----=======<<<<<<<<<< CAMERA CONTROL >>>>>>>>>=======-----! !-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----! !*****************************************************! ! OPEN ACIA ! ! Open ACIA for camera control. ! !*****************************************************! %routine open acia %constinteger acset = 16_03, {master reset} acopt = 16_14 {div 1, 8 bits 1 stop bit} acia cont = acset acia cont = acopt %end !*****************************************************! ! OUTCH ! ! Output character to camera via ACIA. ! !*****************************************************! %routine outch(%integer ch) %integer c, time time=cputime+1000 %cycle c = acia cont %exit %if c&2 # 0 {Time out if no response} %signal 15,1 %if time=cputime {from camera} %repeat acia data = ch %end !*****************************************************! ! GRAB FRAME ! ! Obtain frame from camera and place in consecutive ! ! memory locations starting at location ADDRESS. ! !*****************************************************! %routine grab frame(%integer command, exposure, address, %integername rows,cols,bytes) %integer start, end %label waitch open acia wait(2) command = (command & (\3)) ! tag ! refresh ! nosend outch(command) wait(5) command = (command & (\3)) ! soak ! nosend outch(command) wait(exposure) rows=64; cols=16 command = (command & (\3)) ! send ! noalt ! soak %if command & twoarray # 0 %then rows=rows*2 %if command & narrow = 0 %then cols=cols*2 %if command & noalt # 0 %then rows=rows*2 %and cols=cols*2 bytes=rows*cols {Number of bytes to..} outch(command) {grab from camera } start = address; end = address+bytes *movea.l start,a0 {Address to store in} *movea.l end,a1 waitch: *btst #0,aciacont {Anything available from} *beq waitch {ACIA?} *move.b aciadata,0(a0) {Store in Memory} *adda.l #1,a0 *cmpa.l a0,a1 *bne waitch {All of frame received?} command = (command & (\3)) ! nosend ! refresh outch(command) %end !*****************************************************! ! DISPLAY FRAME ! ! Display single 512*128 image at location (x,y). ! ! Frame is in format of one byte per pixel. ! !*****************************************************! %routine display frame(%integer x,y,%bytename line) %label ok {X,Y and the address LINE are passed in registers D0,D1 and A0 respectively} *move.l d0,d2 {copy X into D2} *lsl.l #7,d1 {Y*128} *andi.l #16_FFFFFFF0,d0 {Clear bottom 4 bits of X} *lsr.l #3,d0 {X*8} *add.l d0,d1 {offset within byte} *lea frame,a1 {address of framestore} *lea 0(a1,d1.l),a1 {obtain actual address of byte} *move.l a1,a3 {copy of address} *adda.l #128,a3 {byte the row above current row} *lea colourreg,a2 {address of colour register} *andi.l #15,d2 *eor.l #15,d2 {pixel within byte} *clr.l d1 *bset d2,d1 {Set pixel position to 1} *moveq #127,d3 {128 rows} z:*move.l #511,d0 {512 columns} a:*move.b (a0)+,(a2) {move pixel colour into colour register} *move.w d1,(a1) {write pixel to framestore} *ror.w #1,d1 {move pixel to the right} *btst #15,d1 {Check if word boundary reached} *beq ok {word boundary not reached} *lea 2(a1),a1 {word boundary reached so increment address} ok: *dbra d0,a {decrement and branch until end of row reached} *move.l a3,a1 {move onto next row} *adda.l #128,a3 {store next row after current one} *bset d2,d1 {reset position of first pixel in row} *dbra d3,z {dec. and branch until all rows done} %end !*****************************************************! ! TRANSFORM ! ! Takes direct image from camera and transforms it ! ! from 8 pixels per byte to 1 pixel per byte. ! !*****************************************************! %routine transform %integer r,c,i,j,k,l,pix,x,y %label L1,L2,L3,L4,L5,L6,L7,L8,L9,L10 !%for r=0,1,127 %cycle {128 rows} *MOVEQ #-1,D0 *MOVE.L D0,r L1: *ADDQ.L #1,r !%for c=0,1,31 %cycle {32 cols of 8 pixels} *MOVEQ #-1,D0 *MOVE.L D0,c L2: *ADDQ.L #1,c !k = (127-r)<<5+c {image arrives upside down} *MOVEQ #127,D0 *SUB.L r,D0 *LSL.L #5,D0 *ADD.L c,D0 *MOVE.L D0,k !%for j=7,-1,0 %cycle {Count through 8 pixels in byte} *MOVEQ #8,D1 *MOVE.L D1,j L3: *SUBQ.L #1,j !l=c<<3 + j *MOVE.L c,D0 *LSL.L #3,D0 *ADD.L j,D0 *MOVE.L D0,l !%if pic(k)&(1<>>>>>>>>>>>>>>>>>>=======-----! !-----=======<<<<<<<< HOUGH TRANSFORM >>>>>>>>>>>>=======-----! !-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----! !*****************************************************! ! HOUGH ! ! Takes an thinned image and an unthinned copy and ! ! determines the lines and their endpoints. Lines ! ! returned in array lines. ! !*****************************************************! %routine hough(%bytearrayname image,copy(0:128*512), %record (linef) %arrayname lines(0:1000),%integername numlines) %recordformat peak(%integer l,r,th) %record (peak) %array order(0:128*512) %integer r,th,max,i,x0,y0, x127,y511,x1,numpeaks=0, x2,y1,y2,ff,numpoints,num,oldnum %constantinteger masklength=3, maxspaces=4, mergedist=10, minlen=8 %real st,ct %integerarray list(0:100,1:2) %bytearray a(0:527,0:359) %string(255) ouths !*****************************************************! ! MOVEDOWN ! ! Move elements between a and b in array ORDER down ! ! one element. ! !*****************************************************! %routine movedown(%integer a,b) %integer i %for i=b+1,-1,a+1 %cycle order(i)_l=order(i-1)_l order(i)_r=order(i-1)_r order(i)_th=order(i-1)_th %repeat %end !*****************************************************! ! INSERT ! ! Insert peak (num,r,th) into sorted array ORDER ! ! between bot and top. ! !*****************************************************! %routine insert(%integer num,r,th,bot,top) %integer middle %while bot#top %cycle {Iteratively find position in array} middle=(bot+top)>>1 %if num>=order(middle)_l %then top=middle %else bot=middle+1 %repeat %if num>order(bot)_l %start {Make space in arry for element to be ins.} movedown(bot,numpeaks-1) order(bot)_l=num order(bot)_r=r order(bot)_th=th %finishelsestart movedown(bot+1,numpeaks-1) {""} order(bot+1)_l=num order(bot+1)_r=r order(bot+1)_th=th %finish numpeaks=numpeaks+1 %end !*****************************************************! ! INSERT PEAK ! ! Insert peak into array order. ! !*****************************************************! %routine insert peak(%integer l,r,th) %integer j,i=0 colour(red) {plot peak on display} plot(r,th+135) %if numpeaks=0 %start {set up array if empty} order(0)_l=l order(0)_r=r order(0)_th=th numpeaks=1 %finishelse insert(l,r,th,0,numpeaks-1) %end {otherwise insert it} !*****************************************************! ! SAME ! ! Checks too see if two lines have the same endpoints! !*****************************************************! %integerfunction same(%integer a,b,c,d,e,f,g,h) %integer r %if |a-e|<=mergedist %and |b-f|<=mergedist %and |c-g|<=mergedist %and %c |d-h|<=mergedist %then r=1 %else r=0 %result=r %end !*****************************************************! ! TOOSHORT ! ! If length of line < minlen then can delete line. ! !*****************************************************! %integerfunction tooshort(%integer a,b,c,d) %integer r %if |a-c|dx %start temp=dx dx=dy dy=temp interchange=1 %finish e=dy<<1-dx %for i=0,1,dx %cycle %if copy(x+y<<9)=1 %start {New endpoint found} x1=x y1=y ->done %finish %while (e>=0) %cycle %if interchange=1 %then x=x+s1 %else y=y+s2 e=e-dx<<1 %repeat %if interchange=1 %then y=y+s2 %else x=x+s1 e=e+dy<<1 %repeat done: %end !*****************************************************! ! EVAPORATE ! ! 'Evaporates' off the overshoots that occur in ! ! the endpoints found. ! !*****************************************************! %routine evaporate(%integer numlines,%record (linef) %arrayname lines(0:1000)) %integer i fillimage(copy) %for i=0,1,numlines-1 %cycle retreat(lines(i)_x1,lines(i)_y1,lines(i)_x2,lines(i)_y2) retreat(lines(i)_x2,lines(i)_y2,lines(i)_x1,lines(i)_y1) %repeat %end !*****************************************************! ! CREATE MASK ! ! Create a mask which is at 90 degrees to line with ! ! angle TH. ! ! Uses Bresenhams Line Drawing Alg. ! !*****************************************************! %routine createmask(%integer th,%integerarrayname mask(-3:3,-3:3)) %integer x1=0,y1=0,x2,y2,ox=-1,oy=0 %integer i,j,x,y,dx,dy,s1,s2,temp,interchange,e %for i=-3,1,3 %cycle %for j=-3,1,3 %cycle mask(i,j)=0 %repeat %repeat x2=int(masklength*cos((pi/180)*th)) y2=int(masklength*sin((pi/180)*th)) x=x1 y=y1 dx=|x2-x1| dy=|y2-y1| s1=sign(x2-x1) s2=sign(y2-y1) interchange=0 %if dy>dx %start temp=dx dx=dy dy=temp interchange=1 %finish e=dy<<1-dx %for i=0,1,dx %cycle mask(x,y)=1 mask(-x,-y)=1 %if (x-ox)=(y-oy) %start mask(x,y-1)=1 mask(-x,-(y-1))=1 %finish ox=x;oy=y %while (e>=0) %cycle %if interchange=1 %then x=x+s1 %else y=y+s2 e=e-dx<<1 %repeat %if interchange=1 %then y=y+s2 %else x=x+s1 e=e+dy<<1 %repeat %end !*****************************************************! ! PFOUND ! ! Checks surrounding pixels of point (x,y) using ! ! mask if a pixel is within reach. ! !*****************************************************! %integerfunction pfound(%integer x,y, %integerarrayname mask(-3:3,-3:3)) %integer p,q,xl,xr,yb,yt,r=0 %label done %if x<3 %then xl=-x %else xl=-3 %if y<3 %then yb=-y %else yb=-3 %if x>508 %then xr=511-x %else xr=3 %if y>124 %then yt=127-y %else yt=3 %for p=xl,1,xr %cycle %for q=yb,1,yt %cycle %if copy(x+p+(y+q)<<9)=1 %and mask(p,q)=1 %start r=1 {pixel found} ->done %finish %repeat %repeat done: %result=r %end !*****************************************************! ! MASK LINE ! ! Take a line which extends from one edge of the ! ! image to the other and follow the line looking for ! ! endpoints using the mask.(Uses Bresenhams Alg.) ! !*****************************************************! %routine maskline(%integername x1,y1,x2,y2,numpoints, %integerarrayname mask(-3:3,-3:3),endp(0:100,1:2)) %integer spaces,i,ox,oy,x,y,dx,dy,s1,s2,temp,interchange,e,ff colour(red) ff=0 numpoints=0 spaces=0 x=x1 y=y1 dx=|x2-x1| dy=|y2-y1| s1=sign(x2-x1) s2=sign(y2-y1) interchange=0 %if dy>dx %start temp=dx dx=dy dy=temp interchange=1 %finish e=dy<<1-dx %for i=0,1,dx %cycle plot(px+x,py+y) %if ff=0 %start {If first point not found..} %if pfound(x,y,mask)=1 %start {and a point is found then..} endp(numpoints,1)=x;endp(numpoints,2)=y {store as an endpoint.} ox=x;oy=y numpoints=numpoints+1 ff=1 %finish %finishelsestart {1st endpoint has been found..} %if pfound(x,y,mask)=0 %start {so assume 2nd endpoint} %if spaces=0 %start endp(numpoints,1)=ox;endp(numpoints,2)=oy numpoints=numpoints+1 spaces=spaces+1 %finishelsestart {Attempting to bridge a gap} spaces=spaces+1 %if spaces>maxspaces %start {gap too large} ff=0 spaces=0 %finish %finish %finishelse %if spaces#0 %start {Gap has been bridged} numpoints=numpoints-1 spaces=0 %finishelsestart {still on image line} ox=x oy=y %finish %finish %while (e>=0) %cycle %if interchange=1 %then x=x+s1 %else y=y+s2 e=e-dx<<1 %repeat %if interchange=1 %then y=y+s2 %else x=x+s1 e=e+dy<<1 %repeat %if ff=1 %and spaces=0 %start {runoff end of image..} endp(numpoints,1)=ox;endp(numpoints,2)=oy {without finding 2nd endpoint} numpoints=numpoints+1 %finish %end !*****************************************************! ! FOLLOWLINE ! ! Create a mask at 90 degrees to line to be followed ! ! then send mask along line to find endpoints. ! !*****************************************************! %routine followline(%integer th,%integername x1,y1,x2,y2,numpoints, %integerarrayname list(0:100,1:2)) %integerarray mask(-3:3,-3:3) %integer i,j %real r createmask(th,mask) maskline(x1,y1,x2,y2,numpoints,mask,list) %end !*****************************************************! ! CLEARA ! ! Clears accumulator array. ! !*****************************************************! %routine cleara %integer i,j %label l1,l2 !%for i=0,1,527 %cycle *MOVEQ #-1,D0 *MOVE.L D0,i L1: *ADDQ.L #1,i !%for j=0,1,359 %cycle *MOVEQ #-1,D0 *MOVE.L D0,j L2: *ADDQ.L #1,j !a(i,j)=0 *MOVEA.L a,A0 *MOVE.L i,D0 *MULS #16_168,D0 *MOVE.L j,D1 *LEA 0(A0,D0.L),A0 *CLR.B 0(A0,D1.L) !%repeat *CMPI.L #16_0167,j *BNE L2 !%repeat *CMPI.L #16_020F,i *BNE L1 %end !*****************************************************! ! SETACC ! ! Set up accumulator array. ! ! (Note : range 180<=th<=270 degs. is ignored. ! ! COSDEG and SINDEG are const. integer arrays of ! ! COS(th) and SIN(th) multiplied by 2^21 ! !*****************************************************! %routine setacc(%integer x,y) %integer th,r %for th=0,1,179 %cycle %if th#90 %start r=(x*cosdeg(th)+y*sindeg(th))>>21 %finishelse r=y %if r>=0 %and r<=527 %then a(r,th)=a(r,th)+1 %repeat %for th=271,1,359 %cycle r=(x*cosdeg(th)+y*sindeg(th))>>21 %if r>=0 %and r<=527 %then a(r,th)=a(r,th)+1 %repeat %end !*****************************************************! ! MOD360 ! ! Count using modulus 360 ! !*****************************************************! %integerfunction mod360(%integer a) %label l1,l2 !%if a<0 %then a=a+360 *MOVE.L D0,D0 *BGE L1 *ADDI.L #16_0168,a L1: !%if a>359 %then a=a-360 *CMPI.L #16_0167,a *BLE L2 *ADDI.L #16_FFFFFE98,a L2: !%result=a *MOVE.L a,D0 %end !*****************************************************! ! FINDPEAKS ! ! Locate all peaks in accumulator array. ! !*****************************************************! %routine findpeaks %integer r,t,l,p1,m1,i,j,k %constinteger minlen=6 %for t=0,1,179 %cycle %for r=1,1,526 %cycle l=a(r,t);p1=mod360(t+1);m1=mod360(t-1) {Wraparound array} %if l>minlen %start %if (l>=a(r+1,t) %and l>=a(r-1,t) %and l>=a(r,p1) %c %and l>=a(r,m1) %and l>=a(r+1,p1) %c %and l>=a(r-1,m1) %and l>=a(r-1,p1) %c %and l>=a(r+1,m1)) %start insert peak(l,r,t) {Insert peak into list} %finish %finish %repeat %repeat %for t=271,1,359 %cycle %for r=1,1,526 %cycle l=a(r,t);p1=mod360(t+1);m1=mod360(t-1) %if l>minlen %start %if (l>=a(r+1,t) %and l>=a(r-1,t) %and l>=a(r,p1) %c %and l>=a(r,m1) %and l>=a(r+1,p1) %c %and l>=a(r-1,m1) %and l>=a(r-1,p1) %c %and l>=a(r+1,m1)) %start insert peak(l,r,t) %finish %finish %repeat %repeat %for t=0,1,179 %cycle l=a(0,t); p1=mod360(t+1);m1=mod360(t-1) %if l>minlen %start %if (l>=a(r+1,t) %and l>=a(r,p1) %c %and l>=a(r,m1) %and l>=a(r+1,p1) %c %and l>=a(r+1,m1)) %start insert peak(l,0,t) %finish %finish l=a(527,t);p1=mod360(t+1);m1=mod360(t-1) %if l>minlen %start %if (l>=a(r-1,t) %and l>=a(r,p1) %and l>=a(r,m1) %c %and l>=a(r-1,m1) %and l>=a(r-1,p1)) %start insert peak(l,527,t) %finish %finish %repeat %for t=271,1,359 %cycle l=a(0,t); p1=mod360(t+1);m1=mod360(t-1) %if l>minlen %start %if (l>=a(r+1,t) %and l>=a(r,p1) %c %and l>=a(r,m1) %and l>=a(r+1,p1) %c %and l>=a(r+1,m1)) %start insert peak(l,0,t) %finish %finish l=a(527,t);p1=mod360(t+1);m1=mod360(t-1) %if l>minlen %start %if (l>=a(r-1,t) %and l>=a(r,p1) %and l>=a(r,m1) %c %and l>=a(r-1,m1) %and l>=a(r-1,p1)) %start insert peak(l,527,t) %finish %finish %repeat %end !*****************************************************! ! SETUPACC ! ! Set up the accumulator array according to which ! ! thinning algorithm used. ! !*****************************************************! %routine setupacc %integer i,x,y,k,s %if dothina=1 %and dothinb=0 %start {Thinning alg. A} i=1;j=0 %for x=1,1,510 %cycle %for y=2+i,2,124+i %cycle k=y<<9+x s=image(k) %if s=1 %then %start setacc(x,y) setcolour(yellow) plot(px+x,py+y) %finish %repeat %if j=0 %then j=1 %elsestart %if i=1 %then i=0 %else i=1 j=0 %finish %repeat %finishelsestart {Thinning alg. B} %for x=1,1,510 %cycle %for y=1,1,126 %cycle k=y<<9+x s=image(k) %if s=1 %then %start setacc(x,y) setcolour(yellow) plot(px+x,py+y) %finish %repeat %repeat %finish %end printstring("Attempting Hough Transform!!");newline cleara {Clear acc. array.} setupacc {Setup acc. array} setcolour(black) fill(0,135,528,512) colour (green) box(0,135,528,495) findpeaks {Locate all peaks} prompt("How many lines do you want found? ");read(lineswanted) threshhold=50;numlines=0 %cycle oldnum=numlines setcolour(black) fill(0,135,528,512) colour (green) box(0,135,528,495) %for i=0,1,numpeaks-1 %cycle {plot peaks} colour(8+order(i)_l&255) plot(order(i)_r,135+order(i)_th) %repeat display frame(px,py,copy(0)) max=threshhold*order(0)_l//100 numlines=0;num=0 %while num=max %cycle r=order(num)_r;th=order(num)_th colour(white);line(r,th+135,r,th+135+order(num)_l) {Determine where line crosses..} st=sin(th/dtor);ct=cos(th/dtor) {edge of image} %if st=0 %then y0=-1 %and y511=-1 %elsestart y0=int(r/st);y511=int((r-(511*ct))/st) %finish %if ct=0 %then x0=-1 %and x127=-1 %elsestart x0=int(r/ct);x127=int((r-(127*st))/ct) %finish ff=0 %if 0<=x0<=511 %then x1=x0 %and y1=0 %and ff=1 %if 0<=y0<=127 %start %if ff=0 %then x1=0 %and y1=y0 %and ff=1 %elsestart x2=0;y2=y0 %finish %finish %if 0<=y511<=127 %start %if ff=0 %then x1=511 %and y1=y511 %elsestart x2=511;y2=y511 %finish %finish %if 0<=x127<=511 %then x2=x127 %and y2=127 followline(th,x1,y1,x2,y2,numpoints,list) {Follow line found} th=th-90 %if th<0 %then th=th+360 %if th=90 %or th=270 %then m=bigreal %else m=tan(th*pi/180) %for i=0,2,numpoints-2 %cycle {Convert endpoints into lines} lines(numlines)_x1=list(i,1) lines(numlines)_y1=list(i,2) lines(numlines)_x2=list(i+1,1) lines(numlines)_y2=list(i+1,2) lines(numlines)_m=m numlines=numlines+1 %repeat num=num+1 %repeat {Repeat until threh. reached} write(numlines,0);printstring(" major lines found");newline printstring("Deleting points");newline deletepoints(numlines,lines) printstring(" Reduced to ");write(numlines,0);printstring(" lines");newline setcolour(white) %for j=0,1,numlines-1 %cycle line(px+lines(j)_x1,py+lines(j)_y1,px+lines(j)_x2,py+lines(j)_y2) %repeat %if numlines>>>>>>>>>>>>>>>>>>=======-----! !-----=======<<<<<<<<< LINE THINNING >>>>>>>>>>>=======-----! !-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----! !*****************************************************! ! THINA ! ! Thinning algorithm A. ! !*****************************************************! %routine thinA(%bytearrayname image,copy(0:128*512)) %integer row,col,s,i,j,k,n,past,nummasks=12 %label l1 %constantintegerarray linemask(0:1,0:1)= 2_10010001010,2_11110111011, 2_11000100011,2_11011101111 %constantintegerarray mask(0:11,0:1)= 2_1001010,2_1011110, 2_0010100,2_1011110, 2_0001111,2_1111111, 2_1111000,2_1111111, 2_0010111,2_1011111, 2_1110100,2_1111101, 2_1111010,2_1111101, 2_1001111,2_1011111, 2_0000101,2_1111111, 2_0001011,2_1111111, 2_0110000,2_1111111, 2_1101000,2_1111111 !*****************************************************! ! THINDOUBLE ! ! Thins vertical lines which are only 2 pixels wide ! !*****************************************************! %routine thindouble(%bytearrayname image,copy(0:128*512),%integer i,j,lr) %integer test,res,a,b,c,d,e,f,g,h,k a=i-1; b=i+1; c=i-2; d=i+2 e=j<<9; f=(j+1)<<9; g=(j-1)<<9; h=(j+2)<<9; k=(j-2)<<9 %if lr=1 %start test=((((((((((image(i+h))<<1!image(a+h))<<1!image(d+f))<<1!image(b+f))<<1+ %c image(c+f))<<1!image(a+e))<<1!image(d+g))<<1!image(b+g))<<1!image(c+g))<<1+ %c image(i+k))<<1!image(a+k) %finishelsestart test=((((((((((image(i+h))<<1!image(b+h))<<1!image(c+f))<<1!image(a+f))<<1+ %c image(d+f))<<1!image(b+e))<<1!image(c+g))<<1!image(a+g))<<1!image(d+g))<<1+ %c image(i+k))<<1!image(b+k) %finish %for a=0,1,1 %cycle res=(test!!linemask(a,0))&linemask(a,1) %if res=0 %start {pixel matches mask} image(i+e)=0 {Delete pixel} copy(i+e)=0 %finish %repeat %end !*****************************************************! ! THINPOINTA ! !*****************************************************! %routine thinpointA(%bytearrayname image,copy(0:128*512),%integer i,j,lr, %integername n) %integer test,res,a,b,e,f,g,h,k a=i-1; b=i+1 e=j<<9; f=(j+1)<<9; g=(j-1)<<9; h=(j+2)<<9; k=(j-2)<<9 %if lr=1 %start !thin left mask test=((((((image(a+h))<<1!image(i+h))<<1!image(b+f))<<1+ %c image(a+e))<<1!image(b+g))<<1!image(a+k))<<1!image(i+k) %finishelsestart !Thin right mask test=((((((image(b+h))<<1!image(i+h))<<1!image(a+f))<<1+ %c image(b+e))<<1!image(a+g))<<1!image(b+k))<<1!image(i+k) %finish n=0 %for a=0,1,nummasks-1 %cycle res=(test!!mask(a,0))&mask(a,1) %if res=0 %start {pixel matches mask} n=1 copy(i+e)=0 %finish %repeat %end n=0 %cycle i=1;j=0;past=0 %for col=1,1,510 %cycle %for row=2+i,2,124+i %cycle k=row<<9+col s=image(k) %if s=1 %start thindouble(image,copy,col,row,j) s=image(k) %finish %if s=1 %start thinpointA(image,copy,col,row,j,n) %finish %if n=1 %then past=1 %repeat %if j=0 %then j=1 %elsestart %if i=1 %then i=0 %else i=1 j=0 %finish %repeat !%for k=0,1,128*512 %cycle ! image(k)=copy(k) !%repeat *MOVE.L copy,A0 {copy image} *MOVE.L image,A1 *MOVEQ #-1,D0 l1: *ADDQ.L #1,D0 *MOVE.B 0(A0,D0.L),0(A1,D0.L) *CMPI.L #16_10000,D0 *BNE l1 %repeatuntil past=0 {Repeat until no more changes} %end !*****************************************************! ! THINB ! ! Thinning algorithm B ! !*****************************************************! %routine thinB(%bytearrayname image(0:128*512)) %integer row,col,s,i,j,k,n,past,msk %constantintegerarray mask(0:15,0:1)= 2_00001011,2_11011011, 2_11010000,2_11011011, 2_01101000,2_01111110, 2_00010110,2_01111110, 2_01001001,2_11111111, 2_00010011,2_11111111, 2_01110000,2_11111111, 2_01010100,2_11111111, 2_00101010,2_11111111, 2_10010010,2_11111111, 2_00001110,2_11111111, 2_11001000,2_11111111, 2_11010110,2_11111111, 2_11111000,2_11111111, 2_01101011,2_11111111, 2_00011111,2_11111111 %routine thinpointB(%bytearrayname image(0:128*512),%integer i,j,msk, %integername n) %integer test,res,a,b,c,d,e,f,g,h a=i-1; b=i+1; c=j<<9; d=(j+1)<<9; e=(j-1)<<9 test=(((((((image(a+d))<<1+image(i+d))<<1+image(b+d))<<1+image(a+c))<<1+ %c image(b+c))<<1+image(a+e))<<1+image(i+e))<<1+image(b+e) n=0 res=(test!!mask(msk,0))&mask(msk,1) %if res=0 %start {Matches mask} n=1 image(i+512*j)=0 {delete point} %finish %end fillimage(image) {Fill image before thinning} displayframe(px,py,image(0)) %cycle past=0 %for msk=0,1,15 %cycle printstring("Mask ");write(msk,3);newline %for row=1,1,126 %cycle %for col=1,1,510 %cycle k=row<<9+col s=image(k) %if s=1 %then thinpointB(image,col,row,msk,n) %if n=1 %then past=1 %repeat %repeat display frame(px,py,image(0)) %repeat %repeatuntil past=0 %end !-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----! !-----=======<<<<<<<<<< THE WORM >>>>>>>>>>>=======-----! !-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----! !*****************************************************! ! WORM ! ! Use Digital Worm to locate lines in image. ! !*****************************************************! %routine worm(%bytearrayname image(0:128*512), %record (linef) %arrayname lines(0:1000), %integername totnl,%integer w,th,cth) %integer m,i,j,k,l,x,y,tot,p, numpoints,numlines %integerarray list(0:100,1:2) %recordformat pointf(%integer x,y) %recordformat listofpoints(%integer num,%record(pointf) %array pt(0:1024)) %record(listofpoints) %array ptlist(0:128) !*****************************************************! ! FINDPOINTS ! ! Find all neighbours to current pixel. ! !*****************************************************! %routine findpoints(%integer x,y,%integername num, %integerarrayname points(0:7,1:2)) %integer i,j num=0 %for i=1,-1,-1 %cycle %for j=1,-1,-1 %cycle %if image(x+i+(y+j)<<9)=1 %start points(num,1)=x+i points(num,2)=y+j num=num+1 %finish %repeat %repeat %end !*****************************************************! ! PUSH ! ! Push point found onto end of current line. ! !*****************************************************! %routine push(%integer x,y, %record(listofpoints) %name ptlist) %integer np %integerarray list(0:7,1:2) image(x+y<<9)=0 ptlist_pt(ptlist_num)_x=x ptlist_pt(ptlist_num)_y=y ptlist_num=ptlist_num+1 findpoints(x,y,np,list) %if np#0 %then push(list(0,1),list(0,2),ptlist) %end !*****************************************************! ! FIND LINES ! ! Sort image into lines, where each line is a ! ! sequence of pixels. ! !*****************************************************! %routine find lines(%record(listofpoints) %arrayname ptlist(0:128), %integername numlines) %integer i,j,col,row,k,s,np,numpoints %integerarray list(0:7,1:2) numlines=0 %for col=1,1,511 %cycle %for row=1,1,126 %cycle k=row<<9+col s=image(k) %if s=1 %start image(k)=0 findpoints(col,row,np,list) %if np#0 %start %for i=0,1,np-1 %cycle ptlist(numlines)_num=1 ptlist(numlines)_pt(0)_x=col ptlist(numlines)_pt(0)_y=row push(list(i,1),list(i,2),ptlist(numlines)) numlines=numlines+1 %repeat %finish %finish %repeat %repeat %end !*****************************************************! ! DX ! ! Rectangular distance in the X direction ! !*****************************************************! %integerfunction dx(%integer n,l,w) %result=ptlist(l)_pt(n)_x-2*ptlist(l)_pt(n-w)_x+ptlist(l)_pt(n-2*w)_x %end !*****************************************************! ! DY ! ! Rectangular distance in the Y direction ! !*****************************************************! %integerfunction dy(%integer n,l,w) %result=ptlist(l)_pt(n)_y-2*ptlist(l)_pt(n-w)_y+ptlist(l)_pt(n-2*w)_y %end !*****************************************************! ! A ! ! Change in rectangular distance. If >0 then corner ! ! has been detected. ! !*****************************************************! %integerfunction a(%integer n,l,th,w) %result=|dx(n,l,w)|+|dy(n,l,w)|-th %end !*****************************************************! ! TX ! ! Accumulative value of rectangular distance in X dir! !*****************************************************! %integerfunction tx(%integer n,l,w,p) %integer res %if p>n-1 %then res=dx(n,l,w) %else res=tx(n-1,l,w,p)+dx(n,l,w) %result=res %end !*****************************************************! ! TY ! ! Accumulative value of rectangular distance in Y dir! !*****************************************************! %integerfunction ty(%integer n,l,w,p) %integer res %if p>n-1 %then res=dy(n,l,w) %else res=ty(n-1,l,w,p)+dy(n,l,w) %result=res %end !*****************************************************! ! C ! ! Accumulative total change in rectangular distance. ! ! If > 0 then curve has been detected. ! !*****************************************************! %integerfunction c(%integer n,w,p,cth) %result=|tx(n,l,w,p)|+|ty(n,l,w,p)|-cth %end findlines(ptlist,numlines) p=w*2 k=w*2 totnl=0 %for l=0,1,numlines-1 %cycle %if ptlist(l)_num>w*2 %start numpoints=1 list(0,1)=ptlist(l)_pt(0)_x {First point on line is an endpoint} list(0,2)=ptlist(l)_pt(0)_y %for i=w*2,1,ptlist(l)_num-1 %cycle j=a(i,l,th,w) {Send worm round image} %if j>0 %start {Corner detected} list(numpoints,1)=ptlist(l)_pt(i-w)_x list(numpoints,2)=ptlist(l)_pt(i-w)_y numpoints=numpoints+1 %finish %if k=i %start j=c(k,w,p,cth) %if j>0 %start {Curve detected} list(numpoints,1)=ptlist(l)_pt(i-w)_x list(numpoints,2)=ptlist(l)_pt(i-w)_y numpoints=numpoints+1 p=k-w k=p+2*w %finish %finish %if k=i %then k=k+1 %repeat list(numpoints,1)=ptlist(l)_pt(ptlist(l)_num-1)_x list(numpoints,2)=ptlist(l)_pt(ptlist(l)_num-1)_y numpoints=numpoints+1 %for i=0,1,numpoints-2 %cycle lines(totnl)_x1=list(i,1) lines(totnl)_y1=list(i,2) lines(totnl)_x2=list(i+1,1) lines(totnl)_y2=list(i+1,2) totnl=totnl+1 %repeat %finish %repeat %end !-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----! !-----=======<<<<<<<<<< ADMINISTRATIVE >>>>>>>>>>>=======-----! !-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----! !*****************************************************! ! CURSOR ! ! Place curor at position (x,y) and delete cursor at ! ! location (ox,oy) ! !*****************************************************! %routine cursor(%integername x,y,ox,oy) %if ox#x %or oy#y %start disable(RGBplane) set colour(black) line(ox-5,oy,ox+5,oy) line(ox,oy-5,ox,oy+5) setcolour(invert) %if x<6 %then x=6 %if y<26 %then y=26 %if x>214 %then x=214 %if y>464 %then y=464 line(x-5,y,x+5,y) line(x,y-5,x,y+5) disable(0) ox=x;oy=y %finish %end !*****************************************************! ! UPDATE STRING ! !*****************************************************! %routine update string(%integer box,%string(255)text) setcolour(black) fill(168,450-30*box,215,460-30*box) setcolour(yellow) textat(168,450-30*box) showstring(text) %end !*****************************************************! ! UPDATE NUMBER ! !*****************************************************! %routine update number(%integer box,val) setcolour(black) fill(168,450-30*box,215,460-30*box) setcolour(yellow) writenum(val,168,450-30*box) %end !*****************************************************! ! SETUPMENU ! ! Set up menu for initial use and display values in ! ! boxes. ! !*****************************************************! %routine setupmenu %integer i %conststring(18) %array menu(0:14)= "DISPLAY FRAME(S) ", "EXPOSURE ", "THIN METHOD A ", "THIN METHOD B ", "DISPLAY HOUGH? ", "HOME POSITION SET?", "NUMBER OF FRAMES ", "DUMP BITMAP ", "ROTATE TURNTABLE ", "MOVE SHOULDER ", "MOVE ELBOW ", "MOVE CAMERA U/D ", "MOVE CAMERA L/R ", "USE THE WORM? ", "QUIT " setcolour(black) fill(0,510,685,480) setcolour(red) box(0,510,685,480) setcolour(yellow) textat(10,490) showstring %c ("ML: MM: MR: MLMR:") %for i=0,1,14 %cycle setcolour(black) fill(0,470-i*30,220,470-(i+1)*30) setcolour(red) box(0,470-i*30,160,470-(i+1)*30) box(160,470-i*30,220,470-(i+1)*30) setcolour(yellow) textat(8,450-i*30) showstring(menu(i)) %repeat update number(1,exposure) %if homeset=0 %then update string(5,"NO") %else update string(5,"YES") update number(6,frames) %if dohough=0 %then update string(4,"NO") %else update string(4,"YES") %if dothina=0 %then update string(2,"NO") %else update string(2,"YES") %if dothinb=0 %then update string(3,"NO") %else update string(3,"YES") %if useworm=0 %then update string(13,"NO") %else update string(13,"YES") %end !*****************************************************! ! GETBOX ! ! Return value of box which cursor is currently in. ! !*****************************************************! %integerfunction getbox(%integer x,y) %result=14-((y-20)//30) %end !*****************************************************! ! HIGHLIGHT ! ! Highlight current box. ! !*****************************************************! %routine highlight(%integer bx) set colour(green) box(0,470-bx*30,220,440-bx*30) %end !*****************************************************! ! LOWLIGHT ! ! Remove highlight of old box. ! !*****************************************************! %routine lowlight(%integer bx) set colour(red) box(0,470-bx*30,220,440-bx*30) %end !*****************************************************! ! UPDATE VAL ! ! Update value in box ! !*****************************************************! %routine update val(%integer box) %if box=1 %start prompt("Enter new exposure:") read(exposure) update number(box,exposure) %finish %if box=2 %start showoptions("-","-","-","-") reply=yesorno("Thin Lines? yes/no: ") %if reply="YES" %then dothina=1 %else dothina=0 update string(box,reply) %finish %if box=3 %start showoptions("-","-","-","-") reply=yesorno("Thin Lines? yes/no: ") %if reply="YES" %then dothinb=1 %else dothinb=0 update string(box,reply) %finish %if box=4 %start showoptions("-","-","-","-") reply=yesorno("Display Hough? yes/no: ") %if reply="YES" %then dohough=1 %else dohough=0 update string(box,reply) %finish %if box=5 %start homeset=1 update string(box,"YES") %finish %if box=6 %start %cycle prompt("Enter new number of frames (1-4):") read(frames) %repeatuntil frames>0 %and frames<5 update number(box,frames) %finish %if box=7 %start reply=yesorno("Do you really want to dump the display? ") %if reply="YES" %start reply=yesorno("Output origional image? (If 'NO' then thinned image is output):") %if reply="YES" %then dump bitmap(orig) %else dump bitmap(image) %finish %finish %if box=8 %then move axis(0) %if box=9 %then move axis(1) %if box=10 %then move axis(2) %if box=11 %then move axis(3) %if box=12 %then move axis(5) %if box=13 %start showoptions("-","-","-","-") reply=yesorno("Use the Worm? yes/no: ") %if reply="YES" %then useworm=1 %else useworm=0 update string(box,reply) %finish %if box=14 %then %signalevent(15) showoptions("PICK","-","PICK","") %end !*****************************************************! ! UPDATE ENVIR ! ! Entered when a value is required to be altered ! !*****************************************************! %routine update envir %label disp %integer ch,cx,cy,bx %byte b setupmenu showoptions("PICK","-","PICK","") %cycle getbuttons(b,25) cx=mousex;cy=mousey cursor(cx,cy,cox,coy) bx=getbox(cx,cy) %if bx=0 %and b#0 %and b#msm %then -> disp %if bx#obx %then lowlight(obx) highlight(bx) obx=bx %if b#0 %and b#msm %then update val(bx) %repeat disp: clear set colour(green) box(85,0,598,2+frames*127) %cycle ch=testsymbol %repeatuntil ch=-1 %end !*****************************************************! ! ARROW ! ! Point to current frame being displayed. ! !*****************************************************! %routine arrow(%integer x,y) line(x,y,x-20,y+10) line(x-20,y+10,x-20,y+5) line(x-20,y+5,x-45,y+5) line(x-45,y+5,x-45,y-5) line(x-45,y-5,x-20,y-5) line(x-20,y-5,x-20,y-10) line(x-20,y-10,x,y) %end !*****************************************************! ! FRAMEARROW ! ! Change colour of arrow every new frame. ! !*****************************************************! %routine framearrow(%integer fr) %integer lfr lfr=fr-1 %if lfr<1 %then lfr=frames setcolour(black) arrow(80,-64+127*lfr) setcolour(arrowcolour) arrow(80,-64+127*fr) %if arrowcolour=yellow %then arrowcolour=magenta %else %c arrowcolour=yellow %end !*****************************************************! ! ! ! MAIN PROGRAM ! ! ! !*****************************************************! %on %event 15 %start reply=yesorno("Do you REALLY want to quit? ") %if reply="YES" %then->quit %else ->cont %finish set terminal mode(8) open interface initialise motors pia breg = 0 wait(2) pia breg = 16_ff wait(2) clear enable(255) mousex=200;mousey=200 setupcube(cx,cy,cz,vert) cont: update envir %cycle ch=testsymbol {loop interupted by pressing any key} %if ch#-1 %then update envir %for i=0,1,5 %cycle;pulses(i)=0;%repeat pulses(3)=steps i=1 %cycle clear frame(image(0)) {Clear all copies of image} clear frame(copy1(0)) clear frame(copy2(0)) clear frame(orig(0)) framearrow(i) py=1+(i-1)*127 camera {Obtain image from camera} display frame(px,py,image(0)) {Display it} %if dothina=1 %start {Thin it} printstring("Thinning image....");newline thinA(image,copy1) display frame(px,py,image(0)) printstring("Thinned.");newline %finish %if dothinb=1 %start {thin it} printstring("Thinning image....");newline thinB(image) display frame(px,py,image(0)) printstring("Thinned.");newline %finish %if dohough=1 %start {Use Hough Transform} hough(image,copy2,lines,numlines) display frame(px,py,orig(0)) {Re-display frame} setcolour(white) openoutput(2,"image2") {Output to High-level} selectoutput(2) write(numlines,3);newline newline %for j=0,1,numlines-1 %cycle {Display lines found} write(int(xmult*lines(j)_x1),5);write(lines(j)_y1,5) write(int(xmult*lines(j)_x2),5);write(lines(j)_y2,5) print(lines(j)_m/xmult,5,5);newline line(px+lines(j)_x1,py+lines(j)_y1,px+lines(j)_x2,py+lines(j)_y2) %repeat closeoutput;selectoutput(0) permit("image2","FRA") prompt("Waiting...");read(reply) %finish %if useworm=1 %start {Use The worm} thinb(image) {Automatically use Thinning Alg. B} %for j=0,1,128*512 %cycle {Copy of thinned image} copy1(j)=image(j) %repeat %cycle displayframe(px,py,image(0)) {Display thinned image} prompt("Worm length:");read(w) {Worm parameters} prompt("Threshold:");read(th) prompt("Curve Threshold:");read(cth) worm(image,lines,numlines,w,th,cth) {Find lines} col=blue %for j=0,1,numlines-1 %cycle {display Lines} colour(col) line(px+lines(j)_x1,py+lines(j)_y1,px+lines(j)_x2,py+lines(j)_y2) %if col=blue %then col=green %else col=blue %repeat reply=yesorno("OK?") {Cycle until output OK} %if reply="NO" %start %for j=0,1,128*512 %cycle image(j)=copy1(j) %repeat %finish %repeatuntil reply="YES" %finish i=i+1 %if i<=frames %then move axes(0,pulses) {Move camera up if more than } %repeatuntil i>frames {one frame to be taken} pulses(3)=steps*(frames-1) move axes(lens,pulses) {Move camera back down} %repeat quit: %endofprogram