Testing code with different motors and check if incremental position smooth out motion of robot