|
@@ -1,7 +1,10 @@
|
|
|
from utils import *
|
|
from utils import *
|
|
|
|
|
|
|
|
-class PID:
|
|
|
|
|
- def __init__(self, kp=0, ki=0, kd=0, dt=1, target=1, Kc=0, Ti=0, Td=0, Ts=0, debug=False):
|
|
|
|
|
|
|
+'''
|
|
|
|
|
+base discrete/takahashi PID controller
|
|
|
|
|
+'''
|
|
|
|
|
+class BasePID:
|
|
|
|
|
+ def __init__(self, target, clip_min, clip_max, controller_type, kp=0, ki=0, kd=0, dt=1, Kc=0, Ti=0, Td=0, Ts=0, debug=False, type='base', swap_error_fn=False):
|
|
|
self.Kp = kp # discrete pid kp
|
|
self.Kp = kp # discrete pid kp
|
|
|
self.Ki = ki # discrete pid ki
|
|
self.Ki = ki # discrete pid ki
|
|
|
self.Kd = kd # discrete pid kd
|
|
self.Kd = kd # discrete pid kd
|
|
@@ -10,14 +13,19 @@ class PID:
|
|
|
self.Td = Td # takahashi td
|
|
self.Td = Td # takahashi td
|
|
|
self.Ts = Ts # takahashi ts
|
|
self.Ts = Ts # takahashi ts
|
|
|
self.Kc = Kc # takahashi kc
|
|
self.Kc = Kc # takahashi kc
|
|
|
- self.target = target # pid set point
|
|
|
|
|
|
|
+ self.target = target # pid set point, target
|
|
|
self.prev_feedback = 0
|
|
self.prev_feedback = 0
|
|
|
self.feedback_hist = [0, 0]
|
|
self.feedback_hist = [0, 0]
|
|
|
- self.f_hist = [0]
|
|
|
|
|
|
|
+ self.output_hist = [0]
|
|
|
self.error_hist = [0, 0]
|
|
self.error_hist = [0, 0]
|
|
|
self.debug=debug
|
|
self.debug=debug
|
|
|
|
|
+ self.clip_min = clip_min
|
|
|
|
|
+ self.clip_max = clip_max
|
|
|
|
|
+ self.controller_type = CONTROLLER_TYPE_DISCRETE
|
|
|
|
|
+ self.swap_error_fn = swap_error_fn
|
|
|
|
|
+ self.type = type
|
|
|
|
|
|
|
|
- def pid(self, feedback):
|
|
|
|
|
|
|
+ def continuous_pid(self, feedback):
|
|
|
ret = (self.Kp * self.proportional(feedback)) + (self.Ki * self.integral(feedback)) + (self.Kd * self.derivative(feedback))
|
|
ret = (self.Kp * self.proportional(feedback)) + (self.Ki * self.integral(feedback)) + (self.Kd * self.derivative(feedback))
|
|
|
self.feedback_hist+=[feedback]
|
|
self.feedback_hist+=[feedback]
|
|
|
self.prev_feedback=feedback
|
|
self.prev_feedback=feedback
|
|
@@ -28,56 +36,54 @@ class PID:
|
|
|
k2 = -1 * self.Kp - 2 * self.Kd
|
|
k2 = -1 * self.Kp - 2 * self.Kd
|
|
|
k3 = self.Kd
|
|
k3 = self.Kd
|
|
|
err = self.proportional(feedback)
|
|
err = self.proportional(feedback)
|
|
|
- #if debug:
|
|
|
|
|
- #print("pid::f-1: {}".format(self.f_hist[-1]))
|
|
|
|
|
- #print("pid::err: {}".format(err))
|
|
|
|
|
- #print("pid::err-1: {}".format(self.error_hist[-1]))
|
|
|
|
|
- #print("pid::err-2: {}".format(self.error_hist[-2]))
|
|
|
|
|
- #print("pid::k1: {}".format(k1))
|
|
|
|
|
- #print("pid::k2: {}".format(k2))
|
|
|
|
|
- #print("pid::k3: {}".format(k3))
|
|
|
|
|
- ret = self.f_hist[-1] + k1 * err + k2 * self.error_hist[-1] + k3 * self.error_hist[-2]
|
|
|
|
|
|
|
+ ret = self.output_hist[-1] + k1 * err + k2 * self.error_hist[-1] + k3 * self.error_hist[-2]
|
|
|
self.error_hist+=[err]
|
|
self.error_hist+=[err]
|
|
|
self.feedback_hist+=[feedback]
|
|
self.feedback_hist+=[feedback]
|
|
|
return ret
|
|
return ret
|
|
|
|
|
|
|
|
|
|
+ def zero_feedback_hist(self):
|
|
|
|
|
+ count = 0
|
|
|
|
|
+ length = len(self.feedback_hist)
|
|
|
|
|
+ for i in range(0,length):
|
|
|
|
|
+ if self.feedback_hist[length-(i+1)]==0:
|
|
|
|
|
+ count+=1
|
|
|
|
|
+ else:
|
|
|
|
|
+ return count
|
|
|
|
|
+ return count
|
|
|
|
|
+
|
|
|
def takahashi(self, feedback, debug=True):
|
|
def takahashi(self, feedback, debug=True):
|
|
|
err = self.proportional(feedback)
|
|
err = self.proportional(feedback)
|
|
|
- ret = self.f_hist[-1] + self.Kc * (self.feedback_hist[-1] - feedback + self.Ts * err/ self.Ti + self.Td / self.Ts * (2*self.feedback_hist[-1] - feedback - self.feedback_hist[-2]))
|
|
|
|
|
|
|
+ ret = self.output_hist[-1] + self.Kc * (self.feedback_hist[-1] - feedback + self.Ts * err/ self.Ti + self.Td / self.Ts * (2*self.feedback_hist[-1] - feedback - self.feedback_hist[-2]))
|
|
|
self.error_hist+=[err]
|
|
self.error_hist+=[err]
|
|
|
self.feedback_hist+=[feedback]
|
|
self.feedback_hist+=[feedback]
|
|
|
return ret
|
|
return ret
|
|
|
|
|
|
|
|
- def pid_clipped(self, feedback, controller=CONTROLLER_TYPE_DISCRETE, debug=True):
|
|
|
|
|
|
|
+ def pid_clipped(self, feedback, debug=True):
|
|
|
pid_value = None
|
|
pid_value = None
|
|
|
- if controller == CONTROLLER_TYPE_TAKAHASHI:
|
|
|
|
|
|
|
+ if self.controller_type == CONTROLLER_TYPE_TAKAHASHI:
|
|
|
pid_value = self.takahashi(feedback, debug)
|
|
pid_value = self.takahashi(feedback, debug)
|
|
|
- elif controller == CONTROLLER_TYPE_DISCRETE:
|
|
|
|
|
|
|
+ elif self.controller_type == CONTROLLER_TYPE_DISCRETE:
|
|
|
pid_value = self.discrete_pid(feedback, debug)
|
|
pid_value = self.discrete_pid(feedback, debug)
|
|
|
else:
|
|
else:
|
|
|
- pid_value = self.pid(feedback)
|
|
|
|
|
|
|
+ pid_value = self.continuous_pid(feedback)
|
|
|
|
|
+
|
|
|
|
|
+ if pid_value <= self.clip_min:
|
|
|
|
|
+ pid_value = self.clip_min
|
|
|
|
|
+ if pid_value >= self.clip_max:
|
|
|
|
|
+ pid_value = self.clip_max
|
|
|
|
|
|
|
|
- if pid_value <= 0.0:
|
|
|
|
|
- pid_value = F_MIN
|
|
|
|
|
- elif pid_value >= 1:
|
|
|
|
|
- pid_value = F_MAX
|
|
|
|
|
if self.integral(feedback) == 0 and len(self.feedback_hist) >=3 and self.feedback_hist[-1] == 0 and self.feedback_hist[-2] == 0 and self.feedback_hist[-3] == 0:
|
|
if self.integral(feedback) == 0 and len(self.feedback_hist) >=3 and self.feedback_hist[-1] == 0 and self.feedback_hist[-2] == 0 and self.feedback_hist[-3] == 0:
|
|
|
- pid_value = 0.9**self.zero_lead_hist()
|
|
|
|
|
- self.f_hist+=[pid_value]
|
|
|
|
|
- return pid_value
|
|
|
|
|
|
|
+ pid_value = 0.9**self.zero_feedback_hist()
|
|
|
|
|
|
|
|
- def zero_lead_hist(self):
|
|
|
|
|
- count = 0
|
|
|
|
|
- length = len(self.feedback_hist)
|
|
|
|
|
- for i in range(0,length):
|
|
|
|
|
- if self.feedback_hist[length-(i+1)]==0:
|
|
|
|
|
- count+=1
|
|
|
|
|
- else:
|
|
|
|
|
- return count
|
|
|
|
|
- return count
|
|
|
|
|
|
|
+ self.output_hist+=[pid_value]
|
|
|
|
|
+ return pid_value
|
|
|
|
|
|
|
|
def error(self, feedback):
|
|
def error(self, feedback):
|
|
|
- return feedback - self.target
|
|
|
|
|
|
|
+ if self.swap_error_fn:
|
|
|
|
|
+ # maintain positive proportional gains
|
|
|
|
|
+ return self.target - feedback
|
|
|
|
|
+ else:
|
|
|
|
|
+ return feedback - self.target
|
|
|
|
|
|
|
|
def proportional(self, feedback):
|
|
def proportional(self, feedback):
|
|
|
return self.error(feedback)
|
|
return self.error(feedback)
|
|
@@ -88,7 +94,7 @@ class PID:
|
|
|
def derivative(self, feedback):
|
|
def derivative(self, feedback):
|
|
|
return (self.error(self.prev_feedback) - self.error(feedback)) / self.T
|
|
return (self.error(self.prev_feedback) - self.error(feedback)) / self.T
|
|
|
|
|
|
|
|
- def write_feedback(self, lead_hist_file):
|
|
|
|
|
|
|
+ def write_feedback(self, feedback_hist_file):
|
|
|
if len(self.feedback_hist)==0:
|
|
if len(self.feedback_hist)==0:
|
|
|
return
|
|
return
|
|
|
buf = ''
|
|
buf = ''
|
|
@@ -96,26 +102,26 @@ class PID:
|
|
|
buf+=','
|
|
buf+=','
|
|
|
for i in self.feedback_hist[1:]:
|
|
for i in self.feedback_hist[1:]:
|
|
|
buf+=str(i)+','
|
|
buf+=str(i)+','
|
|
|
- with open(lead_hist_file, "w+") as f:
|
|
|
|
|
|
|
+ with open(feedback_hist_file, "w+") as f:
|
|
|
f.write(buf)
|
|
f.write(buf)
|
|
|
|
|
|
|
|
- def write_fval(self, f_hist_file):
|
|
|
|
|
- if len(self.f_hist)==0:
|
|
|
|
|
|
|
+ def write_fval(self, output_hist_file):
|
|
|
|
|
+ if len(self.output_hist)==0:
|
|
|
return
|
|
return
|
|
|
buf = ''
|
|
buf = ''
|
|
|
- buf+=str(self.f_hist[0])
|
|
|
|
|
|
|
+ buf+=str(self.output_hist[0])
|
|
|
buf+=','
|
|
buf+=','
|
|
|
- for i in self.f_hist[1:]:
|
|
|
|
|
|
|
+ for i in self.output_hist[1:]:
|
|
|
buf+=str(i)+','
|
|
buf+=str(i)+','
|
|
|
- with open(f_hist_file, "w+") as f:
|
|
|
|
|
|
|
+ with open(output_hist_file, "w+") as f:
|
|
|
f.write(buf)
|
|
f.write(buf)
|
|
|
|
|
|
|
|
- def write(self, lead_hist_file='leads.hist', f_hist_file='f.hist'):
|
|
|
|
|
- self.write_feedback(lead_hist_file)
|
|
|
|
|
- self.write_fval(f_hist_file)
|
|
|
|
|
|
|
+ def write(self, feedback_hist_file='_feedback.hist', output_hist_file='_output.hist'):
|
|
|
|
|
+ self.write_feedback(self.type+feedback_hist_file)
|
|
|
|
|
+ self.write_fval(self.type+output_hist_file)
|
|
|
|
|
|
|
|
def acc(self):
|
|
def acc(self):
|
|
|
return sum(np.array(self.feedback_hist)==1)/float(len(self.feedback_hist))
|
|
return sum(np.array(self.feedback_hist)==1)/float(len(self.feedback_hist))
|
|
|
|
|
|
|
|
def acc_percentage(self):
|
|
def acc_percentage(self):
|
|
|
- return 100*self.acc()
|
|
|
|
|
|
|
+ return self.acc() * 100
|