Added preliminary arm pid control