Add drive_straight_pid