AndroidStudioでカメラのオートフォーカスが上手くいきません

前提・実現したいこと

AndroidStudioでカメラから取り込んだ映像を映し出すシステムを作っています。オートフォーカスを実装したのですが、カメラアプリを立ち上げて焦点を変えないまでは焦点が合っているのですが焦点を変えてしまうと上手く焦点が合いません（例えば、カメラが遠くを写している状態から近くを写した時に焦点が合わない）何か解決法がありましたらご伝授いただきたいです。

該当のソースコード

import java.util.Collections;
public class MainActivity extends Activity {
    private Camera mCamera;
    private static final String TAG = "M Permission";
    private int REQUEST_CODE_CAMERA_PERMISSION = 0x01;

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);


        TextureView textureView = (TextureView) findViewById(R.id.texture_view);
        textureView.setSurfaceTextureListener(new TextureView.SurfaceTextureListener() {
            @Override
            public void onSurfaceTextureAvailable(SurfaceTexture surface, int width, int height) {




                Button bt_red = (Button) findViewById(R.id.Red);
                Button bt_green = (Button) findViewById(R.id.Green);

                bt_red.setOnClickListener(new View.OnClickListener() {
                    @Override
                    public void onClick(View view) {
                        FrameLayout frameLayout = (FrameLayout) findViewById(R.id.aho);
                        frameLayout.setBackgroundColor(Color.argb(127,204,0,0));
                    }
                });

                bt_green.setOnClickListener(new View.OnClickListener(){

                    @Override
                    public void onClick(View view) {
                        FrameLayout frameLayout = (FrameLayout) findViewById(R.id.aho);
                        frameLayout.setBackgroundColor(Color.argb(127,0,100,0));
                    }
                });



                    mCamera.open();

            }

            @Override
            public void onSurfaceTextureSizeChanged(SurfaceTexture surface, int width, int height) {
            }

            @Override
            public boolean onSurfaceTextureDestroyed(SurfaceTexture surface) {
                return false;
            }

            @Override
            public void onSurfaceTextureUpdated(SurfaceTexture surface) {
            }
        });
        //setContentView(textureView);
        mCamera = new Camera(textureView);

    }


    class Camera {
        private CameraDevice mCamera;
        private TextureView mTextureView;
        private Size mCameraSize;
        private CaptureRequest.Builder mPreviewBuilder;
        private CameraCaptureSession mPreviewSession;

        private CameraDevice.StateCallback mCameraDeviceCallback = new CameraDevice.StateCallback() {
            @Override
            public void onOpened(CameraDevice camera) {
                mCamera = camera;
                createCaptureSession();
            }

            @Override
            public void onDisconnected(CameraDevice camera) {
                camera.close();
                mCamera = null;
            }

            @Override
            public void onError(CameraDevice camera, int error) {
                camera.close();
                mCamera = null;
            }
        };

        CameraCaptureSession.StateCallback mCameraCaptureSessionCallback = new CameraCaptureSession.StateCallback() {
            @Override
            public void onConfigured(CameraCaptureSession session) {
                mPreviewSession = session;
                updatePreview();
            }

            @Override
            public void onConfigureFailed(CameraCaptureSession session) {
                Toast.makeText(MainActivity.this, "onConfigureFailed", Toast.LENGTH_LONG).show();
            }
        };

        public Camera(TextureView textureView) {
            mTextureView = textureView;
        }

        public void open() {
            try {
                CameraManager manager = (CameraManager) getSystemService(Context.CAMERA_SERVICE);
                for (String cameraId : manager.getCameraIdList()) {
                    CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
                    if (characteristics.get(CameraCharacteristics.LENS_FACING) == CameraCharacteristics.LENS_FACING_BACK) {
                        StreamConfigurationMap map = characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP);
                        mCameraSize = map.getOutputSizes(SurfaceTexture.class)[0];
                        try {
                            manager.openCamera(cameraId, mCameraDeviceCallback, null);
                        }catch(SecurityException e){

                        }

                        return;
                    }
                }
            } catch (CameraAccessException e) {
                e.printStackTrace();
            }
        }

        private void createCaptureSession() {
            if (!mTextureView.isAvailable()) {
                return;
            }

            SurfaceTexture texture = mTextureView.getSurfaceTexture();
            texture.setDefaultBufferSize(mCameraSize.getWidth(), mCameraSize.getHeight());
            Surface surface = new Surface(texture);
            try {
                mPreviewBuilder = mCamera.createCaptureRequest(CameraDevice.TEMPLATE_PREVIEW);
            } catch (CameraAccessException e) {
                e.printStackTrace();
            }

            mPreviewBuilder.addTarget(surface);
            try {
                mCamera.createCaptureSession(Collections.singletonList(surface), mCameraCaptureSessionCallback, null);
            } catch (CameraAccessException e) {
                e.printStackTrace();
            }
        }

        private void updatePreview() {
            mPreviewBuilder.set(CaptureRequest.CONTROL_AF_MODE, CaptureRequest.CONTROL_AF_MODE_CONTINUOUS_VIDEO);
            HandlerThread thread = new HandlerThread("CameraPreview");
            thread.start();
            Handler backgroundHandler = new Handler(thread.getLooper());
            try {
                mPreviewSession.setRepeatingRequest(mPreviewBuilder.build(), null, backgroundHandler);
            } catch (CameraAccessException e) {
                e.printStackTrace();
            }
        }
    }


}

補足情報(言語/FW/ツール等のバージョンなど

Camera2クラスを使ってます。

端末はgalaxy note 3

apiレベルは21です。