Code cleanup and curb finder started

Cleaned up code for Dr Havens and started on the curb finder logic.