Cleaned up code

Removed redundant parts from autofocus and wormImage