org 100h 
    
    jmp main
    
port    equ 378h    
    
main:

    mov ah,00h
    int 16h
 

    cmp al,'Q'
    jne l5 
    ret
    
l5: mov bx,offset key_table 
l1: mov cl,[bx]
    cmp al,cl
    je l4
    add bx,3
    cmp bx, key_table_end
    jl l1   
    jmp main


l4: mov al,[bx+1]
    mov cl,[bx+2]
                
    mov direction,cl
    call motor_number
    
    mov dx,100
l2: call drive
    dec dx
    jnz l2  
    
    call motor_number
    call motor_off
      
    jmp main

;key table format - KEY , MOTOR #, DIRECTION
    
key_table:
    db 'A',1,0    
    db 'Z',1,1
    db 'S',2,0
    DB 'X',2,1
    DB 'D',3,0
    DB 'C',3,1
    DB 'F',4,0
    DB 'V',4,1
    DB 'G',5,0
    DB 'B',5,1
    DB 'H',6,0
    DB 'N',6,1

key_table_end = $       
    
;*************************************************************
;Name:          motor_number
;
;Author:        Dan Kohn
;
;Date:          12/14/2006
;
;Function:      select the motor to move 
;
;On Call:       AL contains motor number (0-7) where:
;                    0 -> Aux #1
;                    1 -> Wrist 
;                    2 -> gripper
;                    3 -> wrist
;                    4 -> elbow
;                    5 -> base
;                    6 -> shoulder
;                    7 -> Aux #2
;
;On Return:     All Registers Restored

motor_number:
    push bx
    push ax
    
    mov motor_num,al
    mov bx,offset motor_table
    mov ah,00
    add bx,ax
    mov al,[bx]
    mov motor,al
    
    pop ax
    pop bx
    ret
    
    
motor_table:
    db  0000_0000b
    db  0000_0010b
    db  0001_0000b
    db  0001_0010b
    db  0010_0000b
    db  0010_0010b
    db  0011_0000b
    db  0011_0010b    
      
motor       db 0 
motor_num   db 0  

 
;*************************************************************
;Name:          motor_off
;
;Author:        Dan Kohn
;
;Date:          12/14/2006
;
;Function:      turns off all coils of selected stepper motor
;
;On Call:       motor_num contains motor number to turn off
;
;On Return:     All Registers Restored

motor_off:
    push ax
    push dx
    
    mov al,1100_1100b       
    or al,motor
    mov dx,port
    out dx,al
    call time_delay
    or al,01h                                 
    out dx,al
    call time_delay
    
    pop dx
    pop ax
    ret
    
;*************************************************************
;Name:          drive
;
;Author:        Dan Kohn
;
;Date:          12/14/2006
;
;Function:      Steps motor (selected via "motor_num routine") one time
;
;On Call:       motor contains mask of motor to move
;               motor_num contains motor number to move
;               direction contains 0 for one direction, 1 for reverse direction
;
;On Return:     All Registers Restored

direction   db 0
motor_count db 8 dup(0)    

drive:
    push ax
    push bx
    push cx
    push dx
    
    mov bx,offset motor_count
    mov ah,0
    mov al,motor_num
    add bx,ax
    
    mov cl,[bx]
    
    mov dl,direction
    and dl,1
    
    jnz dr_l1
    
    inc cl
    jmp dr_l2    
    

dr_l1:
    dec cl
    
    
dr_l2:
    mov [bx],cl
    
  
    mov bx,offset step_table
    mov al,cl
    mov ah,0
    mov dl,4
    
    div dl
    
    mov al,ah
    mov ah,0
    add bx,ax
    
    mov al,[bx]       
    or al,motor
    mov dx,port
    out dx,al
    call time_delay
    or al,01h                                 
    out dx,al
    call time_delay  
    
    pop dx
    pop cx
    pop bx
    pop ax
    
    ret
             
     
step_table:
    db 1100_1000b
    db 0100_1100b
    db 1100_0100b
    db 1000_1100b

;old
;    db 0000_0100b
;    db 1000_0000b
;    db 0000_1000b
;    db 0100_0000b
    
     
step_table_end = $   
    

    
;*************************************************************
;Name:          TIME_DELAY
;
;Author:        Dan Kohn
;
;Date:          10/30/2005
;
;Revised:       10/31/2005
;               PUSHA does not work on Toshiba XT Compatable
;               laptop. Replaced with individual push and pops.
;
;Function:      Wait for ??
;
;On Call:       NONE
;
;On Return:     All Registers Restored

time_delay: push ax         ;put all reg on stack for restore
            push bx
            push cx
            push dx

            mov ah,86h
            mov cx,0
            mov dx,2500
            int 15h
                        
            pop dx            ;restore all registers
            pop cx
            pop bx
            pop ax
            RET