From 10ba8e07fdb35f09cc7db062ae397576f6ff4063 Mon Sep 17 00:00:00 2001 From: Pat Thoyts Date: Tue, 26 May 2015 20:49:08 +0100 Subject: [PATCH] Preserve the debugging and plotting scripts. --- backup_simulate.py | 261 +++++++++++++++++++++++++++++++++++++++++++++ cleanup.tcl | 24 +++++ plot.gnuplot | 29 +++++ simulate.gnuplot | 8 ++ simulate.py | 90 ++++++++++++++++ 5 files changed, 412 insertions(+) create mode 100644 backup_simulate.py create mode 100644 cleanup.tcl create mode 100644 plot.gnuplot create mode 100644 simulate.gnuplot create mode 100644 simulate.py diff --git a/backup_simulate.py b/backup_simulate.py new file mode 100644 index 0000000..504ac26 --- /dev/null +++ b/backup_simulate.py @@ -0,0 +1,261 @@ +#!/usr/bin/python + +import os,sys + +class PID3: + """From http://www.chrismarion.net/index.php?option=com_content&view=article&id=166:adventures-in-pid-control&catid=44:robotics&Itemid=240""" + def __init__(self, Kp, Ki, Kd): + self.Kp = Kp + self.Ki = Ki + self.Kd = Kd + self.initialize() + def SetKp(self, Kp): + self.Kp = Kp + def SetKi(self, Ki): + self.Ki = Ki + def SetKd(self, Kd): + self.Kd = Kd + def SetPoint(self, point): + self.setpoint = point + def SetPrevErr(self, preverr): + self.prev_err = preverr + def initialize(self): + self.setpoint = 0 + self.currtm = 0 #time.time() + self.prevtm = self.currtm + self.prev_err = 0 + self.Cp = 0 + self.Ci = 0 + self.Cd = 0 + def update(self, current_time, current_value): + """ Performs a PID computation and returns a control value based on the + elapased time (dt) and the error signal from a summing junction. """ + self.currtm = current_time #get t + error = self.setpoint - current_value + dt = self.currtm - self.prevtm #get delta t + de = error - self.prev_err #get delta error + self.Cp = self.Kp * error #proportional term + self.Ci += error * dt #integral term + self.Cd = 0 + if dt > 0: #no div by zero + self.Cd = de/dt #derivative term + self.prevtm = self.currtm #save t for next pass + self.prev_err = error #save t-1 error + return self.Cp + (self.Ki * self.Ci) + (self.Kd * self.Cd) + + +class PID: + """Discrete PID control""" + def __init__(self, P=2.0, I=0.0, D=1.0, Derivator=0, Integrator=0, + Integrator_max=0, Integrator_min=5000): + self.Kp = P + self.Ki = I + self.Kd = D + self.Derivator = Derivator + self.Integrator = 0 + self.Integrator_max = Integrator_max + self.Integrator_min = Integrator_min + self.set_point = 0.0 + + def update(self, current_value): + """Calculate PID output value for given input""" + error = self.set_point - current_value + P = self.Kp * error + D = self.Kd * (error - self.Derivator) + self.Derivator = error + self.Integrator += error + if self.Integrator > self.Integrator_max: + self.Integrator = self.Integrator_max + elif self.Integrator < self.Integrator_min: + self.Integrator = self.Integrator_min + I = self.Integrator * self.Ki + return P + I + D + + @property + def point(self): + return self.set_point + @point.setter + def point(self, set_value): + """Initialize the setpoint of the PID""" + self.set_point = value + self.Integrator = 0 + self.Derivator = 0 + @property + def Kp(self): + return self.Kp + @Kp.setter + def Kp(self, value): + self.Kp = value + @property + def Ki(self): + return self.Ki + @Ki.setter + def Ki(self, value): + self.Ki = value + @property + def Kd(self): + return self.Kd + @Kd.setter + def Kd(self, value): + self.Kd = value + @property + def Integrator(self): + return self.Integrator + @Integrator.setter + def Integrator(self, value): + self.Integrator = value + @property + def Derivator(self): + return self.Derivator + @Derivator.setter + def Derivator(self, value): + self.Derivator = value + +class PID_Arduino: + def __init__(self, Kp, Ki, Kd): + self.target = 0.0 + self.last_input = None + self.last_error = 0.0 + self.iterm = 0.0 + self.sampletime = 100.0 + self.out_max = 5000.0 + self.out_min = 0.0 + self.SetTunings(Kp, Ki, Kd) + @property + def point(self): + return self.target + @point.setter + def point(self, value): + self.target = float(value) + def SetTunings(self, Kp, Ki, Kd): + sampletime_secs = self.sampletime / 1000.0 + self.Kp = float(Kp) + self.Ki = float(Ki) * sampletime_secs + self.Kd = float(Kd) / sampletime_secs + def SetSampleTime(self, ms): + ratio = float(ms) / self.sampletime + self.Ki *= ratio + self.Kd /= ratio + self.sampletime = float(ms) + + def update(self, val): + val = float(val) + if self.last_input is None: + self.last_input = val + error = self.target - val + self.iterm += (self.Ki * error) + if self.iterm > self.out_max: + self.iterm = self.out_max + elif self.iterm < self.out_min: + self.iterm = self.out_min + #dv = val - self.last_input + de = error - self.last_error + PID = (self.Kp * error) + self.iterm - (self.Kd * de) + if PID > self.out_max: + PID = self.out_max + elif PID < self.out_min: + PID = self.out_min + self.last_input = val + self.last_error = error + return PID + + def update2(self, val): + err = val - self.target + de = err - self.last_error; + self.iterm += self.Ki * self.iterm + PID = (self.Kp * err) + self.iterm + (self.Kd * de) + self.last_error = err + self.iterm += self.iterm + return PID + +import numpy as np +import pylab as plt + +def main2(argv = None): + if argv is None: + argv = sys.argv + time,target,temp,output = np.loadtxt(argv[1],usecols=(0,1,2,3),unpack=True) + pid = PID3(0.3, 0.0, 5.0) + pid.SetPoint(target[0]) + pideval = np.vectorize(pid.update) + pidres = pideval(time, temp) + #pid = PID_Arduino(0.8, 0.0, 0.0) + #pid.point = target[0] + #pideval = np.vectorize(pid.update2) + #pidres = pideval(temp) + print temp + print pidres + time /= 1000 + time_plot, = plt.plot(time, temp) + plt.setp(time_plot, label='temperature') + #plt.plot(time, target) + #plt.plot(time, output) + pidres_plot, = plt.plot(time, pidres) + plt.setp(pidres_plot, label='PID output') + plt.ylabel("Temperature (\u00b0 C)") + plt.xlabel("Time (s)") + plt.grid(True) + plt.show() + + + + + +import Gnuplot, Gnuplot.funcutils + +#def plot_pid(): + +def main3(argv = None): + if argv is None: + argv = sys.argv + + with open(argv[1], 'r') as f: + pid = PID3(0.5, 0.0, 0.0) + for line in f: + if not line.startswith('#'): + T,Set,In,Out,Power = line.strip().split() + if not init: + pid.SetPoint(float(Set)) + init = True + o = pid.update(int(T), float(In)) + print T, In, Out, o + elif line.startswith('# PID'): + print line + + gp = Gnuplot.Gnuplot(persist=1) + gp.clear() + gp.title('PID simulation') + gp.xlabel('Time (ms)') + #qplot_pid(1, 0, 0) + gp.plot(Gnuplot.File('z.log', using=(1,2), with_='lines', title="actual"), + Gnuplot.File('z.log', using=(1,4), with_='lines', title="predicted")) + +def main(argv = None): + if argv is None: + argv = sys.argv + if len(argv) < 2: + print("usage: simulate filename") + return 1 + + lines = [] + init = False + with open(argv[1], 'r') as f: + #pid = PID(2.0,5.0,1.0) + #pid = PID_Arduino(2.0,5.0,1.0) + pid = PID3(0.5, 0.0, 0.0) + for line in f: + if not line.startswith('#'): + T,Set,In,Out,Power = line.strip().split() + if not init: + pid.SetPoint(float(Set)) + init = True + o = pid.update(int(T), float(In)) + print T, In, Out, o + elif line.startswith('# PID'): + print line + + return 0 + + +if __name__=='__main__': + sys.exit(main2()) diff --git a/cleanup.tcl b/cleanup.tcl new file mode 100644 index 0000000..4d6a879 --- /dev/null +++ b/cleanup.tcl @@ -0,0 +1,24 @@ +#!/usr/bin/env tclsh + +proc main {filename} { + set f [open $filename r] + fconfigure $f -encoding ascii + while {[gets $f line] != -1} { + if {[string match "#*" $line]} { puts $line; continue } + set cols [split $line] + if {[llength $cols] != 5} {continue} + binary scan [string range $line 0 end-2] cu* chars + set sum 0 + foreach c $chars { + set sum [expr {($sum + $c) & 0xff}] + } + if {[format %02X $sum] eq [lindex $cols end]} { + puts $line + } + } +} +if {!$tcl_interactive} { + set r [catch [linsert $argv 0 main] err] + if {$r} {puts stderr $::errorInfo} + exit $r +} diff --git a/plot.gnuplot b/plot.gnuplot new file mode 100644 index 0000000..84222b0 --- /dev/null +++ b/plot.gnuplot @@ -0,0 +1,29 @@ +# Call as: gnuplot -e "filename='log1.log'" to use an alternate source +if (!exists("filename")) filename='log.log' + +mytitle=system("sed -n '/# PID/s/# //p' ".filename) +set title mytitle + +# Tell gnuplot to ignore lines starting with # +set datafile commentschars "#" + +set grid ytics +set yrange [0:10000] +set ytics nomirror +set y2range [0:5000] +set y2tics nomirror + +#set xrange [4500:] +set xlabel "Time (s)" +set ylabel "Temperature (°C)" +set y2label "Heater on time (ms) + +plot filename using 1/1000:3 axis x1y1 title "profile" with linespoints lw 2.0 lt 1 ps 0.0,\ + filename using 1/1000:2 axis x1y1 title "target" with linespoints lt 3 pt 7 ps 0.0,\ + filename using 1/1000:4 axis x1y2 title "control" with steps lc 0 lw 1.5 + +# filename using 1/1000:4 axis x1y2 title "control" with points lt 2 pt 6 ps 0.6, + +# Call using gnuplot filename - to go interactive, or -persist, or uncomment below +# pause mouse any + diff --git a/simulate.gnuplot b/simulate.gnuplot new file mode 100644 index 0000000..5659057 --- /dev/null +++ b/simulate.gnuplot @@ -0,0 +1,8 @@ +if (!exists("filename")) filename="z.log" +set datafile commentschars "#" +set grid ytics +set xlabel "Time (s)" +set y2range [0:5000] +plot filename using 1/1000:2 axis x1y1 title "temperature" with linespoints ps 0.0,\ + filename using 1/1000:4 title "predict" with linespoints lt 2 ps 0.0,\ + filename using 1/1000:3 axis x1y2 title "output" with steps lc 0 lw 1.5 diff --git a/simulate.py b/simulate.py new file mode 100644 index 0000000..c33b98e --- /dev/null +++ b/simulate.py @@ -0,0 +1,90 @@ +#!/usr/bin/python + +import os,sys +import numpy as np +import pylab as plt + +class PID3: + """From http://www.chrismarion.net/index.php?option=com_content&view=article&id=166:adventures-in-pid-control&catid=44:robotics&Itemid=240""" + def __init__(self, Kp, Ki, Kd): + self.Kp = Kp + self.Ki = Ki + self.Kd = Kd + self.initialize() + def SetKp(self, Kp): + self.Kp = Kp + def SetKi(self, Ki): + self.Ki = Ki + def SetKd(self, Kd): + self.Kd = Kd + def SetPoint(self, point): + self.setpoint = point + def SetPrevErr(self, preverr): + self.prev_err = preverr + def initialize(self): + self.setpoint = 0 + self.currtm = 0 #time.time() + self.prevtm = self.currtm + self.prev_err = 0 + self.Cp = 0 + self.Ci = 0 + self.Cd = 0 + def update(self, current_time, current_value): + """ Performs a PID computation and returns a control value based on the + elapased time (dt) and the error signal from a summing junction. """ + self.currtm = current_time #get t + error = self.setpoint - current_value + dt = self.currtm - self.prevtm #get delta t + de = error - self.prev_err #get delta error + self.Cp = self.Kp * error #proportional term + self.Ci += error * dt #integral term + if self.Ci < 0: + self.Ci = 0 + elif self.Ci > 5000: + self.Ci = 5000 + self.Cd = 0 + if dt > 0: #no div by zero + self.Cd = de/dt #derivative term + self.prevtm = self.currtm #save t for next pass + self.prev_err = error #save t-1 error + pid = self.Cp + (self.Ki * self.Ci) + (self.Kd * self.Cd) + if pid > 5000: + pid = 5000 + elif pid < 0: + pid = 0 + return pid + +def main(argv = None): + if argv is None: + argv = sys.argv + defaults = [None, 1.0, 0.0, 0.0] + filename,Kp,Ki,Kd = argv[1:] + defaults[len(argv)-1:] + if filename is None: + print("usage: simulate filename ?Kp? ?Kd? ?Ki?") + return 1 + + time,target,temp,output = np.loadtxt(argv[1],usecols=(0,1,2,3),unpack=True) + timex = time / 1000 + time_plot, = plt.plot(timex, temp) + plt.setp(time_plot, label='temperature') + + #for f in range(0,10,3): + #kd = (float(Kd) * f) / 1.0 + kd = float(Kd) + pid = PID3(float(Kp), float(Ki), kd) + pid.SetPoint(target[0]) + pideval = np.vectorize(pid.update) + pidres = pideval(timex, temp) + pidres_plot, = plt.plot(timex, pidres) + #c = ["#ff0000","#00ff00","#0000ff","#ffff00","#ff00ff","#00ffff"][f%6] + c="#00ffff" + plt.setp(pidres_plot, label='PID Kd %f' % (kd),color=c) + + plt.title("PID Kp=%.3f Ki=%3f Kd=%.3f" % (pid.Kp, pid.Ki, pid.Kd)) + plt.ylabel(u"Temperature (\u00b0 C)") + plt.xlabel("Time (s)") + plt.grid(True) + plt.show() + +if __name__=='__main__': + sys.exit(main()) -- 2.23.0