/*--------------------------------------------------------------------
 * File:        emul.g
 *
 * Description: A Gamma program that simulates a PID-controlled loop.
 *
 * Functions:
 *                PID_Controller.init
 *                PID_Controller.calculate
 *                change_procvar
 *                auto_control
 *                change_values
 *------------------------------------------------------------------*/

/* Initialize interprocess communication for this process */
if (init_ipc("emul", "emulq", "toolsdemo") == nil)
     error("Could not initialize IPC.");
     
/* See if a DataHub is running in the toolsdemo domain. If not, start it. */
if (locate_task ("/dh/toolsdemo", t) == nil) 
     system("datahub -d toolsdemo");

/* Register the first points with the DataHub. */     
SP_001 = register_point(#SP_001);     // The set point variable.
MV_001 = register_point(#MV_001);     // The manipulated variable.
PV_001 = register_point(#PV_001);     // The process variable.

/* Ensure that the confidence values are set to 100, or the
 * logger will not log the values that we produce.  The computation
 * of PV and MV are relative to previous values, so they preserve
 * whatever confidence we initially set. */
set_conf (SP_001, 100);
set_conf (MV_001, 100);
set_conf (PV_001, 100);

/* Create a global variable for demonstrating gsend and lsend. */
demo_send = 1;             

/********************************************************
 *                PID Controller Class                  *
 ********************************************************/

/*--------------------------------------------------------------------
 * Class:       PID_Controller
 * Description: Contains data and methods for creating and maintaining
 *              a simulated PID loop.
 *------------------------------------------------------------------*/
class PID_Controller
{
  name;

  errorPrev;
  errorInt = 0;
  errorDrv = 0;
  
  kProp;
  kInt;
  kDrv;
}

/*--------------------------------------------------------------------
 * Method:      PID_Controller.init
 * Returns:     a name
 * Description: Initializes the PID control loop by creating three
 *              variables representing proportion, integration, and
 *              derivative.
 *------------------------------------------------------------------*/
method PID_Controller.init (name)
{
  local val;
  
  .name = name;
  
  .kProp = symbol(string(name, "_Kp"));
  val = register_point(.kProp);
  if (number_p(val) && (conf(val) != 0))
    set(.kProp, val);
  else
    write_point(.kProp, 0.05);

  .kInt = symbol(string(name, "_Ki"));
  val = register_point(.kInt);
  if (number_p(val) && (conf(val) != 0))
    set(.kInt, val);
  else
    write_point(.kInt, 0.7);
  
  .kDrv = symbol(string(name, "_Kd"));
  val = register_point(.kDrv);
  if (number_p(val) && (conf(val) != 0))
    set(.kDrv, val);
  else
    write_point(.kDrv, 0.05);
  .name;
}

/*--------------------------------------------------------------------
 * Method:      PID_Controller.calculate
 * Returns:     an approximate value for MV, as a real number
 * Description: Sets and writes a new MV value.
 *------------------------------------------------------------------*/
method PID_Controller.calculate (sp, mv, pv)  
{                                             
  local error, res;
  
  error = eval(sp) - eval(pv);

  /* Appproximate the integral. */
  .errorInt += error;       

  /* Prevent error "wind-up" effect. */
  if (.errorInt < 0)        
    .errorInt = 0;

  /* Approximate the derivative. */
  if (.errorPrev)           
    .errorDrv = error - .errorPrev;
  .errorPrev = error;
  
  res = (eval(.kProp) * error
         + eval(.kInt) * .errorInt
         + eval(.kDrv) * .errorDrv);
  res = floor (res * 100) / 100;
  
  /* Set boundary conditions. */
  if (res > 200)            
    res = 200;
  if (res <= 0)
    res = 0;  
  write_point (mv, res);
  res;
}

/********************************************************
 *                   The Plant Model                    *
 ********************************************************/

/* The proportion and integral for PV. */
PROP_001 = register_point(#PROP_001); 
INT_001 = register_point(#INT_001);   

/* Check and correct out-of-range values,
 * by writing default "Poor" values.*/
if (PROP_001 < .01 || PROP_001 > 10)  
     write_point(#PROP_001, .7);      
if (INT_001 <= 0 || INT_001 > 10)
     write_point(#INT_001, 5);

/*--------------------------------------------------------------------
 * Function:    change_procvar
 * Returns:     t on success, nil on failure
 * Description: Sets and writes a new PV value.
 *------------------------------------------------------------------*/
function change_procvar (sptag, mvtag, pvtag) // 
{
  local diff = eval(mvtag) * PROP_001 - eval (pvtag),
        newval = eval(pvtag) + diff / INT_001;
    
  newval = floor (newval * 100) / 100;
  newval = newval * demo_send;
  write_point (pvtag, newval);
}

/********************************************************
 *            Automatically Change SP Value             *
 ********************************************************/

/* Initialize a global variable for the automatic timer. */
AUTO_TIMER = nil;          

/* Register points, exceptions, and echos with the DataHub.
 * The FREQ_001 variable holds the delay for automatic change of SP. */
AUTO_001 = register_point(#AUTO_001);
add_exception_function(#AUTO_001, #(AUTO_TIMER = auto_control(FREQ_001)));
add_echo_function(#AUTO_001, #(AUTO_TIMER = auto_control(FREQ_001)));

FREQ_001 = register_point(#FREQ_001); 
if (FREQ_001 < 1 || FREQ_001 > 20)
     write_point(#FREQ_001, 8);    
add_exception_function(#FREQ_001, #(AUTO_TIMER = auto_control(FREQ_001)));

/*--------------------------------------------------------------------
 * Function:    auto_control
 * Returns:     a timer number
 * Description: Sets a timer to change the value of SP at a specified
 *              frequncy.
 *------------------------------------------------------------------*/
function auto_control(freq)
{
  if (AUTO_001 != 0)       
    {
      /* Start a timer to move SP to random settings. */
      if (AUTO_TIMER == nil)
        {
          if (45 < SP_001 && SP_001 < 55)
            write_point(#SP_001, 80);
          else
            write_point(#SP_001, 50);
          AUTO_TIMER = every(eval(freq), `write_point(#SP_001, (random() * 100)));
        }
      else
        {
          /* Cancel an existing timer. */
          cancel(AUTO_TIMER);
          AUTO_TIMER = nil;
          auto_control(FREQ_001);
        }
    }
  else                          
    {
      /* Cancel the timer if the AUTO_001 button is not pressed. */
      if (AUTO_TIMER != nil)
        {
          cancel(AUTO_TIMER);
          AUTO_TIMER = nil;
        }
    }
  AUTO_TIMER;
}

/********************************************************
 *               Emulating the PID Loop                 *
 ********************************************************/

/*--------------------------------------------------------------------
 * Function:    change_values
 * Returns:     function
 * Description: Calls PID_Controller.calculate() and change_procvar()
 *------------------------------------------------------------------*/
function change_values (controller)
{
  controller.calculate(#SP_001, #MV_001, #PV_001);    
  change_procvar (#SP_001, #MV_001, #PV_001);   
}

/* Create a controller. */
PID1 = new(PID_Controller);   
PID1.init ("PID1");

/* Start the controller running. */
every (0.1, #change_values(PID1));

write_point(#AUTO_001, 1);
AUTO_TIMER = auto_control(#FREQ_001);

/* Loop continuously. */
while (t)
{
  next_event();
}
