# 1 "../psControl.st"
# 1 "<built-in>"
# 1 "<command-line>"
# 1 "/afs/slac.stanford.edu/package/linuxRT/vol7/buildroot-2019.08/host/linux-x86_64/x86_64/x86_64-buildroot-linux-gnu/sysroot/usr/include/stdc-predef.h" 1 3 4
# 1 "<command-line>" 2
# 1 "../psControl.st"
program psControl("ps=00G-QD1,limit=DRVH,vi=Curr,vis=I,turnonmin=0")
# 54 "../psControl.st"
option +r;
option -a;
option +c;

%%#include <epicsPrint.h>
%%#include <string.h>
# 1 "../psControl.h" 1
# 37 "../psControl.h"
double wait_delay_sofar;
assign wait_delay_sofar to "{ps}:ControlDelay";

double wait_delay;
assign wait_delay to "{ps}:ControlDelayMax";
monitor wait_delay;

int control_setpt;
assign control_setpt to "{ps}:Control";
monitor control_setpt;

int norampoff;
assign norampoff to "{ps}:Ramp";
monitor norampoff;

int ps_on;
assign ps_on to "{ps}:State";
monitor ps_on;

int ps_on_des;
assign ps_on_des to "{ps}:StateSetpt";

int ps_ramp;
assign ps_ramp to "{ps}:RampStatus";
monitor ps_ramp;

int ps_status;
assign ps_status to "{ps}:ControlStatus";

double ps_curr_setpt;
assign ps_curr_setpt to "{ps}:{vi}Setpt";
monitor ps_curr_setpt;
evflag ps_curr_setpt_ef;
sync ps_curr_setpt ps_curr_setpt_ef;

double ps_curr_des;
assign ps_curr_des to "{ps}:{vi}SetptDes";
monitor ps_curr_des;
evflag ps_curr_des_ef;
sync ps_curr_des ps_curr_des_ef;

int ps_limits_flag;
assign ps_limits_flag to "{ps}:WarnDelta.F";

int ps_didt_select;
assign ps_didt_select to "{ps}:{vi}D{vis}DtSelect";
# 61 "../psControl.st" 2
# 70 "../psControl.st"
int standardize;
assign standardize to "{ps}:Standardize";
monitor standardize;

int cycle_count;
assign cycle_count to "{ps}:StandardizeNumCycle";

int max_cycle_count;
assign max_cycle_count to "{ps}:StandardizeNumCycleMax";
monitor max_cycle_count;

double ps_curr_drvh;
assign ps_curr_drvh to "{ps}:{vi}Setpt.DRVH";
monitor ps_curr_drvh;

double ps_curr_drvl;
assign ps_curr_drvl to "{ps}:{vi}Setpt.DRVL";
monitor ps_curr_drvl;

double ps_curr_min;
assign ps_curr_min to "{ps}:{vi}SetptMin";
monitor ps_curr_min;

double ps_curr_res;
assign ps_curr_res to "{ps}:{vi}SetptConf";
monitor ps_curr_res;
evflag ps_curr_res_ef;
sync ps_curr_res ps_curr_res_ef;

double ps_didt;
assign ps_didt to "{ps}:{vi}D{vis}Dt";
monitor ps_didt;

double ps_rampdelay;
assign ps_rampdelay to "{ps}:RampStepDelay";
monitor ps_rampdelay;



char* ps_c;
char* macro_c;
int use_drv;
int use_min;
int pos_pol;
int setpt_change;
int zero_current;
int des_current;
int ramp_current;
int wait_needed;
int ramp_timeout;
int change_count;
int standardizing;
int ramping;
double wait_delay_increment;
double wait_delay_total;
double sw_ramp_delay;
double ps_curr_setpt_new;
double ps_curr_setpt_prev;

