Changed CMakefiles
Added Comments to Code
Added 5th order Interpolation Method for Trajectory Planning

Signed-off-by: Master <master@master-OptiPlex.(none)>