The FSS's SLOW PID Servo

attachment:slow.pl

#!/usr/bin/perl -w

# PID Servo for PSL-FSS (Slow)
# Tobin Fricke 2007-01-09

use strict;
#use Scalar::Util qw(looks_like_number);

sub looks_like_number {
    return ($_[0] =~ /^-?\d+\.?\d*$/);  #FIXME
}

use EpicsTools;

# Parameters
my $process  = 'C1:PSL-FSS_FAST';
my $actuator = 'C1:PSL-FSS_SLOWDC';
my $setpoint = 0;
my $blinkystatus = 0;

my ($KpParam, $KiParam, $KdParam) = 
        ('C1:PSL-FSS_SLOWKP', 'C1:PSL-FSS_SLOWKI', 'C1:PSL-FSS_SLOWKD');
my $timestepParam = 'C1:PSL-FSS_TIMEOUT';

#($KpParam, $KiParam, $KdParam) = (-0.001, -0.0025, -0.005);
# Old values were Kp=-.15, Ki=0,Dk=-0.005
# More parameters -- these ones actually have to be numbers
my @hard_stops = (-5.0, 5.0);  # Perl5 apparently doesn't have an Inf value
my $increment_limit = 0.01;

# Variables
my @u;  # outputs to the actuator
my @e;  # error signal                                                                                            

my $debug = 1;

print "Starting FSS Slow Servo\n";

# Shift some initial values into our registers                                                                    

while ($#u < 2) {
    unshift(@u, get_value($actuator));
    unshift(@e, 0);
    print("Current value of actuator = $u[0]\n");
}

# Start our controller                                                                                            

while (1) {
    # Get the current time step                                                                                   
    my $timestep = get_value($timestepParam);

    # Sleep for the rest of the time interval                                                                     
    sleep($timestep);

    # Blink the blinky light                                                                                      
    if ($blinkystatus) {
        $blinkystatus = 0;
    } else {
        $blinkystatus = 1;
    }

    if ($debug) {
        print "\nSLOW_BEAT --> $blinkystatus\n";
    }
    epWrite('C1:PSL-FSS_SLOWBEAT', $blinkystatus);


    # Make sure the "ENABLE" button is checked                                                                    
    if (get_value("C1:PSL-FSS_SLOWLOOP") == 0) {
        printf("FSS_SLOWLOOP disabled -- control loop disabled.\n");
        next;
    }

    # Make sure the loop is supposed to be active                                                                 
    if (get_value("C1:PSL-FSS_RCTRANSPD") < get_value("C1:PSL-FSS_LOCKEDLEVEL")) {
        print("Reference Cavity not locked -- control loop disabled.\n");
        next;
    }

    # Instead of using the actuation we requested in the previous step as the                                     
    # previous value of the actuation, we'll read the current actuation from                                      
    # EPICS.  This prevents us from monopolizing the slider.                                                      
    $u[0] = get_value($actuator);

    if ($debug) {
        print "\nActuator --> $u[0]\n";
   }

    # Make room for the present time                                                                              
    unshift(@e, undef);
    unshift(@u, undef);

    # Read the PID parameters in case they have changed                                                           
    my ($Kp, $Ki, $Kd) = get_value($KpParam, $KiParam, $KdParam);
    $Ki *= $timestep;
    $Kd /= $timestep;

    if ($debug) {
         print("Kp = $Kp\tKi = $Ki\tKd = $Kd\n");
    }

    # Read the current value of the Process Variable and the Setpoint                                             
    my $p = &get_value($process);
    my $s = &get_value($setpoint);

    # The basic finite-difference PID approximation                                                               
    $e[0] = $p - $s;
    # $u[0] = $u[1] + ($Kp + $Ki + $Kd)*$e[0] - ($Kp + 2*$Kd)*$e[1] + $Kd * $e[2];                                
    $u[0] = $u[1];
    $u[0] = $u[0] + $Ki * ($e[0]);
    $u[0] = $u[0] + $Kp * ($e[0] - $e[1]);
    $u[0] = $u[0] + $Kd * ($e[0] - 2*$e[1] + $e[2]);

    # Enforce hard stops and maximum |increment|                                                                  
    $u[0] = $u[1] + rail($u[0] - $u[1],$increment_limit);
    $u[0] = rail($u[0], @hard_stops);

    # Bullshit rounding to prevent epWrite from complaining about the                                             
    # number of digits in the $u[0] variable                                                                      
    my $tempn = int($u[0]*10000)/10000;
    $u[0] = $tempn;

    # Perform the actuation                                                                                       
    if ($debug) {
        print "Actuator <-- $u[0]\n";
    }
    epWrite($actuator, $u[0]);

    # Discard samples sufficiently far in the past                                                                
    pop(@e);
    pop(@u);
}

# We need to distinguish between literal numbers and EPICS                                                        
# process variables.  If we're given the name of a process                                                        
# variable, we'll use ezcaread to dereference it.                                                                 

sub get_value {
    my @results = ();
    foreach (@_) {
        if (looks_like_number($_)) {
            push(@results, $_);
        } else {
            my @epResult = epRead($_);
            pop @epResult or die "Could not read EPICS process variable.";
            push(@results, pop(@epResult));
        }
    }
    if ($#results == 0) {  #FIXME: Is this necessary?                                                             
        return $results[0];
    } else {
        return @results;
    }
}

sub rail {
    my $value = shift(@_);
    my @limits = @_;

    # If we're only given one value for the limits, we'll                                                         
    # assume the limits are symmetric, i.e. lim ==> (-lim,lim)                                                    
    if ($#limits == 0) {
        unshift(@limits, -$limits[0]);
    }

    if ($limits[1] < $limits[0]) {
        die "Upper limit is lower than lower limit!";
    }

    ($value < $limits[0] ? $limits[0] :
    ($value > $limits[1] ? $limits[1] : $value));
}