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)
