Fixed all stupid bugs (tested on simulation with LEDs instead of motors)
Added code comments