ss psControl
{



    state psControl_init
    {
 when()
 {
     ps_c = macValueGet("ps");
     macro_c = macValueGet("limit");
     pos_pol = 1;
     if (macro_c && (strncmp(macro_c, "DRV", 3))) {
  use_drv = 0;
     } else {
  use_drv = 1;
  if (macro_c && (strncmp(macro_c, "DRVH", 4)))
     pos_pol = 0;
     }
     macro_c = macValueGet("turnonmin");
     if (macro_c && (strcmp(macro_c, "0"))) use_min = 1;
     else use_min = 0;
     ps_status = 10;
     pvPut(ps_status);
     cycle_count = 0;
     pvPut(cycle_count);
     wait_delay_sofar = 0;
     pvPut(wait_delay_sofar);
     standardize = 0;
     pvPut(standardize);
     norampoff = 1;
     pvPut(norampoff);
     control_setpt = 0;
     pvPut(control_setpt);
     efClear(ps_curr_setpt_ef);
     efClear(ps_curr_res_ef);
     ps_limits_flag = 0;
     pvPut(ps_limits_flag);
     ps_curr_setpt_prev = ps_curr_setpt;

 } state psControl_request
    }



    state psControl_request
    {
 entry
 {
     setpt_change = 0;
     ramping = 0;
     standardizing = 0;
     cycle_count = 0;
     change_count = 0;
     wait_needed = 0;
     ramp_timeout = 0;
     zero_current = 0;
     des_current = 0;
     ramp_current = 0;
 }



 when((!ps_on) && (control_setpt == 2))
 {
     epicsPrintf("Ramp on started for %s\n", ps_c);
     pvPut(cycle_count);
     ps_status = 2;
     pvPut(ps_status);
     if (use_min) ps_curr_setpt = ps_curr_min;
     else ps_curr_setpt = 0.0;
     pvPut(ps_curr_setpt);
     ps_curr_setpt_prev = ps_curr_setpt;

 } state psControl_on_wait



 when((!ps_on) && ((!norampoff) || standardize ||
     efTest(ps_curr_setpt_ef) ||
     efTest(ps_curr_res_ef) ||
     ((control_setpt != 2) &&
      (control_setpt != 0))))
 {
     if ((control_setpt == 5) ||
  (control_setpt == 1)) {
  ps_curr_setpt = 0.0;
  pvPut(ps_curr_setpt);
     }
     ps_status = 10;
     pvPut(ps_status);
     norampoff = 1;
     pvPut(norampoff);
     standardize = 0;
     pvPut(standardize);
     control_setpt = 0;
     pvPut(control_setpt);
     efClear(ps_curr_setpt_ef);
     efClear(ps_curr_res_ef);
     ps_curr_setpt_prev = 0.0;

 } state psControl_request



 when((control_setpt < 0) || (control_setpt > 7)) {
     control_setpt = 0;
     pvPut(control_setpt);
 } state psControl_request



 when((!norampoff) || standardize || (control_setpt != 0) ||
      efTest(ps_curr_res_ef))
 {
     ps_limits_flag = 1;
     pvPut(ps_limits_flag);
     ps_curr_setpt_prev = ps_curr_setpt;
     if ((control_setpt == 4) || standardize) {
  ps_didt_select = 0;
  pvPut(ps_didt_select);
  zero_current = 1;
  standardize = 1;
  pvPut(standardize);
  control_setpt = 4;
  pvPut(control_setpt);
  standardizing = 1;
  epicsPrintf("Standardize started for %s\n", ps_c);
     }
     else if ((!norampoff) || (control_setpt == 1)) {
  ps_didt_select = 0;
  pvPut(ps_didt_select);
  norampoff = 0;
  pvPut(norampoff);
  control_setpt = 1;
  pvPut(control_setpt);
  ramping = 1;
  epicsPrintf("Ramp off started for %s\n", ps_c);
     } else if (efTest(ps_curr_res_ef)) {
  efClear(ps_curr_res_ef);
  control_setpt = 7;
  pvPut(control_setpt);
  ramping = 1;
     } else if (control_setpt == 7) {
  pvGet(ps_curr_res);
  ramping = 1;
     } else {
  ramping = 1;
     }
     pvPut(cycle_count);
            if (ps_rampdelay >= 0) {
              ps_ramp = 0;
              pvPut(ps_ramp);
            }

 } state psControl_ramp_wait

 when(efTest(ps_curr_setpt_ef))
 {
     efClear(ps_curr_setpt_ef);
     setpt_change = 1;
     wait_needed = 1;
     cycle_count = max_cycle_count + 1;
     ps_limits_flag = 1;
     pvPut(ps_limits_flag);
            if (ps_rampdelay >= 0) {
  ps_ramp = 1;
  pvPut(ps_ramp);
  sw_ramp_delay = 0;
            } else if (ps_curr_setpt != ps_curr_setpt_prev) {
  sw_ramp_delay = 5.0;
     } else {
  sw_ramp_delay = 0;
     }
 } state psControl_ramp_start
    }



    state psControl_on_wait
    {
 entry
 {
     ps_on_des = 1;
     pvPut(ps_on_des);
        }
 when(control_setpt != 2)
 {
 } state psControl_request

 when(delay(5))
 {
 } state psControl_on_check
    }




    state psControl_on_check
    {
 when(ps_on)
 {
     epicsPrintf("PS on - setting to des for %s\n", ps_c);

 } state psControl_request

 when(cycle_count > 2)
 {
     epicsPrintf("Supply off - Ramp on aborted for %s\n", ps_c);
     ps_status = 4;
     pvPut(ps_status);
     control_setpt = 0;
     pvPut(control_setpt);

 } state psControl_request

 when()
 {
     cycle_count++;
     pvPut(cycle_count);

 } state psControl_on_wait
    }



    state psControl_ramp_wait
    {
 entry
 {
     if (ps_ramp && (!setpt_change)) {
  ps_status = 1;
  pvPut(ps_status);
     }




     if ((ps_rampdelay >= 0) && ps_ramp) {
  if ((ps_rampdelay >= 0.02) && (!setpt_change) &&
       (ps_curr_setpt_new != ps_curr_setpt)) {
      sw_ramp_delay = 0;




  } else {
      if (ps_didt > 0) {
   sw_ramp_delay = (ps_curr_setpt - ps_curr_setpt_prev)/
     ps_didt;
       if (sw_ramp_delay < 0.0) sw_ramp_delay *= -1;
      } else {
   sw_ramp_delay = ps_rampdelay;
      }
  }
     } else {
  sw_ramp_delay = 30.0;
     }
        }
 when(!ps_on)
 {
 } state psControl_done

 when(!ps_ramp)
 {
 } state psControl_wait

 when((ps_rampdelay >= 0) && delay(sw_ramp_delay))
 {




     if ((!setpt_change) && (ps_rampdelay >= 0.02) &&
  (ps_curr_setpt != ps_curr_setpt_new)) {
  sw_ramp_delay = ps_rampdelay;
  if (ps_didt <= 0) {
      ps_curr_setpt = ps_curr_setpt_new;
  } else if (ps_curr_setpt_new > ps_curr_setpt) {
      ps_curr_setpt += ps_didt * ps_rampdelay;
      if (ps_curr_setpt >= ps_curr_setpt_new)
   ps_curr_setpt = ps_curr_setpt_new;
  } else {
      ps_curr_setpt -= ps_didt * ps_rampdelay;
      if (ps_curr_setpt <= ps_curr_setpt_new)
   ps_curr_setpt = ps_curr_setpt_new;
  }
  pvPut(ps_curr_setpt);
     } else {
  ps_ramp = 0;
  pvPut(ps_ramp);
     }
 } state psControl_ramp_wait

 when(delay(30.0))
 {
     ramp_timeout = 1;
     efClear(ps_curr_setpt_ef);

 } state psControl_done
    }





    state psControl_wait
    {
 entry
 {
     wait_delay_increment = 1.0;
     wait_delay_sofar = 0;
     pvPut(wait_delay_sofar);
     wait_delay_total = wait_delay;
     efClear(ps_curr_setpt_ef);
        }

 when((!ps_on) || (standardizing && ((!standardize) || (!norampoff) ||
         (control_setpt != 4))) ||
      (setpt_change && (standardize || (!norampoff) ||
          (control_setpt != 0))) ||
      ((cycle_count > max_cycle_count) &&
              ((wait_delay_sofar >= wait_delay_total) ||
        (control_setpt == 1))))
 {
 } state psControl_done

 when(ps_ramp)
 {
            if (ps_rampdelay >= 0) {
  ps_ramp = 0;
  pvPut(ps_ramp);
     }
 } state psControl_ramp_wait

 when ((!wait_needed) || ((cycle_count <= max_cycle_count) &&
     (wait_delay_sofar >= wait_delay)))
 {
 } state psControl_set_current

 when (delay(wait_delay_increment))
 {
     if (!setpt_change) {
  ps_status = 2;
  pvPut(ps_status);
     }
     wait_delay_sofar += wait_delay_increment;
     pvPut(wait_delay_sofar);
     wait_delay_increment = wait_delay_total - wait_delay_sofar;
     if (wait_delay_increment > 1.0)
  wait_delay_increment = 1.0;

 } state psControl_wait
    }



    state psControl_set_current
    {
 when() {
     wait_needed = 1;
     ps_curr_setpt_prev = ps_curr_setpt;
     if (ramping)
     {
  cycle_count = max_cycle_count + 1;
  if ((control_setpt == 3) ||
      (control_setpt == 2)) {
      ps_curr_setpt = ps_curr_des;
      des_current = 1;
  } else if (control_setpt == 6) {
      ps_curr_setpt = ps_curr_min;
      ramp_current = 1;
  } else if (control_setpt == 7) {
      ps_curr_setpt = ps_curr_res;
      ramp_current = 1;
  } else {
      ps_curr_setpt = 0.0;
      zero_current = 1;
  }
     }
     else if (cycle_count < max_cycle_count)
     {
  change_count++;
  cycle_count = change_count/2;
  pvPut(cycle_count);
  zero_current = zero_current ? 0 : 1;
  if (zero_current) ps_curr_setpt = ps_curr_min;
  else if (use_drv ||
    ((ps_curr_des < 0.1) &&
     (ps_curr_des > -0.1)))
      if (pos_pol) ps_curr_setpt = ps_curr_drvh;
      else ps_curr_setpt = ps_curr_drvl;
  else ps_curr_setpt = ps_curr_des;
     }
     else
     {
  cycle_count = max_cycle_count + 1;
  ps_curr_setpt = ps_curr_des;
     }
     ps_curr_setpt_new = ps_curr_setpt;
            if (ps_rampdelay >= 0) {
  ps_ramp = 1;
  pvPut(ps_ramp);
  sw_ramp_delay = 0;
            } else if (ps_curr_setpt != ps_curr_setpt_prev) {
  sw_ramp_delay = 5.0;
     } else {
  sw_ramp_delay = 0;
     }
     if ((ps_rampdelay < 0.02) || (ps_didt <= 0)) {
  pvPut(ps_curr_setpt);
     } else {
  ps_curr_setpt = ps_curr_setpt_prev;
     }
 } state psControl_ramp_start
    }



    state psControl_ramp_start
    {
 when(!ps_on)
 {
 } state psControl_done

 when(ps_ramp || delay(sw_ramp_delay))
 {
 } state psControl_ramp_wait
    }




    state psControl_done
    {
 when()
 {
     ps_status = 10;
     if (standardizing) {
  if (!ps_on) {
      epicsPrintf("Supply off - Standardize aborted for %s\n", ps_c);
      ps_status = 4;
  }
  else if ((!standardize) || (!norampoff) ||
           (control_setpt != 4)) {
      epicsPrintf("Standardize aborted by user for %s\n", ps_c);
      ps_status = 3;
  }
  else if (ramp_timeout) {
      epicsPrintf("Ramp timeout - Standardize aborted for %s\n", ps_c);
      ps_status = 5;
  }
  else {
      epicsPrintf("Standardize completed for %s\n", ps_c);
  }
  standardize = 0;
  pvPut(standardize);
  if (control_setpt == 4) {
      control_setpt = 0;
      pvPut(control_setpt);
  }
  if (ps_status != 10) {
      if (ps_curr_setpt != 0.0) {
   ps_curr_setpt = 0.0;
   pvPut(ps_curr_setpt);
   efSet(ps_curr_setpt_ef);
      }
  }
     } else if (zero_current || ramp_timeout || (!ps_on)) {
   if ((!norampoff) || (control_setpt == 1)) {
      epicsPrintf("PS ramped to zero - setting PS off for %s\n", ps_c);
      efClear(ps_curr_setpt_ef);
      norampoff = 1;
      pvPut(norampoff);
      ps_on_des = 0;
      pvPut(ps_on_des);
  }
  if (ramp_timeout || (!ps_on) ||
      (control_setpt == 1) ||
      (control_setpt == 5)) {
      control_setpt = 0;
      pvPut(control_setpt);
  }
     } else if ((!setpt_change) &&
         ((des_current &&
    ((control_setpt == 2) ||
     (control_setpt == 3))) ||
          (ramp_current &&
    ((control_setpt == 6) ||
     (control_setpt == 7))))) {
  control_setpt = 0;
  pvPut(control_setpt);
     }
     pvPut(ps_status);
     if (!efTest(ps_curr_setpt_ef) || (!ps_on)) {
  ps_limits_flag = 0;
  pvPut(ps_limits_flag);
     }
            if ((ps_rampdelay >= 0) && ps_ramp) {
              ps_ramp = 0;
              pvPut(ps_ramp);
            }
     ps_curr_setpt_prev = ps_curr_setpt;

 } state psControl_request
    }
}

exit{}
