Added basic autofocus support,not in main program yet. easier to test this way.