Fix: Corrected velocity measurement in CascadeController #36
Conversation
There was a problem hiding this comment.
Pull request overview
Note
Copilot was unable to run its full agentic suite in this review.
Updates CascadeController to compute measured velocity from successive measured values (instead of error deltas) and exposes the computed velocity via a new accessor.
Changes:
- Track previous measured value (
prevMeasuredValue) to calculatevelMeasuredValuefrom position changes over time. - Adjust timestamp/period handling in
calculateOutput. - Add
getMeasuredVel()accessor.
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| double currentTimeStamp = (double) System.nanoTime() / 1E9; | ||
| if (lastTimeStamp == 0) lastTimeStamp = currentTimeStamp; | ||
| period = currentTimeStamp - lastTimeStamp; | ||
| lastTimeStamp = currentTimeStamp; | ||
|
|
||
| if (measuredValue == pv) { | ||
| errorVal_p = setPoint - measuredValue; | ||
| } else { | ||
| errorVal_p = setPoint - pv; | ||
| if (measuredValue != pv) { | ||
| measuredValue = pv; | ||
| } | ||
| errorVal_p = setPoint - measuredValue; | ||
|
|
||
| if (Math.abs(period) > 1E-6) { | ||
| velMeasuredValue = (errorVal_p - prevErrorVal) / period; | ||
| } else { | ||
| velMeasuredValue = 0; | ||
| } | ||
| velMeasuredValue = (measuredValue - prevMeasuredValue) / period; | ||
| lastTimeStamp = currentTimeStamp; | ||
| } |
| setPoint = psp; | ||
| velSetPoint = vsp; | ||
| errorVal_p = setPoint - measuredValue; | ||
| velMeasuredValue = (errorVal_p - prevErrorVal) / period; | ||
| velMeasuredValue = (measuredValue - prevMeasuredValue) / period; | ||
| errorVal_v = velSetPoint - velMeasuredValue; |
| private final Controller primary; | ||
| private final Controller secondary; | ||
| private double velMeasuredValue; | ||
| private double prevMeasuredValue; |
| @Override | ||
| protected double calculateOutput(double pv) { | ||
| prevErrorVal = errorVal_p; | ||
| prevMeasuredValue = measuredValue; |
| public double getMeasuredVel() { | ||
| return velMeasuredValue; | ||
| } |
Nataso08
changed the title
CascadeController CascadeController
Nataso08
changed the title
CascadeController CascadeController
Nataso08
changed the title
CascadeController CascadeController
Nataso08
changed the title
CascadeController CascadeController
|
Also, because my bad at the first time, I saw in This cause a wrong reference to the An easy adjutment to this problem is the extension of If possible, I'm able to add this change to my PR, in order to simplify your work. Thanks |
What fix
This PR fixes the velocity calculation in the
CascadeControllerclass. The previous implementation incorrectly calculated measured velocity as the derivative of position error rather than the derivative of measured position itself, resulting in inaccurate velocity feedback in the cascade controller loop.Previous error (fixed)
The original code computed velocity as
velMeasuredValue = (errorVal_p - prevErrorVal) / period;, which is mathematically incorrect since velocity should represent the rate of change of position, not error. This fix implements the correct calculation by tracking the previous measured position and computingvelMeasuredValue = (measuredValue - prevMeasuredValue) / period;.Fix implementation
The implementation introduces a
prevMeasuredValuefield to maintain position history, updates the timestamp only when the period is valid (greater than 1E-6) for improved efficiency, applies the corrected formula to thesetSetPoints()method, and adds a newgetMeasuredVel()getter for public access to the measured velocity. These changes improve accuracy of velocity feedback, ensure physical consistency with the mathematical definition of velocity, and enhance the overall stability of cascade control behavior.Possible breaks
This fix could brake some previous tuning because of it change measured value of velocity controller (
private Controller secondary) as above explained.CLOSES #35