!*******************************************************************************
!*                                                                             *
!*        MicronEye Optic RAM camera / Cyber Robot arm  Routine library        *
!*                                                                             *
!*                  John Butler, Ian Gebbie, Brian Craigie                     *
!*                                                                             *
!*                        Version 1.1 31 Oct 1986                              *
!*                                                                             *
!*******************************************************************************

%option "-nons-nowarn"
%include "inc:util.imp"
%include "camera:imgraph.inc"
%include "camera:region.inc"

%begin

%bytearray epic(0:128*512)

%integer       x,y,
               i,j,ch,inc,

%integer       exposure,
               initial,
               final,
               grey,
               gnog,
               step,
               add,
         ref,size,i,
               strobe=128,
               outtofile=0,
               obx=0,
               cox=250,
               coy=250,
               px=86,
               py=1
;

%byte          a,b,c

%string (255)  file,reply


%bytearray     pic(0:65535)
%bytearray     image(0:128*512)


%constinteger  base = 16_400FF0,
shutprio=0,    aperprio=16_100,
               tag = 16_C0,
alt=0,         noalt=32,
wide=0,        narrow=16,
sevenbit=0,    eightbit=8,
onearray=0,    twoarray=4,
refresh=0,     soak=2,
send=0,        nosend=1

@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

!******************************************************************************!
!                                  OPEN ACIA                                   !
!******************************************************************************!
%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                                       !
!******************************************************************************!
%routine outch(%integer ch)
   %integer c, time
   time=cputime+1000
   %cycle
       c = acia cont
       %exit %if c&2 # 0
!       %signal 15,1 %if time=cputime
       %signal 15,1 %if time >= cputime  {BSC !}
   %repeat
   acia data = ch
%end


!******************************************************************************!
!                                  GRAB FRAME                                  !
!******************************************************************************!
%externalroutine grab frame(%integer command, %integername exposure, 
%integer address, %integername rows,cols,bytes)
      %integer start, end, max, min, tot, previous
{s}   %integer time
      %real ave
      %label waitch

      %routine photo(%integer exposure)
{s}      %integer time1,time2
{s}      time1 = cputime
         wait(2)
         command = (command & (\soak)) ! tag ! nosend
         outch(command)
      
         wait(5)
         command = command ! soak
         outch(command)

         wait(exposure)
         command = (command & (\nosend)) ! noalt
{s}      time2 = cputime
         outch(command)

         *movea.l start,a0
         *movea.l end,a1
 
 waitch:
         *btst    #0,aciacont
         *beq     waitch
         *move.b  aciadata,0(a0)
         *adda.l  #1,a0
         *cmpa.l   a0,a1
         *bne     waitch
 
         command = (command & (\soak)) ! nosend
         outch(command)
{s}      printstring("Photo     ")
{s}      write(time2-time1, 5);  write(cputime-time2, 5); write(bytes, 5); newline
      %end

{s}   time = cputime
      rows=64; cols=16
      %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
      start = address; end = address+bytes
 
      open acia


      %if command & aperprio # 0 %start
         !Aperture priority: "Exposure" is the desired average pixel value.
         !We loop, varying the exposure until the desired average is achieved.
         exposure=255 %if exposure>255; average=exposure
         !Set arbitrary tolerance of 10% either way
         avemin = (exposure*9)//10; avemax = (exposure*11)//10
         avemax = 255 %if avemax>255
         
         exposure = 100 ;!Got to start somewhere...
         %cycle
            photo(exposure)

            !Work out average pixel value
            tot=0
            %for i=start,1,end-1 %cycle
               tot=tot+byteinteger(i)
            %repeat
            ave = tot/bytes
            write(exposure, 4); print(ave, 7, 3); newline

            !"Servo" algorithm - just something to get us off the ground:
            !Divide by 2 if saturated...
            %if ave>254 %then exposure = exposure//2 %else %c
            previous = exposure
            !If not, adjust exposure in proportion to difference from desired
            !endpoint.   This is slightly prone to oscillation.
            exposure = exposure + 2*int(average-ave)
            !Failed to get an image (too bright)
            previous=-1 %and %exit %if exposure<=0
        %repeatuntil avemin<=ave<=avemax
        exposure = previous
     %else
        photo(exposure)
     %finish
{s}  printstring("Grab Frame"); write(cputime-time, 5); newline
 %end

