I am trying to write in some code that uses the current forward velocity (feel) to interpolate values from a PID gain table gammaSchedule.[n]. I need to take the current forward velocity and determine which two values on my gain table to interpolate between( gammaSchedule. [n] and gammaSchedule._ [n+1]. Right now I have a a list of if/ else if statements to determine which values to interpolate between, like so:
if (fvel < _gammaSchedule[0]){
//extrapolate
}
else if (fvel >= _gammaSchedule.fvel[0] && fvel < _gammaSchedule.fvel[1]){
_kpg = _interpGamma(_gammaSchedule.kpg[0], _gammaSchedule.kpg[1],
_gammaSchedule.fvel[0], _gammaSchedule.fvel[1],
fvel);
_kdg = _interpGamma(_gammaSchedule.kdg[0], _gammaSchedule.kdg[1],
_gammaSchedule.fvel[0], _gammaSchedule.fvel[1],
fvel);
_kig = _interpGamma(_gammaSchedule.kig[0], _gammaSchedule.kig[1],
_gammaSchedule.fvel[0], _gammaSchedule.fvel[1],
fvel);
}
else if (fvel >= _gammaSchedule.fvel[1] && fvel < _gammaSchedule.fvel[2]){
_kpg = _interpGamma(_gammaSchedule.kpg[1], _gammaSchedule.[2],
_gammaSchedule.fvel[1], _gammaSchedule.fvel[2],
fvel);
_kdg = _interpGamma(_gammaSchedule.kdg[1], _gammaSchedule.kdg[2],
_gammaSchedule.fvel[1], _gammaSchedule.fvel[2],
fvel);
_kig = _interpGamma(_gammaSchedule.kig[1], _gammaSchedule.kig[2],
_gammaSchedule.fvel[1], _gammaSchedule.fvel[2],
fvel);
}
This same conditional structure is continued until the array indexes reach at least 10. I know that I could easily write a for loop that would loop through all of this stuff so that I don’t have to type out all of these conditional statements, but I don’t see too many for loops in the APM code. Is this because it messes with the scheduled interrupts?
Overall, my question is: is there a better/more efficient way to write this nasty conditional structure?
Thank you!