Path follower overhaul

Re-wrote S-Function from MATLAB to C MEX.