A Star movement test problem

Strict
Import "pathfinder.bmx"

Graphics 1280,1024

Global ship:TImage=LoadImage("ship1.png")
Global grass:TImage=LoadImage("grass.png")
Global rock:TImage=LoadImage("rock.png")
Global pathball:TImage=LoadImage("pathball.png")
Global rectangle:TImage=LoadImage("rectangle.png")
' ----------------------------  SET UP A STAR
Global Grid:Float[20, 20]
' THE TYPE TO HOLD THE PATHPOINTS
Global pathpoints:TList=CreateList()
Global path:Int[]
Global pathcount:Int=0  ' DENOTES WHERE WE CURRENTLY ARE ON THE PATH
Const TS = 80
Global fx, fy
Global my, mx
Local startx = 3, starty = 3,endx=1,endy=1
Local i
Global moveonpath=False
Global finishedmove=False
Global angle:Float=0
Global speed=0
Global dx:Float,dy:Float,shipx:Float,shipy:Float
'add obstacles to grid map
Grid[3,1]=1
grid[5,5]=1 
grid[5,4]=1
Grid[5,3]=1
Grid[10,10]=1
Grid[8,8]=1
Grid[8,10]=1
Grid[8,9]=1

TPathfinder.SetUp(Grid, 1, .03)
' --------------------------------------------------   MAIN PROGRAM LOOP
While Not KeyHit(key_escape)
	mx = MouseX() /TS
	my = MouseY() /TS
	mx=Max(mx,0); mx=Min(mx,19)
	my=Max(my,0); my=Min(my,19)
'----------------------------------------- LEFT MOUSE= START COORDINATES	
If MouseDown(1) Then
'reset path stuff
moveonpath=False
pathcount=0
ClearList(pathpoints)
'
If(Grid[mx,my]=0) Then
	startx=mx
	starty=my
	End If
	shipx=startx*TS
	shipy=starty*TS
End If	
' -----------------------------------------RIGHT MOUSE=END COORDINATES
If MouseDown(2)
pathcount=0
moveonpath=True
endx=mx
endy=my
dx=MouseX()
dy=MouseY()
speed=2
'COPY THE PATH ROUTE SO WE CAN DRAW IT LATER
	If TPathfinder.FindPath(startx, starty, endx,endy) Then
		path=TPathfinder.Route
	End If
'-------------------------- CONVERT THE PATH ARRAY INTO X AND Y COORDINATES
ClearList(pathpoints)
Local c=0
	For i=0 Until path.length Step 2
	Local ppoint:pathpoint
	ppoint=New pathpoint
	ppoint.px=path[i]*TS
	ppoint.py=path[i+1]*TS
	ppoint.count=c
	ListAddLast(pathpoints ,ppoint)
	c=c+1
	Next
	'we need to flip the list
	ReverseList(pathpoints)
'-----------------------------	
End If
'DRAW THE MAP
	For fy = 0 To 19
		For fx = 0 To 19
			If Grid[fx, fy]=0 Then
				DrawImage grass,fx * TS, fy * TS
			End If
			If Grid[fx, fy] >0 Then
				DrawImage rock,fx * TS, fy * TS
			End If
		Next
	Next
	'------------------------------- MOVE AND  DRAW PATH
	If moveonpath=True
	moveShip()
		For Local ppoint:pathpoint=EachIn pathpoints
		DrawImage pathball,ppoint.px,ppoint.py
		DrawRect ppoint.px,ppoint.py,5,5
		DrawText "Count="+ppoint.count,ppoint.px,ppoint.py+20
		Next	
	End If
	' DRAW MOUSEOVER RECTANGLE
	DrawImage rectangle,mx*TS,my*TS
'------------------------------------  DRAW THE SHIP
DrawImage ship,shipx,shipy
DrawRect shipx,shipy,5,5
DrawText pathcount+"/"+CountList(pathpoints),shipx,shipy-30

DrawText "LEFT CLICK TO PLACE START POINT, RIGHT CLICK TO PLACE END POINT",400,60
	Flip
	Cls
Wend
'-------------------------------------------------------------------   END MAIN LOOP
'
'-------------------------------------------   FUNCTIONS
'
'--------------------------------------------- GET DESTINATION FUNCTION
Function getDestination()
'get the pathpoint at the pathcount index
If(ListIsEmpty(pathpoints)=False)
Local p:pathpoint = pathpoint( pathpoints.ValueAtIndex( pathcount ) ) 
dx=p.px
dy=p.py
End If
'IF WE ARE AT THE AT THE NODE, INCREMENT THE PATHCOUNT
	If pointInRect(shipx,shipy,dx,dy,5,5)=1
	pathcount=pathcount+1
	End If
	
'IF WE ARE AT THE END OF THE PATH, RESET EVERYTHING
If pathcount=>CountList(pathpoints)
pathcount=0
ClearList(pathpoints)
moveonpath=False
End If
'====================================================
If pathcount=>CountList(pathpoints) Then pathcount=0
End Function
'-------------------------------------------------------------POINT IN RECT FUNCTION
Function pointInRect(sx,sy,rectx,recty,rectwidth,rectheight)
	Local inRect:Int=0
	If (sx=>rectx-rectwidth And sx=<rectx+rectwidth)
		If(sy=>recty-rectheight And sy=<sy+rectheight)
		inRect=1
		End If
	End If
	Return inRect
End Function
'===================================================     MOVE SHIP FUNCTION
Function moveShip()
getDestination()
angle=ATan2(dy-shipy,dx-shipx)
shipx=shipx+Cos(angle)*speed
shipy=shipy+Sin(angle)*speed
'
End Function
'
'-----------------------------------------------------   TYPES
Type pathpoint
Field px:Float
Field py:Float
Field count:Int
End Type