Create endofnigthcode

Alot of things work now they just need a little fine tuning
The robot now stops at the stop box.
If also kind of does sharp turns.