Added the pathfinding module

nodes/robots/modules/path_finder_module.gd

@@ -0,0 +1,75 @@
+extends Module
+
+var enabled := true
+
+var pos_x_register_index:int
+var pos_y_register_index:int
+var pos_z_register_index:int
+var index_register_index:int
+
+var prev_index := 0
+var target := Vector3i.ZERO
+var path : Array[Vector3i]
+
+func _ready() -> void:
+	if Terrain.instance == null:
+		print("No terrain to plan path on")
+	if cpu.get_parent() is not Robot:
+		print("Module not part of Robot")
+
+func get_register_reservation_size()->int:
+	return 4
+
+func set_reserved_registers(index:int)->void:
+	pos_x_register_index = index
+	pos_y_register_index = index+1
+	pos_z_register_index = index+2
+	index_register_index = index+3
+
+func get_register_types()->Dictionary[int, String]:
+	return { 
+		pos_x_register_index: "[X] Path", 
+		pos_y_register_index: "[Y] Path",
+		pos_z_register_index: "[Z] Path",
+		index_register_index: "Idx Path"
+		}
+
+func early_tick()->void:
+	var registers := cpu.cobor_vm.get_registers()
+	prev_index = registers[index_register_index]
+
+func late_tick()->void:
+	var registers := cpu.cobor_vm.get_registers()
+	var index := registers[index_register_index]
+	if index < 0:
+		target = Vector3i(
+			registers[pos_x_register_index],
+			registers[pos_y_register_index],
+			registers[pos_z_register_index]
+		)
+		print("Target set: ", target)
+	else:
+		if index == prev_index:
+			print("index stays same")
+			return
+		if prev_index < 0:
+			var path_finder := VoxelAStarGrid3D.new()
+			path_finder.set_terrain(Terrain.instance)
+			path_finder.set_region(AABB(cpu.get_parent().global_position + Vector3.ONE * -100, Vector3.ONE * 200))
+			path = path_finder.find_path(cpu.get_parent().global_position, target)
+			if not path.is_empty():
+				path.append(target)
+			print("Recalculating path of size:", path.size(), " towards: ", target)
+		if index >= path.size():
+			index = path.size()-1
+		var step:Vector3i
+		if path.size() == 0: 
+			step = Vector3i.ZERO
+		else:
+			step = path[index]
+		print("next step: ", step)
+		registers[pos_x_register_index] = step.x
+		registers[pos_y_register_index] = step.y
+		registers[pos_z_register_index] = step.z
+		registers[index_register_index] = index
+		cpu.cobor_vm.set_registers(cpu.register_size, registers)