updated cubic and quintic interps

Still some major testing to do.  not convinced they work completely but
it looks promising.  We still need to choose new via points as they are
not working with servo, only move.