0

(Snippet) Manhattan distance

simon987 edited this page 2019-11-03 09:23:25 -05:00

This simple program calculates the Manhattan distance between the current position of the Cubot and another tile.

; Define constants
HWID_LIDAR equ 0x3
HWID_HOLO  equ 0x9

HOLO_DISPLAY_HEX equ 0x1
LIDAR_GET_POS    equ 0x1

; Data segment
.data
    counter: DW 0x0000     ; Counter global variable
    ; The DW (Define word) directive writes the specified value(s), seperated
    ; by a comma at assembly time. You can define a label before the directive
    ; to refer to it by name.

; Code/executable segment
.text
    ; Program entry point
    call getPos         ; Execute procedure
    
    PUSH 7              ; y2
    PUSH 11             ; x2
    call manhattanDist  ; Execute procedure
    
    TEST [counter], 1   ; Display 'ABCD' every odd tick
    JNZ counter_is_odd
    MOV A, 0xABCD

counter_is_odd:
    ADD [counter], 1        ; Increment counter
    MOV B, A
    MOV A, HOLO_DISPLAY_HEX
    HWI HWID_HOLO           ; Display the contents of A
    BRK                     ; Exit program
    
;**************************************
;* getPos()
;* X = X pos
;* Y = Y pos
getPos:
    PUSH A                ; Save A
    MOV A, LIDAR_GET_POS 
    HWI HWID_LIDAR        ; Send hardware interrupt to the LiDAR
    POP A                 ; Restore A
    RET
    

;**************************************
;* manhattanDist(x2,y2)
;* A = Distance between (X,Y) and (x2,y2)      
manhattanDist:
    PUSH BP             ; Save previous stack frame
    MOV BP, SP
    
    ; The old value of BP is at [BP + 0] 
    ; The return address is at [BP + 1]
    ; The x2 argument is at [BP + 2]
    ; The y2 argument is at [BP + 3]

    ; formula: |x1-x2| + |y1-y2|
    SUB X, [BP + 2]     ; X = (X - x2)
    
    ; A fancy way to get the absolute value of X
    MOV A, X            ; Copy X
    SAR A, 15           ; Fill A with sign
    XOR X, A            ; Do 'NOT X' if negative
    SUB X, A            ; Do 'ADD X, 1' if negative
    
    SUB Y, [BP + 3]     ; Y = (Y - y2)
    ; A simpler way to get the absolute value of Y
    JNS y_is_positive
    NEG Y               ; If Y was negative, it is NEG'd
y_is_positive:

    MOV A, X            ; A = |x1-x2|    
    ADD A, Y            ; A = |x1-x2| + |y1-y2|
    
    POP BP              ; Restore the previous stack frame
    RET 2               ; Return and POP our 2 arguments