Fixed index out of bounds error in guidance. Removed debug logging in motion control