Как написать функцию, проверяющую работу кода

Как ввести функцию чтобы проверить работу программы. Дана задача: В робототехнике одной из классических проблем является задача определения того, как робот должен перемещаться в окружающем пространстве, чтобы перейти от текущего положения в некоторую конечную позицию, при этом, например, избегая столкновений и/или минимизируя время движения. Реализовать алгоритм решения задачи о планировании грузового робота. Робот двигается в плоскости, движение осуществляется дискретно с шагом длины 1, по четырем направлениям север-юг-восток-запад. Робот может поднять одну ед. груза и опустить одну ед. груза (в любой соседней с ним клетке). На плоскости в некоторых ячейках (клетках) размера 1 × 1 сосредоточены грузы. Задача состоит в нахождении последовательности действий робота для перемещения всех грузов в заданную клетку. Начальное положение робота задано.

Так же дана полностью рабочая программа:

(defun shag (start sp)
(cond
 ((prin start sp) nil)
 ((prin start '((1 1) (2 1) (2 2) (2 3) (3 3) (3 4) (4 4) 
              '((1 1) (2 1) (3 1) (4 1) (4 2) (5 2) ( 5 3) (5 4)(4 4))) 
              '((1 2) (1 3) (1 4) (1 5) (2 5) (3 5) (4 5)(4 4)) nil)))
 '((> (NTH 0 start) 2) nil)
 '((> (NTH 1 start) 3) nil)
 '((< (NTH 1 start) 0) nil)
 (t T))
 
;
(defun robot (start finish is)
    (cond
        ((null start) nil)
        ((equal start finish) (reverse (cons start is)))
        ((shag (up start) sp) (robot (up start)
                                     finish (cons start sp) (cons start is)))
        ((shag (right start) sp) (robot (right start)
                                        finish (cons start sp) (cons start is)))
        ((shag (left start) sp) (robot (left start)
                                       finish (cons start sp) (cons start is)))
        ((shag (down start) sp) (robot (down start)
                                       finish (cons start sp) (cons start is)))(
                                                                                (t (robot (car is) finish (cons start sp) (cdr is))))))