First tests pass

Author: ufechner7

Project.toml

@@ -3,7 +3,13 @@ uuid = "9e74470a-8ab1-4f32-b878-f32ebd236ea2"
 authors = ["Uwe Fechner <[email protected]> and contributors"]
 version = "1.0.0-DEV"
 
+[deps]
+Parameters = "d96e819e-fc66-5662-9728-84c9c7592b0a"
+StructTypes = "856f2bd8-1eba-4b0a-8007-ebc267875bd4"
+
 [compat]
+Parameters = "0.12.3"
+StructTypes = "1.11.0"
 julia = "1.6.7"
 
 [extras]

data/wc_settings.yaml

@@ -0,0 +1,36 @@
+wc_settings:
+    dt: 0.02         # time step of the winch controller
+    test: false
+    fac: 0.25        # factor for I and P of lower force controller
+    max_iter: 100    # max iterations limit for the PID solvers
+    iter: 0          # actual max iterations of the PID solvers
+    t_startup: 0.25  # startup time for soft start  
+    t_blend: 0.1     # blending time of the mixers in seconds
+    v_sat_error: 1.0 # limit of reel-out speed error, used by the input sat block of the speed controller
+    v_sat: 8.0       # limit of reel-out speed, used by the output sat block of the speed controller
+    v_ri_max: 8.0    # maximal reel-in speed [m/s]
+    p_speed: 0.125   # P value of the speed controller
+    i_speed: 4.0     # I value of the speed controller
+    kb_speed: 4.0    # back calculation constant for the anti-windup loop of the speed controller
+    kt_speed: 5.0    # tracking constant of the speed controller
+    vf_max: 2.75     # reel-out velocity where the set force should reach it's maximum
+    pf_low: 0.00014  # P constant of the lower force controller 0.013,  0.00014 also works
+    if_low: 0.01125  # I constant of the lower force controller 0.0225, 0.01125 also works
+
+    df_low: 0.0      # D constant of upper force controller 0.000017
+    nf_low:  0       # filter constant n of upper force controller
+    kbf_low: 8.0     # back calculation constant for the anti-windup loop of the lower force controller
+    ktf_low: 8.0     # tracking constant of the lower force controller
+    f_low: 350       # lower force limit [N]
+    f_reelin: 700    # set force for reel-in phase [N]
+    f_high: 3800     # upper force limit [N]
+    pf_high: 0.0002304 # P constant of upper force controller
+    if_high: 0.012     # I constant of upper force controller
+    df_high: 0.000034  # D constant of upper force controller
+    nf_high: 15.0    # filter constant n of upper force controller
+    kbf_high: 1.0    # back calculation constant for the anti-windup loop of the upper force controller
+    ktf_high: 10.0   # tracking constant of the upper force controller
+    winch_iter: 10   # iterations of the winch model
+    max_acc: 8.0     # maximal acceleration of the winch (derivative of the set value of the reel-out speed)
+    kv: 0.06         # proportional factor of the square root law, see function calc_vro
+    

src/WinchControllers.jl

@@ -1,5 +1,11 @@
 module WinchControllers
 
+using Parameters, StructTypes
+
+export WCSettings
+
 # Write your package code here.
+include("utils.jl")
+include("wc_settings.jl")
 
 end

src/utils.jl

@@ -0,0 +1,67 @@
+"""
+A collection of control functions for discrete control
+
+Functions:
+
+- saturate
+- limit 
+- merge_angles
+- moving_average
+
+Implemented as described in the PhD thesis of Uwe Fechner.
+"""
+
+""" Calculate a saturated value, that stays within the given limits. """
+function saturate(value, min_, max_)
+    result = value
+    if result > max_
+        result = max_
+    elseif result < min_
+        result = min_
+    end
+    result
+end
+
+""" 
+Limit the value of a variable. 
+
+Usage:
+@limit x 1 4 # limits the value to the range    1 .. 4,  modifies x
+@limit x 10  # limits the value to the range -inf .. 10, modifies x
+
+"""
+macro limit(name, min, max=nothing)
+    if isnothing(max)
+        max = min
+        min = :(typemin($min))
+    end
+
+    return esc( :($name = clamp($name, $min, $max)) )
+end
+
+"""
+    merge_angles(alpha, beta, factor_beta)
+
+Calculate the weighted average of two angles. The weight of beta,
+factor_beta must be between 0 and 1.
+"""
+function merge_angles(alpha, beta, factor_beta)
+    x1 = sin(alpha)
+    y1 = cos(alpha)
+    x2 = sin(beta)
+    y2 = cos(beta)
+    x = x1 * (1.0 - factor_beta) + x2 * factor_beta
+    y = y1 * (1.0 - factor_beta) + y2 * factor_beta
+    atan(x, y)
+end
+
+# calculate the moving average of a vector over a window
+function moving_average(data, window)
+    local result
+    if length(data) <= window
+        result = mean(data)
+    else
+        result = mean(data[length(data)-window:end])
+    end
+    result
+end