update_v0.7: Added Control Function, still need PID and feedback