public static void GUIDrawPidResponse(PID pid, Rect area, float timeUnit) { Color c = new Color(1f, 1f, 1f, .1f); pid.Init(); //unit step pid.m_target = 1; float r = 0; Vector2 start = new Vector2(area.x, area.y + area.height); Vector2 end = start; Handles.color = c; for (int i = 0; i < timeUnit; ++i) { start = new Vector2((float)i * area.width / timeUnit, area.y + area.height); end = new Vector2((float)i * area.width / timeUnit, area.y); //GLDraw.DrawLine (start, end, c, 1f); Handles.DrawLine(start, end); } start = new Vector2(area.x, area.y + area.height * .5f); end = new Vector2(area.x + area.width, area.y + area.height * .5f); //GLDraw.DrawLine (start, end, c, 1f); Handles.DrawLine(start, end); start = new Vector2(area.x, area.y + area.height); end = start; for (int i = 0; i < area.width; ++i) { float dt = (float)timeUnit / (float)area.width; for (int j = 0; j < 10f; ++j) { r = pid.Compute(r, dt * .1f); } end.x++; end.y = area.height - r * area.height * .5f + area.y; end.y = Mathf.Clamp(end.y, area.y, area.y + area.height); //GLDraw.DrawLine (start, end, Color.green, 1f); Handles.color = VertExmotionEditor.orange; Handles.DrawLine(start, end); start = end; // //draw error // errEnd.x++; // errEnd.y = area.height - (float) lastErr; // errEnd.y = Mathf.Clamp( errEnd.y, area.y, area.y+area.height ); // GLDraw.DrawLine (errStart, errEnd, Color.red, 1f); // errStart = errEnd; } }
public void Init() { m_pidX.Init(); m_pidY.Init(); m_pidZ.Init(); }