The obstacle following behaviour seems to work well now, but I have problems leaving an obstacle when I reach the line to the goal, so I will have to fix that