Path planner for implementation demo

Utilize the kinematic libraries to implement the functions for path planner Serial and file output