private void Excute(float current, float target) { if (currentTar == target) { return; } currentTar = target; currentId++; int ownId = currentId; bool reverse = false; if (target < current) { var buf = target; target = current; current = buf; reverse = true; } //重置 ActualSpeed = current; err = 0; err_last = 0; err_next = 0; float lastResult = current; float currentResult; while (Math.Abs(target - (currentResult = PIDCaculate(target))) > 0.1 && ownId == currentId) { ActualSpeed = currentResult; if (reverse) { PIDOutEvent_Float?.Invoke(target - currentResult); if ((int)lastResult < (int)currentResult) { PIDOutEvent_Int?.Invoke(target - currentResult); } } else { PIDOutEvent_Float?.Invoke(currentResult); if ((int)lastResult < (int)currentResult) { PIDOutEvent_Int?.Invoke(currentResult); } } lastResult = currentResult; } if (ownId == currentId && !reverse) { PIDOutEvent_Float?.Invoke(target); PIDOutEvent_Int?.Invoke(target); } if (ownId == currentId && reverse) { PIDOutEvent_Float?.Invoke(current); PIDOutEvent_Int?.Invoke(current); } }
private void Excute(float current, float target) { if (currentTar == target) { return; } float originTar = current; bool reverse = false; if (target < current) { target = current - target; current = 0; reverse = true; } currentTar = target; //重置 ActualSpeed = current; err_last = err_next = 0; float lastResult = current; float currentResult; while (Math.Abs(target - (currentResult = PIDCaculate(target))) > 0.1 && currentTar == target) { if (reverse) { PIDOutEvent_Float?.Invoke(originTar - currentResult); if ((int)lastResult < (int)currentResult) { PIDOutEvent_Int?.Invoke(originTar - currentResult); } } else { PIDOutEvent_Float?.Invoke(currentResult); if ((int)lastResult < (int)currentResult) { PIDOutEvent_Int?.Invoke(currentResult); } } lastResult = currentResult; } if (currentTar == target && !reverse) { PIDOutEvent_Float?.Invoke(target); PIDOutEvent_Int?.Invoke(target); } else if (currentTar == target && reverse) { PIDOutEvent_Float?.Invoke(current); PIDOutEvent_Int?.Invoke(current); } }