!******************************************************************************!
!                                  DISPLAY FRAME                               !
!******************************************************************************!
%externalroutine display frame(%integer x,y,%bytename line)
%label ok
!s!%integer time
!s!time=cputime

  *move.l d0,d2
  *lsl.l #7,d1
  *andi.l #16_FFFFFFF0,d0
  *lsr.l #3,d0
  *add.l d0,d1
  *lea   frame,a1
  *lea   0(a1,d1.l),a1
  *move.l a1,a3
  *adda.l #128,a3
  *lea   colourreg,a2
  *andi.l #15,d2
  *eor.l  #15,d2
  *clr.l  d1
  *bset   d2,d1
  *moveq  #127,d3
z:*move.l  #511,d0
a:*move.b (a0)+,(a2)
  *move.w d1,(a1)
  *ror.w #1,d1
  *btst  #15,d1
  *beq   ok
  *lea    2(a1),a1
ok:
  *dbra   d0,a
  *move.l a3,a1
  *adda.l #128,a3
  *bset   d2,d1
  *dbra   d3,z
!s!printstring("DisplayFrame"); write(cputime-time, 5); newline
%end


!******************************************************************************!
!                                  TRANSFORM                                   !
!******************************************************************************!
%externalroutine transform(%bytearrayname pic(0:65535),
                                         %bytearrayname image(0:128*512))
%integer r,c,i,j,k,l,pix,x,y
{s}%integer time
{s}time = cputime

%for r=0,1,127 %cycle
  %for c=0,1,31 %cycle
    k = (127-r)<<5+c
    %for j=7,-1,0 %cycle
      l=c<<3 + j
      %if pic(k)&(1<<j)#0 %then pix=1 %else pix=0
      y=r; x=l<<1+1
      %if (r&1=0 %and l&1=0) %or (r&1#0 %and l&1#0) %then %c
      y=y-1 %and x=x-3
      %if (x&1)=0 %then y=y+2
      i = y<<9+x
      image(i)=image(i)+pix %if 0<=x<=511 %and 0<=y<=127
    %repeat
  %repeat
%repeat
{s}printstring("Transform "); write(cputime-time, 5); newline
%end

!-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----!
!-----=======<<<<<<<<<<  ROBOT CONTROL  >>>>>>>>>>=======-----!
!-----=======<<<<<<<<<<<<<<<<<->>>>>>>>>>>>>>>>>>>=======-----!

!*****************************************************!
!         PULSE                                       !
!  Sends a single pulse to the robot.                 !
!*****************************************************!
%externalroutine 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.                                              !
!*****************************************************!
%externalroutine 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<<i) %and pulses(i)=pulses(i)-1
      %repeat
      pia breg = mep
      pulse(pulsewdth)
      %if pulses(0)<=0 %then pulsewdth=5
   %repeatuntil mep=0
%end

!*****************************************************!
!         INITIALISE MOTORS                           !
!*****************************************************!
%externalroutine initialise motors   {Switch all motors off}
   pia breg = 0
   pia breg = strobe
   wait(1)
   pia breg = 0
%end

!*****************************************************!
!         OPEN INTERFACE                              !
!  Initialise interface to robot.                     !
!*****************************************************!
%externalroutine open interface
   pia acont = 0
   pia bcont = 4
   pia breg = 0
   pia bcont = 16_30
   pia breg  = 16_BF
   pia bcont = 16_34
%end

