Some modification on traj & test arduino