Modifierad kod för nya sensor uppsättningen

Har även gjort om regleringen lite funkar fint.
Öar och sånt är den kod som är tänkt för att utforska resten av kartan
som krävs för krav 1.