1 #!/usr/bin/perl -w
   2 
   3 # PID Servo for PSL-FSS (Slow)
   4 # Tobin Fricke 2007-01-09
   5 
   6 use strict;
   7 #use Scalar::Util qw(looks_like_number);
   8 
   9 sub looks_like_number {
  10     return ($_[0] =~ /^-?\d+\.?\d*$/);  #FIXME
  11 }
  12 
  13 use EpicsTools;
  14 
  15 # Parameters
  16 my $process  = 'C1:PSL-FSS_FAST';
  17 my $actuator = 'C1:PSL-FSS_SLOWDC';
  18 my $setpoint = 0;
  19 my $blinkystatus = 0;
  20 
  21 my ($KpParam, $KiParam, $KdParam) = 
  22 	('C1:PSL-FSS_SLOWKP', 'C1:PSL-FSS_SLOWKI', 'C1:PSL-FSS_SLOWKD');
  23 my $timestepParam = 'C1:PSL-FSS_TIMEOUT';
  24 
  25 #($KpParam, $KiParam, $KdParam) = (-0.001, -0.0025, -0.005);
  26 # Old values were Kp=-.15, Ki=0,Dk=-0.005
  27 # More parameters -- these ones actually have to be numbers
  28 my @hard_stops = (-5.0, 5.0);  # Perl5 apparently doesn't have an Inf value
  29 my $increment_limit = 0.01;
  30 
  31 # Variables
  32 my @u;	# outputs to the actuator
  33 my @e;  # error signal
  34 
  35 my $debug = 1;
  36 
  37 print "Starting FSS Slow Servo\n";
  38 
  39 # Shift some initial values into our registers
  40 
  41 while ($#u < 2) {
  42     unshift(@u, get_value($actuator));
  43     unshift(@e, 0);
  44     print("Current value of actuator = $u[0]\n");
  45 }
  46 
  47 # Start our controller
  48 
  49 while (1) {
  50     # Get the current time step
  51     my $timestep = get_value($timestepParam);
  52 
  53     # Sleep for the rest of the time interval
  54     sleep($timestep);
  55 
  56     # Blink the blinky light
  57     if ($blinkystatus) {
  58         $blinkystatus = 0;
  59     } else {
  60         $blinkystatus = 1;
  61     }
  62  
  63     if ($debug) {
  64 	print "\nSLOW_BEAT --> $blinkystatus\n";
  65     }
  66     epWrite('C1:PSL-FSS_SLOWBEAT', $blinkystatus);
  67 
  68 
  69     # Make sure the "ENABLE" button is checked
  70     if (get_value("C1:PSL-FSS_SLOWLOOP") == 0) {
  71         printf("FSS_SLOWLOOP disabled -- control loop disabled.\n");
  72 	next;
  73     }
  74 
  75     # Make sure the loop is supposed to be active
  76     if (get_value("C1:PSL-FSS_RCTRANSPD") < get_value("C1:PSL-FSS_LOCKEDLEVEL")) {
  77 	print("Reference Cavity not locked -- control loop disabled.\n");
  78 	next;
  79     }
  80 
  81     # Instead of using the actuation we requested in the previous step as the 
  82     # previous value of the actuation, we'll read the current actuation from
  83     # EPICS.  This prevents us from monopolizing the slider.
  84     $u[0] = get_value($actuator);
  85 
  86     if ($debug) {
  87 	print "\nActuator --> $u[0]\n";
  88    }
  89     
  90     # Make room for the present time
  91     unshift(@e, undef);
  92     unshift(@u, undef);
  93     
  94     # Read the PID parameters in case they have changed
  95     my ($Kp, $Ki, $Kd) = get_value($KpParam, $KiParam, $KdParam);
  96     $Ki *= $timestep;
  97     $Kd /= $timestep;
  98 
  99     if ($debug) {
 100          print("Kp = $Kp\tKi = $Ki\tKd = $Kd\n");
 101     }
 102 
 103     # Read the current value of the Process Variable and the Setpoint
 104     my $p = &get_value($process);
 105     my $s = &get_value($setpoint);
 106  
 107     # The basic finite-difference PID approximation
 108     $e[0] = $p - $s;
 109     # $u[0] = $u[1] + ($Kp + $Ki + $Kd)*$e[0] - ($Kp + 2*$Kd)*$e[1] + $Kd * $e[2];
 110     $u[0] = $u[1];
 111     $u[0] = $u[0] + $Ki * ($e[0]);
 112     $u[0] = $u[0] + $Kp * ($e[0] - $e[1]);
 113     $u[0] = $u[0] + $Kd * ($e[0] - 2*$e[1] + $e[2]);
 114    
 115     # Enforce hard stops and maximum |increment|
 116     $u[0] = $u[1] + rail($u[0] - $u[1],$increment_limit);
 117     $u[0] = rail($u[0], @hard_stops);
 118 
 119     # Bullshit rounding to prevent epWrite from complaining about the
 120     # number of digits in the $u[0] variable
 121     my $tempn = int($u[0]*10000)/10000;
 122     $u[0] = $tempn;
 123 
 124     # Perform the actuation
 125     if ($debug) {
 126 	print "Actuator <-- $u[0]\n";
 127     }
 128     epWrite($actuator, $u[0]);
 129 
 130     # Discard samples sufficiently far in the past
 131     pop(@e);
 132     pop(@u);
 133 }
 134 
 135 # We need to distinguish between literal numbers and EPICS
 136 # process variables.  If we're given the name of a process
 137 # variable, we'll use ezcaread to dereference it.
 138 
 139 sub get_value {
 140     my @results = ();
 141     foreach (@_) {
 142         if (looks_like_number($_)) {
 143 	    push(@results, $_);
 144         } else {
 145             my @epResult = epRead($_);
 146             pop @epResult or die "Could not read EPICS process variable.";
 147             push(@results, pop(@epResult)); 
 148         }
 149     }
 150     if ($#results == 0) {  #FIXME: Is this necessary?
 151 	return $results[0];
 152     } else {
 153 	return @results;
 154     }
 155 }
 156 
 157 sub rail {
 158     my $value = shift(@_);
 159     my @limits = @_;
 160 
 161     # If we're only given one value for the limits, we'll
 162     # assume the limits are symmetric, i.e. lim ==> (-lim,lim)
 163     if ($#limits == 0) { 
 164         unshift(@limits, -$limits[0]);
 165     }
 166 
 167     if ($limits[1] < $limits[0]) {
 168 	die "Upper limit is lower than lower limit!";
 169     }
 170     
 171     ($value < $limits[0] ? $limits[0] :
 172     ($value > $limits[1] ? $limits[1] : $value));
 173 }