Finish 5619Drive.h and teleop_wf.c