fixed compile issues i think
it works!, now to try to finalize the code, maybe at obstacle avoidance?