C Controller V3

thirdord debugged and fixed
interpola debugged and fixed
todo: eliminate redundant points at end of trajectory
todo: figure out why other trajectory changes after point 1 reached.