Adapted the code to make it easier for me to read. Found some bugs and corrected them (if( x =3 ) should be if(x == 3))
Changed some parameters on control_motion. Control_logic is too simple to work properly.