Przepisanie i debug głównego skryptu sterowania robotem

• Poprawiono działanie skryptu, teraz śmiga na callback'ach.
• Robot nie dostaje spazmów.
• Robot poprawnie zatrzymuje się podczas mierzenia boxa.
• Dodano ładne obłoczki nad punkty gdzie robot dojeżdża.
• Radek też tu był ale nie mogę go dodać do Issue