From c0f4805a6c8c904fb6430100176d5301cc14d4ad Mon Sep 17 00:00:00 2001
From: Douglas Clowes <dcl@ansto.gov.au>
Date: Fri, 28 Feb 2014 11:15:03 +1100
Subject: [PATCH] Add a PID function for each variable

---
 site_ansto/instrument/util/gen_sct.py | 47 +++++++++++++++++++++++++++
 1 file changed, 47 insertions(+)

diff --git a/site_ansto/instrument/util/gen_sct.py b/site_ansto/instrument/util/gen_sct.py
index a2ed0f61..8f748c28 100755
--- a/site_ansto/instrument/util/gen_sct.py
+++ b/site_ansto/instrument/util/gen_sct.py
@@ -51,6 +51,7 @@ FunctionTypes = [
     'write_function',
     'fetch_function',
     'check_function',
+    'pid_function',
     'checkrange_function',
     ]
 
@@ -123,6 +124,7 @@ reserved = {
     'WRITE_COMMAND' : 'WRITE_COMMAND',
     'WRITE_FUNCTION' : 'WRITE_FUNCTION',
     'CHECK_FUNCTION' : 'CHECK_FUNCTION',
+    'PID_FUNCTION' : 'PID_FUNCTION',
     'CHECKRANGE_FUNCTION' : 'CHECKRANGE_FUNCTION',
     'CHECKLIMITS_FUNCTION' : 'CHECKLIMITS_FUNCTION',
     'CHECKSTATUS_FUNCTION' : 'CHECKSTATUS_FUNCTION',
@@ -356,6 +358,7 @@ def p_var_typ_ass(p):
                | WRITE_COMMAND EQUALS TEXT_STRING
                | WRITE_FUNCTION EQUALS id_or_str
                | CHECK_FUNCTION EQUALS id_or_str
+               | PID_FUNCTION EQUALS id_or_str
                | CHECKRANGE_FUNCTION EQUALS id_or_str
                | CHECKLIMITS_FUNCTION EQUALS id_or_str
                | CHECKSTATUS_FUNCTION EQUALS id_or_str
@@ -467,6 +470,7 @@ def p_code_type(p):
             | FETCH_FUNCTION
             | WRITE_FUNCTION
             | CHECK_FUNCTION
+            | PID_FUNCTION
             | CHECKRANGE_FUNCTION
             | CHECKLIMITS_FUNCTION
             | CHECKSTATUS_FUNCTION
@@ -948,6 +952,33 @@ def put_halt_function(MyDriver, func):
   txt += ['}']
   emit(txt)
 
+def put_pid_function(MyDriver, func):
+  txt = ['']
+  txt += ['# pid function for PID control']
+  txt += ['proc %s::%s {tc_root sp pv} {' % (MyDriver['namespace'], func)]
+  txt += ['  debug_log 1 "%s tc_root=${tc_root} sct=[sct] pv=${pv} sp=${sp}"' % func]
+  txt += ['  sct pid_error [expr ${sp} - ${pv}]']
+  txt += ['  set p_value [expr [sct pid_pvalue] * [sct pid_error]]']
+  txt += ['  set d_value [expr [sct pid_dvalue] * (${pv} - [sct oldval])]']
+  txt += ['  sct pid_deriv [sct pid_error]']
+  txt += ['  sct pid_integ [expr [sct pid_integ] + [sct pid_error]]']
+  txt += ['  if { [sct pid_integ] > [sct pid_imax] } {']
+  txt += ['    sct pid_integ [sct pid_imax]']
+  txt += ['  }']
+  txt += ['  if { [sct pid_integ] < -[sct pid_imax] } {']
+  txt += ['    sct pid_integ -[sct pid_imax]']
+  txt += ['  }']
+  txt += ['  set i_value [expr [sct pid_ivalue] * [sct pid_integ]]']
+  txt += ['  set pid [expr ${p_value} + ${i_value} + ${d_value}]']
+  if func in MyDriver['Funcs'] and len(MyDriver['Funcs'][func]['text']) > 0:
+    txt += ['# hook code starts']
+    txt += MyDriver['Funcs'][func]['text']
+    txt += ['# hook code ends']
+  txt += ['  sct pid_output ${pid}']
+  txt += ['  return ${pid}']
+  txt += ['}']
+  emit(txt)
+
 def put_create_node(MyDriver):
   txt = ['']
   txt += ['##']
@@ -1126,6 +1157,20 @@ def put_group(MyDriver, MyGroup):
           msg = 'Driveable: %s does not have required attribute: %s' % (nodename, attr)
           print 'Warning:', msg
           txt += ['    # Warning: ' + msg]
+    # Check PID attributes are present if required
+    if 'pid_function' in MyVar:
+      for attr in ('pid_pvalue',
+                    'pid_ivalue',
+                    'pid_dvalue',
+                    'pid_imax',
+                    'pid_error',
+                    'pid_deriv',
+                    'pid_integ',
+                    ):
+        if attr not in MyVar['Property']:
+          msg = 'PID: %s does not have required attribute: %s' % (nodename, attr)
+          print 'Warning:', msg
+          txt += ['    # Warning: ' + msg]
     txt += ['    hfactory ${scobj_hpath}/%s plain %s %s' % (nodename, MyVar['priv'], MyVar['type'])]
     if MyVar['readable'] > 0:
       readable_or_writeable = True
@@ -1364,6 +1409,8 @@ def put_standard_code(MyDriver):
       put_checkstatus_function(MyDriver, func);
     elif theFunc['type'] == 'halt_function':
       put_halt_function(MyDriver, func);
+    elif theFunc['type'] == 'pid_function':
+      put_pid_function(MyDriver, func);
 
 def generate_driver(MyDriver):
   global NumberOfLinesOut