Tried to fix some stuff and add sme smart features

Hvis denne ikke fungerer revert til More from the lab committen. Har
tatt litt av koden vi lagde på laben og modifisert det slik at den skal
oppføre seg nesten helt som tilstandsmaskinen tilsier.