!*****************************************************!
!         FILLIMAGE                                   !
!  Routine fills out blank values in image array.     !
!  If blank pixel has two or more neighbours set to 1 !
!  then set pixel to a 1 as well                      !
!*****************************************************!
%externalroutine fillimage(%bytearrayname image(0:128*512))
   %integer i,j,col,row,k,np
{s}%integer time
{s}time = cputime
   i=0;j=0
   %for col=1,1,511 %cycle
     %for row=2+i,2,124+i %cycle
       k=row<<9+col
       np=image(k-512)+image(k+512)
       %if j=0 %then np=np+image(k-1) %else np=np+image(k+1)
       %if np=2 %or np=3 %then image(k)=1 %else image(k)=0
     %repeat                          
     %if j=0 %then j=1 %elsestart
       %if i=1 %then i=0 %else i=1
       j=0
     %finish
   %repeat
{s}printstring("Fill image"); write(cputime-time, 5); newline
%end

!******************************************************************************!
!                                  CAMERA                                      !
!******************************************************************************!
%externalroutine camera
   %integer command,actbytes,rows,cols,i

%for i=0,1,grey*15 %cycle { Grey either 0 or 1 }
   command = noalt ! narrow ! eightbit ! onearray
   printstring("Exposure=");write(exposure,3);newline

   grab frame(command, exposure, addr(pic(0)), rows,cols, actbytes)
   transform(pic,image)
   display frame(px,py,image(0))
   exposure = exposure + step %if grey#0
%repeat
%end

!******************************************************************************!
!                                  CLEAR FRAME                                 !
!******************************************************************************!

%externalroutine clear frame(%bytename base)
%label LP
{s}%integer time
{s}time=cputime
    *MOVEQ   #-4,D0
LP: *ADDQ.L  #4,D0
    *MOVE.L  #0,0(A0,D0.L)
    *CMPI.L  #16_10000,D0
    *BNE     LP
{s}printstring("ClearFrame"); write(cputime-time, 5); newline
%end

!******************************************************************************!
!                                  DUMP BITMAP                                 !
!                           Dumps image to a file.                             !
!******************************************************************************!
%externalroutine dump bitmap(%bytearrayname image(0:128*512),%string(255) file)
%integer ref,size
  open output(1,file)
  select output(1)
  printsymbol('*')
  closeoutput
  selectoutput(0)
  accessfile(file,1,ref,size)
  writeregion(ref,0,128*512,image(0)) 
  deaccessfile(ref)
  printstring("Done");newline
%end

!------------------------------------------------------------------------------!
!                      M  A  I  N    P  R  O  G  R  A  M                       !
!------------------------------------------------------------------------------!

   set terminal mode(8)

   final = 0
   grey=0

!!   prompt("Grey/Nogrey ?")
!!   readsymbol(gnog)
!!   readline(file)
!!   %if (gnog & 16_5f) ='G' %then grey = 1
   %if grey=0 %then prompt ("Exposure (0 for Auto)?") %else %c
   prompt("Initial exposure (0 for Auto)?")
   read(initial)
{} exposure = initial
!!   readline(file)
   %if grey=1 %then %start
      prompt("Final exposure ?")
      read(final)
      readline(file)
   %finish
   prompt(":")
   step = int ((final - initial)/15) { 15 different greys. }
   %if grey=0 %then add=32 %else add=0
   colour map(i+add)=i<<10!i<<5!i %for i=0,1,31
   clear
   enable(255)

    clear frame(image(0))
{}%cycle
{}  clear frame(image(0))
    camera
     display frame(px, py+128, image(0))
{}    fillimage(image)
     display frame(px, py+128, image(0))
!!    prompt("Fill image?") ; readline(file)
!!    %if file = "y" %then fillimage(image) %and display frame(px,py+128,image(0))

!!    prompt("output file ?") ; readline(file)
!!    %if file # "" %then dump bitmap(image,file)
{}%repeat
 %endofprogram