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();
}
