def iterate(self, value): curTime = time.time() dt = curTime - self.prevTime self.prevVals.append((curTime, value)) for i in xrange(len(self.prevVals)): if self.prevVals[i][0] + self.dTime > curTime: self.prevVals = self.prevVals[i:] break longDt = self.prevVals[-1][0]-self.prevVals[0][0] longDx = self.prevVals[-1][1]-self.prevVals[0][1] p = self.target-value d = 0 if longDt == 0 else (longDx/longDt) self.i += p*dt; self.i = utils.absBound(self.i, self.iLim) out = p*self.kp + d*self.kd + self.i*self.ki out = utils.absBound(out, self.oLim) self.prevTime = curTime self.prevValue = value self.prevOut = out return out
def accelBound(self, prevSpeed, goalSpeed): return goalSpeed delta = goalSpeed - prevSpeed maxDelta = ACCEL_LIM * (time.time() - self.prevTime) newSpeed = prevSpeed + utils.absBound(delta, maxDelta) c.LOG(newSpeed) return utils.absBound(newSpeed, goalSpeed)