Trajectory follow change

New method for setting setpoints
Trajectory follow only if gumstix is not enabled