corrected servo reverse setAngle and changed angle->pwd formula