diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index ccd69640..1b5ff977 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -21,7 +21,6 @@ jobs: version: - '1.10' - '1' - - 'pre' os: - ubuntu-latest arch: @@ -34,6 +33,9 @@ jobs: arch: x64 version: 1 steps: + - name: Install matplotlib + run: if [ "$RUNNER_OS" = "Linux" ]; then sudo apt-get install -y python3-matplotlib; export PYTHON=$PYTHON_PATH; fi + shell: bash - uses: actions/checkout@v4 - uses: julia-actions/setup-julia@v2 with: diff --git a/.gitignore b/.gitignore index 7e674994..efbb41e3 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,7 @@ data/corr_vec.jld2.bak output/*.arrow data/gui.yaml bin/kps-image-1.10-plot_II.so +docs/.$parking.drawio.bkp +examples_3d/Manifest.toml +bin/kps-image-1.10-fix_yaw.so +Manifest-v1.11.toml diff --git a/Project.toml b/Project.toml index 88309b06..d275de79 100644 --- a/Project.toml +++ b/Project.toml @@ -5,14 +5,17 @@ version = "0.2.10" [deps] JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819" +KiteModels = "b94af626-7959-4878-9336-2adc27959007" KiteUtils = "90980105-b163-44e5-ba9f-8b1c83bb0533" NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56" NonlinearSolve = "8913a72c-1f9b-4ce2-8d82-65094dcecaec" Observables = "510215fc-4207-5dde-b226-833fc4488ee2" Parameters = "d96e819e-fc66-5662-9728-84c9c7592b0a" Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7" +Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" StaticArrayInterface = "0d7ed370-da01-4f52-bd93-41d350b8b718" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" +StatsBase = "2913bbd2-ae8a-5f71-8c99-4fb6c76f3a91" StructTypes = "856f2bd8-1eba-4b0a-8007-ebc267875bd4" Timers = "21f18d07-b854-4dab-86f0-c15a3821819a" WinchModels = "7dcfa46b-7979-4771-bbf4-0aee0da42e1f" @@ -21,9 +24,9 @@ YAML = "ddb6d928-2868-570f-bddf-ab3f9cf99eb6" [compat] ControlPlots = "0.2.1" JLD2 = "0.5.2" -KiteModels = "0.6.6" -KiteUtils = "0.7" -KiteViewers = "0.4.16" +KiteModels = "0.6.8" +KiteUtils = "0.8.3" +KiteViewers = "0.4.19" NLsolve = "4.5" NonlinearSolve = "3.10.0" Observables = "0.3, 0.4, 0.5" diff --git a/bin/create_sys_image b/bin/create_sys_image index 4832c0a0..09e09032 100755 --- a/bin/create_sys_image +++ b/bin/create_sys_image @@ -19,6 +19,7 @@ fi mkdir -p output export MPLBACKEND=qt5agg +export JULIA_PKG_SERVER_REGISTRY_PREFERANCE=eager if ! command -v juliaup &> /dev/null; then echo "Please install the Julia installer 'juliaup'!" diff --git a/bin/run_julia b/bin/run_julia index a3c00342..623ff8fd 100755 --- a/bin/run_julia +++ b/bin/run_julia @@ -13,6 +13,7 @@ else fi GCT="--gcthreads=$FAST_CORES,1" +export JULIA_PKG_SERVER_REGISTRY_PREFERANCE=eager julia_version=$(julia --version | awk '{print($3)}') julia_major=${julia_version:0:3} diff --git a/data/fpc_settings.yaml b/data/fpc_settings.yaml index 6b937fc1..ecf3dd26 100644 --- a/data/fpc_settings.yaml +++ b/data/fpc_settings.yaml @@ -1,3 +1,4 @@ +# settings for the 20 qm kite fpc_settings: dt: 0.025 # time step of the flight path controller log_level: 2 # log level, 0=quiet @@ -13,9 +14,9 @@ fpc_settings: reset_int1_to_zero: true init_opt_to_zero: false # if the root finder should start with zero - p: 20.0 # P gain of the PID controller + p: 20 # P gain of the PID controller i: 1.2 # I gain of the PID controller - d: 10.0 # D gain of the PID controller + d: 10 # D gain of the PID controller gain: 0.04 # additional factor for P, I and D c1: 0.0612998898221 # was: 0.0786 c2: 1.22597628388 # was: 2.508 diff --git a/data/settings.yaml b/data/settings.yaml index 6f8afea6..27beb11e 100644 --- a/data/settings.yaml +++ b/data/settings.yaml @@ -31,6 +31,7 @@ steering: k_ds: 1.5 # influence of the depower angle on the steering sensitivity delta_st: 0.02 # steering increment (when pressing RIGHT) max_steering: 16.83 # max. steering angle of the side planes for four point model [degrees] + cs_4p: 1.0 # correction factor for the steering of the four point model depower: alpha_d_max: 31.0 # max depower angle [deg] @@ -77,10 +78,12 @@ bridle: rel_damping: 6.0 # relative damping of the kite spring (relative to main tether) [-] kcu: - kcu_mass: 8.4 # mass of the kite control unit [kg] - power2steer_dist: 1.3 # [m] - depower_drum_diameter: 0.069 # [m] - tape_thickness: 0.0006 # [m] + kcu_model: "KCU1" # name of the kite control unit model, KCU1 or KCU2 + kcu_mass: 8.4 # mass of the kite control unit [kg] + kcu_diameter: 0.4 # diameter of the KCU for drag calculation [m] + power2steer_dist: 1.3 # [m] + depower_drum_diameter: 0.069 # [m] + tape_thickness: 0.0006 # [m] v_depower: 0.075 # max velocity of depowering in units per second (full range: 1 unit) v_steering: 0.2 # max velocity of steering in units per second (full range: 2 units) depower_gain: 3.0 # 3.0 means: more than 33% error -> full speed diff --git a/data/settings_hydra20_600.yaml b/data/settings_hydra20_600.yaml index 582903b9..204fa5d1 100644 --- a/data/settings_hydra20_600.yaml +++ b/data/settings_hydra20_600.yaml @@ -31,6 +31,7 @@ steering: k_ds: 1.5 # influence of the depower angle on the steering sensitivity delta_st: 0.02 # steering increment (when pressing RIGHT) max_steering: 16.83 # max. steering angle of the side planes for four point model [degrees] + cs_4p: 1.0 # correction factor for the steering of the four point model depower: alpha_d_max: 31.0 # max depower angle [deg] diff --git a/data/settings_v9b.yaml b/data/settings_v9b.yaml new file mode 120000 index 00000000..ab67a2af --- /dev/null +++ b/data/settings_v9b.yaml @@ -0,0 +1 @@ +../../pykitesim/data/settings_v9b.yaml \ No newline at end of file diff --git a/data/system_v9.yaml b/data/system_v9.yaml new file mode 120000 index 00000000..19c79478 --- /dev/null +++ b/data/system_v9.yaml @@ -0,0 +1 @@ +../../pykitesim/data/system_v9.yaml \ No newline at end of file diff --git a/docs/parking.drawio b/docs/parking.drawio new file mode 100644 index 00000000..a1c8af6f --- /dev/null +++ b/docs/parking.drawio @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/autopilot.jl b/examples/autopilot.jl index 04bc5c9f..22bab469 100644 --- a/examples/autopilot.jl +++ b/examples/autopilot.jl @@ -2,7 +2,6 @@ using Pkg if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies)) using TestEnv; TestEnv.activate() - # pkg"add KiteModels#main" end using Timers; tic() @@ -73,21 +72,17 @@ function init(app::KiteApp; init_viewer=false) project=(KiteUtils.PROJECT) app.kps4 = KPS4(app.kcu) KiteUtils.PROJECT = project - app.wcs = WCSettings() - update(app.wcs) + app.wcs = WCSettings(true; dt=1/app.set.sample_freq) app.wcs.dt = 1/app.set.sample_freq app.dt = app.wcs.dt - app.fcs = FPCSettings(dt=app.dt) - update(app.fcs) - app.fcs.dt = app.wcs.dt + app.fcs = FPCSettings(true; dt=app.dt) app.fcs.log_level = app.set.log_level - app.fpps = FPPSettings() - update(app.fpps) + app.fpps = FPPSettings(true) app.fpps.log_level = app.set.log_level u_d0 = 0.01 * se(project).depower_offset u_d = 0.01 * se(project).depower - app.ssc = SystemStateControl(app.wcs, app.fcs, app.fpps; u_d0, u_d) + app.ssc = SystemStateControl(app.wcs, app.fcs, app.fpps; u_d0, u_d, v_wind=app.set.v_wind) if init_viewer app.viewer= Viewer3D(app.set, app.show_kite; menus=true) app.viewer.menu.options[]=["plot_main", "plot_power", "plot_control", "plot_control_II", "plot_winch_control", "plot_aerodynamics", @@ -177,7 +172,11 @@ function simulate(integrator, stopped=true) if i > 100 dp = KiteControllers.get_depower(app.ssc) if dp < 0.22 dp = 0.22 end - steering = calc_steering(app.ssc) + heading = calc_heading(app.kps4; neg_azimuth=true, one_point=false) + app.ssc.sys_state.heading = heading + app.ssc.sys_state.azimuth = -calc_azimuth(app.kps4) + # steering = calc_steering(app.ssc; heading) + steering = -calc_steering(app.ssc) set_depower_steering(app.kps4.kcu, dp, steering) end if i == 200 && ! app.parking @@ -187,6 +186,7 @@ function simulate(integrator, stopped=true) v_ro = calc_v_set(app.ssc) # t_sim = @elapsed KiteModels.next_step!(app.kps4, integrator; set_speed=v_ro, dt=app.dt) + sys_state.orient .= calc_orient_quat(app.kps4) update_sys_state!(sys_state, app.kps4) acc = (app.kps4.vel_kite - last_vel)/app.dt last_vel = deepcopy(app.kps4.vel_kite) diff --git a/examples/autopilot_1p.jl b/examples/autopilot_1p.jl index 67b633e6..42b8a37c 100644 --- a/examples/autopilot_1p.jl +++ b/examples/autopilot_1p.jl @@ -5,19 +5,19 @@ if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies)) end using Timers; tic() -using KiteControllers, KiteViewers, KiteModels +using KiteControllers, KiteViewers, KiteModels, Rotations set = deepcopy(load_settings("system.yaml")) kcu::KCU = KCU(set) kps3::KPS3 = KPS3(kcu) -wcs::WCSettings = WCSettings(dt = 1/set.sample_freq) -fcs::FPCSettings = FPCSettings(dt = wcs.dt) -fpps::FPPSettings = FPPSettings() +wcs::WCSettings = WCSettings(true, dt = 1/set.sample_freq) +fcs::FPCSettings = FPCSettings(true, dt = wcs.dt) +fpps::FPPSettings = FPPSettings(true) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower -ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) +ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d, v_wind=set.v_wind) dt::Float64 = wcs.dt # result of tuning, factor 0.9 to increase robustness @@ -53,7 +53,10 @@ function simulate(integrator) if i > 100 dp = KiteControllers.get_depower(ssc) if dp < 0.22 dp = 0.22 end - steering = calc_steering(ssc) + heading = calc_heading(kps3; neg_azimuth=true, one_point=false) + sys_state.heading = heading + sys_state.azimuth = -calc_azimuth(kps3) + steering = -calc_steering(ssc) set_depower_steering(kps3.kcu, dp, steering) end if i == 200 @@ -65,7 +68,8 @@ function simulate(integrator) t_sim = @elapsed KiteModels.next_step!(kps3, integrator; set_speed=v_ro, dt=dt) sys_state = SysState(kps3) on_new_systate(ssc, sys_state) - if mod(i, TIME_LAPSE_RATIO) == 0 + if mod(i, TIME_LAPSE_RATIO) == 0 + sys_state.orient = quat2viewer(QuatRotation(sys_state.orient)) KiteViewers.update_system(viewer, sys_state; scale = 0.04/1.1, kite_scale=6.6) set_status(viewer, String(Symbol(ssc.state))) wait_until(start_time_ns + 1e9*dt, always_sleep=true) diff --git a/examples/joystick.jl b/examples/joystick.jl index 0a0ad8b4..1d18c145 100644 --- a/examples/joystick.jl +++ b/examples/joystick.jl @@ -16,12 +16,12 @@ if ! @isdefined js; const jsbuttons = JSButtonState() async_read!(js, jsaxes, jsbuttons) end -wcs::WCSettings = WCSettings(); wcs.dt = 1/set.sample_freq -fcs::FPCSettings = FPCSettings(dt=wcs.dt) -fpps::FPPSettings = FPPSettings() +wcs::WCSettings = WCSettings(true, dt = 1/set.sample_freq) +fcs::FPCSettings = FPCSettings(true, dt=wcs.dt) +fpps::FPPSettings = FPPSettings(true) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower -ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) +ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d, v_wind = set.v_wind) dt::Float64 = wcs.dt # the following values can be changed to match your interest @@ -51,7 +51,8 @@ function simulate(integrator) depower = KiteControllers.get_depower(ssc) # println("dp: ", dp) if depower < 0.22; depower = 0.22; end - steering = calc_steering(ssc, jsaxes.x) + heading = calc_heading(app.kps4; neg_azimuth=true) + steering = calc_steering(ssc, jsaxes.x; heading) set_depower_steering(kps4.kcu, depower, steering) println("depower: ", depower, " steering: ", round(steering, digits=3)) # set_depower_steering(kps4.kcu, depower, jsaxes.x) diff --git a/examples/menu.jl b/examples/menu.jl index 49eeeb85..c5c49b23 100644 --- a/examples/menu.jl +++ b/examples/menu.jl @@ -1,7 +1,6 @@ using REPL.TerminalMenus -options = ["autopilot_1p = include(\"autopilot_1p.jl\")", - "autopilot_4p = include(\"autopilot.jl\")", +options = ["autopilot_4p = include(\"autopilot.jl\")", "joystick = include(\"joystick.jl\")", "minipilot = include(\"minipilot.jl\")", "minipilot_12 = include(\"minipilot_12.jl\")", diff --git a/examples/minipilot.jl b/examples/minipilot.jl index 15d39d19..d68fead7 100644 --- a/examples/minipilot.jl +++ b/examples/minipilot.jl @@ -11,24 +11,24 @@ set = deepcopy(load_settings("system.yaml")) kcu::KCU = KCU(set) kps4::KPS4 = KPS4(kcu) -wcs::WCSettings = WCSettings(); update(wcs); wcs.dt = 1/set.sample_freq -fcs::FPCSettings = FPCSettings(dt=wcs.dt) -fpps::FPPSettings = FPPSettings() +wcs::WCSettings = WCSettings(true, dt = 1/set.sample_freq) +fcs::FPCSettings = FPCSettings(true, dt=wcs.dt) +fpps::FPPSettings = FPPSettings(true) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower -ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) +ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d, v_wind=set.v_wind) dt::Float64 = wcs.dt function init_globals() global kcu, kps4, wcs, fcs, fpps, ssc - kcu = KCU(se()) + kcu = KCU(set) kps4 = KPS4(kcu) - wcs = WCSettings(); update(wcs); wcs.dt = 1/set.sample_freq - fcs = FPCSettings(dt=wcs.dt) - fpps = FPPSettings() + wcs = WCSettings(true, dt = 1/set.sample_freq) + fcs = FPCSettings(true, dt=wcs.dt) + fpps = FPPSettings(true) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower - ssc = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) + ssc = SystemStateControl(wcs, fcs, fpps; u_d0, u_d, v_wind=set.v_wind) end # the following values can be changed to match your interest @@ -45,10 +45,10 @@ viewer::Viewer3D = Viewer3D(SHOW_KITE) PARKING::Bool = false steps = 0 -if ! @isdefined T; const T = zeros(Int64(MAX_TIME/dt)); end -if ! @isdefined DELTA_T; const DELTA_T = zeros(Int64(MAX_TIME/dt)); end -if ! @isdefined STEERING; const STEERING = zeros(Int64(MAX_TIME/dt)); end -if ! @isdefined DEPOWER_; const DEPOWER_ = zeros(Int64(MAX_TIME/dt)); end +T::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +DELTA_T::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +STEERING::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +DEPOWER_::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) LAST_I::Int64=0 function simulate(integrator, stopped=true) @@ -77,7 +77,10 @@ function simulate(integrator, stopped=true) if i > 100 dp = KiteControllers.get_depower(ssc) if dp < 0.22 dp = 0.22 end - steering = calc_steering(ssc) + heading = calc_heading(kps4; neg_azimuth=true, one_point=false) + ssc.sys_state.heading = heading + ssc.sys_state.azimuth = -calc_azimuth(kps4) + steering = -calc_steering(ssc) set_depower_steering(kps4.kcu, dp, steering) end if i == 200 && ! PARKING diff --git a/examples/minipilot_12.jl b/examples/minipilot_12.jl index a80bd19a..f34eaf4d 100644 --- a/examples/minipilot_12.jl +++ b/examples/minipilot_12.jl @@ -12,24 +12,24 @@ set.segments = 12 kcu::KCU = KCU(set) kps4::KPS4 = KPS4(kcu) -wcs::WCSettings = WCSettings(); update(wcs); wcs.dt = 1/set.sample_freq -fcs::FPCSettings = FPCSettings(dt=wcs.dt) -fpps::FPPSettings = FPPSettings() +wcs::WCSettings = WCSettings(true, dt = 1/set.sample_freq) +fcs::FPCSettings = FPCSettings(true, dt=wcs.dt) +fpps::FPPSettings = FPPSettings(true) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower -ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) +ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d, v_wind=set.v_wind) dt::Float64 = wcs.dt function init_globals() global kcu, kps4, wcs, fcs, fpps, ssc kcu = KCU(set) kps4 = KPS4(kcu) - wcs = WCSettings(); update(wcs); wcs.dt = 1/set.sample_freq - fcs = FPCSettings(dt=wcs.dt) - fpps = FPPSettings() + wcs = WCSettings(true, dt = 1/set.sample_freq) + fcs = FPCSettings(true, dt=wcs.dt) + fpps = FPPSettings(true) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower - ssc = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) + ssc = SystemStateControl(wcs, fcs, fpps; u_d0, u_d, v_wind=set.v_wind) end # the following values can be changed to match your interest @@ -46,10 +46,10 @@ viewer::Viewer3D = Viewer3D(set, SHOW_KITE) PARKING::Bool = false steps = 0 -if ! @isdefined T; const T = zeros(Int64(MAX_TIME/dt)); end -if ! @isdefined DELTA_T; const DELTA_T = zeros(Int64(MAX_TIME/dt)); end -if ! @isdefined STEERING; const STEERING = zeros(Int64(MAX_TIME/dt)); end -if ! @isdefined DEPOWER_; const DEPOWER_ = zeros(Int64(MAX_TIME/dt)); end +T::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +DELTA_T::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +STEERING::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +DEPOWER_::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) LAST_I::Int64=0 function simulate(integrator, stopped=true) @@ -78,7 +78,10 @@ function simulate(integrator, stopped=true) if i > 100 dp = KiteControllers.get_depower(ssc) if dp < 0.22 dp = 0.22 end - steering = calc_steering(ssc) + heading = calc_heading(kps4; neg_azimuth=true, one_point=false) + ssc.sys_state.heading = heading + ssc.sys_state.azimuth = -calc_azimuth(kps4) + steering = -calc_steering(ssc) set_depower_steering(kps4.kcu, dp, steering) end if i == 200 && ! PARKING diff --git a/examples/parking_1p.jl b/examples/parking_1p.jl index dac73149..76d06ec3 100644 --- a/examples/parking_1p.jl +++ b/examples/parking_1p.jl @@ -7,37 +7,39 @@ using Timers; tic() using KiteUtils set = deepcopy(load_settings("system.yaml")) -set.abs_tol=0.00000006 -set.rel_tol=0.0000001 +set.abs_tol=0.00006 +set.rel_tol=0.0001 +set.v_wind = 6.5 -using KiteControllers, KiteViewers, KiteModels, ControlPlots +using KiteControllers, KiteViewers, KiteModels, ControlPlots, Rotations kcu::KCU = KCU(set) kps::KPS3 = KPS3(kcu) -wcs::WCSettings = WCSettings(dt = 1/set.sample_freq) -fcs::FPCSettings = FPCSettings(dt = wcs.dt) -fpps::FPPSettings = FPPSettings() +wcs::WCSettings = WCSettings(true, dt = 1/set.sample_freq) +fcs::FPCSettings = FPCSettings(true, dt = wcs.dt) +fpps::FPPSettings = FPPSettings(true) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower -ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) +ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d, v_wind = set.v_wind) dt::Float64 = wcs.dt -# result of tuning, factor 0.9 to increase robustness -fcs.p = 13.63*0.9 -fcs.i = 0.0 -fcs.d = 27.75*0.9 +# result of tuning +fcs.p = 13.63*0.2 +fcs.i = 0.2 +fcs.d = 27.75*0.85 +MIN_DEPOWER = 0.24 # the following values can be changed to match your interest MAX_TIME::Float64 = 60 -TIME_LAPSE_RATIO = 1 +TIME_LAPSE_RATIO = 6 SHOW_KITE = true # end of user parameter section # viewer::Viewer3D = Viewer3D(SHOW_KITE, "WinchON") steps = 0 -if ! @isdefined T; const T = zeros(Int64(MAX_TIME/dt)); end -if ! @isdefined AZIMUTH; const AZIMUTH = zeros(Int64(MAX_TIME/dt)); end +T::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +AZIMUTH::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) function simulate(integrator) start_time_ns = time_ns() @@ -50,15 +52,14 @@ function simulate(integrator) on_new_systate(ssc, sys_state) while true if i > 100 - depower = KiteControllers.get_depower(ssc) - if depower < 0.22; depower = 0.22; end - steering = calc_steering(ssc, 0) + heading = calc_heading(kps; neg_azimuth=true, one_point=false) + steering = calc_steering(ssc, 0; heading) time = i * dt # disturbance if time > 20 && time < 21 steering = 0.1 end - set_depower_steering(kps.kcu, depower, steering) + set_depower_steering(kps.kcu, MIN_DEPOWER, steering) end v_ro = 0.0 t_sim = @elapsed KiteModels.next_step!(kps, integrator; set_speed=v_ro, dt=dt) @@ -70,6 +71,7 @@ function simulate(integrator) AZIMUTH[i] = sys_state.azimuth on_new_systate(ssc, sys_state) if mod(i, TIME_LAPSE_RATIO) == 0 + sys_state.orient = quat2viewer(QuatRotation(sys_state.orient)) KiteViewers.update_system(viewer, sys_state; scale = 0.08, kite_scale=3) set_status(viewer, String(Symbol(ssc.state))) wait_until(start_time_ns + 1e9*dt, always_sleep=true) @@ -102,15 +104,7 @@ function play() global steps integrator = KiteModels.init_sim!(kps, stiffness_factor=0.04) toc() - try - steps = simulate(integrator) - catch e - if isa(e, AssertionError) - println("AssertionError! Halting simulation.") - else - println("Exception! Halting simulation.") - end - end + steps = simulate(integrator) GC.enable(true) end diff --git a/examples/parking_4p.jl b/examples/parking_4p.jl index 32c3d0b1..02242a83 100644 --- a/examples/parking_4p.jl +++ b/examples/parking_4p.jl @@ -5,39 +5,63 @@ if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies)) end using Timers; tic() -using KiteControllers, KiteViewers, KiteModels, ControlPlots +using KiteControllers, KiteViewers, KiteModels, ControlPlots, Rotations set = deepcopy(load_settings("system.yaml")) -set.abs_tol=0.00000006 -set.rel_tol=0.0000001 +set.abs_tol=0.00006 +set.rel_tol=0.0001 +set.v_wind = 8.5 # v_min1 7.7; v_min2 8.5 kcu::KCU = KCU(set) kps4::KPS4 = KPS4(kcu) -wcs::WCSettings = WCSettings(dt = 1/set.sample_freq) -fcs::FPCSettings = FPCSettings(dt = wcs.dt) -fpps::FPPSettings = FPPSettings() +@assert set.sample_freq == 20 +wcs::WCSettings = WCSettings(true; dt = 1/set.sample_freq) +fcs::FPCSettings = FPCSettings(true; dt = wcs.dt) +fpps::FPPSettings = FPPSettings(true) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower -ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) +ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d, v_wind = set.v_wind) dt::Float64 = wcs.dt -# result of tuning, factor 0.6 to increase robustness -fcs.p=100 * 0.6 -fcs.i=0.0 -fcs.d=35.81 * 0.6 +if KiteUtils.PROJECT == "system.yaml" + # result of tuning + fcs.p=1.2 + fcs.i=0.04 + fcs.d=13.25*0.95 + MIN_DEPOWER = 0.22 + fcs.use_chi = false + @assert fcs.gain == 0.04 +else + # result of tuning + println("not system.yaml") + fcs.p=1.05 + fcs.i=0.012 + fcs.d=13.25*2.0 + MIN_DEPOWER = 0.4 + fcs.use_chi = false + fcs.gain = 0.04 + fcs.c1 = 0.048 + fcs.c2 = 5.5 +end +println("fcs.p=$(fcs.p), fcs.i=$(fcs.i), fcs.d=$(fcs.d), fcs.gain=$(fcs.gain)") # the following values can be changed to match your interest -MAX_TIME::Float64 = 60 -TIME_LAPSE_RATIO = 1 +MAX_TIME::Float64 = 60 # was 60 +TIME_LAPSE_RATIO = 6 SHOW_KITE = true # end of user parameter section # viewer::Viewer3D = Viewer3D(SHOW_KITE, "WinchON") steps = 0 -if ! @isdefined T; const T = zeros(Int64(MAX_TIME/dt)); end -if ! @isdefined AZIMUTH; const AZIMUTH = zeros(Int64(MAX_TIME/dt)); end +T::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +AZIMUTH::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +HEADING::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +SET_STEERING::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +STEERING::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +AoA::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) function simulate(integrator) + global sys_state start_time_ns = time_ns() clear_viewer(viewer) i=1; j=0; k=0 @@ -47,17 +71,20 @@ function simulate(integrator) sys_state = SysState(kps4) on_new_systate(ssc, sys_state) while true + steering = 0.0 if i > 100 - depower = KiteControllers.get_depower(ssc) - if depower < 0.22; depower = 0.22; end - steering = calc_steering(ssc, 0) + heading = calc_heading(kps4; neg_azimuth=true, one_point=false) + steering = calc_steering(ssc, 0; heading) time = i * dt # disturbance if time > 20 && time < 21 steering = 0.1 end - set_depower_steering(kps4.kcu, depower, steering) - end + set_depower_steering(kps4.kcu, MIN_DEPOWER, steering) + end + SET_STEERING[i] = steering + STEERING[i] = get_steering(kps4.kcu)/set.cs_4p + AoA[i] = kps4.alpha_2 # execute winch controller v_ro = 0.0 t_sim = @elapsed KiteModels.next_step!(kps4, integrator; set_speed=v_ro, dt=dt) @@ -67,9 +94,14 @@ function simulate(integrator) sys_state = SysState(kps4) T[i] = dt * i AZIMUTH[i] = sys_state.azimuth + HEADING[i] = wrap2pi(sys_state.heading) on_new_systate(ssc, sys_state) if mod(i, TIME_LAPSE_RATIO) == 0 - KiteViewers.update_system(viewer, sys_state; scale = 0.08, kite_scale=3) + if KiteUtils.PROJECT == "system.yaml" + KiteViewers.update_system(viewer, sys_state; scale = 0.08, kite_scale=3) + else + KiteViewers.update_system(viewer, sys_state; scale = 0.08*0.5, kite_scale=3) + end set_status(viewer, String(Symbol(ssc.state))) wait_until(start_time_ns + 1e9*dt, always_sleep=true) mtime = 0 @@ -99,26 +131,16 @@ end function play() global steps - integrator = KiteModels.init_sim!(kps4, stiffness_factor=0.04) + integrator = KiteModels.init_sim!(kps4, stiffness_factor=0.5) toc() - try - steps = simulate(integrator) - catch e - if isa(e, AssertionError) - println("AssertionError! Halting simulation.") - else - println("Exception! Halting simulation.") - end - end + steps = simulate(integrator) GC.enable(true) end -function async_play() +function play1() if viewer.stop - @async begin - play() - stop(viewer) - end + play() + stop(viewer) end end @@ -131,10 +153,14 @@ function autopilot() end on(viewer.btn_STOP.clicks) do c; stop(viewer); on_stop(ssc) end -on(viewer.btn_PLAY.clicks) do c; async_play(); end +on(viewer.btn_PLAY.clicks) do c; play1(); end on(viewer.btn_PARKING.clicks) do c; parking(); end play() stop(viewer) -p = plot(T, rad2deg.(AZIMUTH); xlabel="Time [s]", ylabel="Azimuth [deg]") +p = plotx(T, rad2deg.(AZIMUTH), rad2deg.(HEADING), [100*(SET_STEERING), 100*(STEERING)], AoA; + xlabel="Time [s]", + ylabels=["Azimuth [°]", "Heading [°]", "steering [%]", "AoA [°]"], + labels=["azimuth", "heading", ["set_steering", "steering", "AoA"]], + fig="Azimuth, heading, steering and AoA",) display(p) diff --git a/examples/parking_wind_dir.jl b/examples/parking_wind_dir.jl index 593ca351..c042a0e9 100644 --- a/examples/parking_wind_dir.jl +++ b/examples/parking_wind_dir.jl @@ -1,47 +1,69 @@ # park the kind while the wind direction changes using Pkg if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies)) + using Pkg using TestEnv; TestEnv.activate() end using Timers; tic() -using KiteControllers, KiteViewers, KiteModels, ControlPlots +using KiteControllers, KiteViewers, KiteModels, ControlPlots, Rotations, StatsBase + set = deepcopy(load_settings("system.yaml")) +@assert KiteUtils.PROJECT == "system.yaml" +@assert se().v_wind == 9.51 +@assert set.v_wind == 9.51 set.abs_tol=0.00006 set.rel_tol=0.0001 kcu::KCU = KCU(set) kps4::KPS4 = KPS4(kcu) +@assert set.sample_freq == 20 wcs::WCSettings = WCSettings(dt = 1/set.sample_freq) +@assert wc_settings() == "wc_settings.yaml" +update(wcs); wcs.dt = 1/set.sample_freq fcs::FPCSettings = FPCSettings(dt = wcs.dt) +@assert fpc_settings() == "fpc_settings.yaml" +update(fcs); fcs.dt = wcs.dt fpps::FPPSettings = FPPSettings() +@assert fpp_settings() == "fpp_settings.yaml" +update(fpps) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower -ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) +ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d, v_wind = set.v_wind) dt::Float64 = wcs.dt -# result of tuning, factor 0.6 to increase robustness -fcs.p=100 * 0.6 -fcs.i=0.0 -fcs.d=35.81 * 0.6 +# result of tuning +fcs.p=2.0 +fcs.i=0.05 +fcs.d=13.25*0.95 +MIN_DEPOWER = 0.22 +fcs.use_chi = false +@assert fcs.gain == 0.04 # the following values can be changed to match your interest -MAX_TIME::Float64 = 60 -TIME_LAPSE_RATIO = 4 +MAX_TIME::Float64 = 120 +TIME_LAPSE_RATIO = 6 SHOW_KITE = true # For position and velocity vectors of the model the ENU (East North Up) UPWIND_DIR = -pi/2 # the direction the wind is coming from. -UPWIND_DIR2 = 0 # Zero is at north; clockwise positive +UPWIND_DIR2 = -pi/2+deg2rad(90) # Zero is at north; clockwise positive # end of user parameter section # viewer::Viewer3D = Viewer3D(SHOW_KITE, "WinchON") steps = 0 -T::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) -if ! @isdefined AZIMUTH; const AZIMUTH = zeros(Int64(MAX_TIME/dt)); end +T::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +AZIMUTH::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +AZIMUTH_EAST::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +UPWIND_DIR_::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +AV_UPWIND_DIR::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +HEADING::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +SET_STEERING::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +STEERING::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) -function simulate(integrator) +function sim_parking(integrator) upwind_dir=UPWIND_DIR + av_upwind_dir = upwind_dir start_time_ns = time_ns() clear_viewer(viewer) i=1; j=0; k=0 @@ -52,28 +74,42 @@ function simulate(integrator) on_new_systate(ssc, sys_state) while true time = i * dt + steering = 0.0 if i > 100 depower = KiteControllers.get_depower(ssc) - if depower < 0.22; depower = 0.22; end - steering = calc_steering(ssc, 0) + if depower < MIN_DEPOWER; depower = MIN_DEPOWER; end + heading = calc_heading(kps4; neg_azimuth=true, one_point=false) + steering = calc_steering(ssc, 0; heading) set_depower_steering(kps4.kcu, depower, steering) end + SET_STEERING[i] = steering + STEERING[i] = get_steering(kps4.kcu) / set.cs_4p # execute winch controller v_ro = 0.0 - if time > 20 && upwind_dir < UPWIND_DIR2 - upwind_dir += 0.01 - if upwind_dir > 0 - upwind_dir = 0 + if time > 20 + upwind_dir += deg2rad(0.04*2) + if upwind_dir > UPWIND_DIR2 + upwind_dir = UPWIND_DIR2 end + UPWIND_DIR_[i] = upwind_dir + av_upwind_dir = moving_average(UPWIND_DIR_[1:i], 400) + else + upwind_dir=UPWIND_DIR + UPWIND_DIR_[i] = upwind_dir + av_upwind_dir = upwind_dir end - t_sim = @elapsed KiteModels.next_step!(kps4, integrator; set_speed=v_ro, dt, upwind_dir) + t_sim = @elapsed KiteModels.next_step!(kps4, integrator; set_speed=v_ro, dt, upwind_dir=av_upwind_dir) + AV_UPWIND_DIR[i] = av_upwind_dir if t_sim < 0.3*dt t_gc_tot += @elapsed GC.gc(false) end sys_state = SysState(kps4) + sys_state.orient .= calc_orient_quat(kps4) T[i] = dt * i AZIMUTH[i] = sys_state.azimuth + AZIMUTH_EAST[i] = calc_azimuth_east(kps4) + HEADING[i] = wrap2pi(sys_state.heading) on_new_systate(ssc, sys_state) if mod(i, TIME_LAPSE_RATIO) == 0 KiteViewers.update_system(viewer, sys_state; scale = 0.08, kite_scale=3) @@ -104,22 +140,27 @@ function simulate(integrator) return div(i, TIME_LAPSE_RATIO) end -function play() - global steps - integrator = KiteModels.init_sim!(kps4, stiffness_factor=0.04) +function play_parking() + integrator = KiteModels.init_sim!(kps4; delta=0.001, stiffness_factor=0.5) toc() try - steps = simulate(integrator) + steps = sim_parking(integrator) catch e if isa(e, AssertionError) println("AssertionError! Halting simulation.") else println("Exception! Halting simulation.") + throw(e) end end GC.enable(true) end -play() +play_parking() stop(viewer) -plot(T, rad2deg.(AZIMUTH); xlabel="Time [s]", ylabel="Azimuth [deg]") +p=plotx(T, rad2deg.(AZIMUTH), rad2deg.(AZIMUTH_EAST),[rad2deg.(UPWIND_DIR_), rad2deg.(AV_UPWIND_DIR)], + rad2deg.(HEADING), [100*(SET_STEERING), 100*(STEERING)]; + xlabel="Time [s]", + ylabels=["Azimuth [°]", "azimuth_east [°]", "upwind_dir [°]", "Heading [°]", "Steering [%]"], + labels=["azimuth", "azimuth_east", ["upwind_dir", "filtered_upwind_dir"], "heading", ["set_steering", "steering"]]) +display(p) diff --git a/examples/plots.jl b/examples/plots.jl index 069a6949..ad45bf75 100644 --- a/examples/plots.jl +++ b/examples/plots.jl @@ -59,7 +59,7 @@ end function plot_control() log = load_log(basename(KiteViewers.plot_file[]); path=fulldir(KiteViewers.plot_file[])) sl = log.syslog - display(plotx(log.syslog.time, rad2deg.(sl.elevation), rad2deg.(sl.azimuth), rad2deg.(sl.heading), sl.force, 100*sl.depower, 100*sl.steering, sl.sys_state, sl.var_01, sl.var_02; + display(plotx(log.syslog.time, rad2deg.(sl.elevation), rad2deg.(sl.azimuth), rad2deg.(wrap2pi.(sl.heading)), sl.force, 100*sl.depower, 100*sl.steering, sl.sys_state, sl.var_01, sl.var_02; ylabels=["elevation [°]", "azimuth [°]", "heading [°]", "force [N]", "depower [%]", "steering [%]", "fpp_state", "cycle", "fig8"], fig="control", ysize=10, yzoom=0.7)) sleep(0.05) diff --git a/examples/tune_1p.jl b/examples/tune_1p.jl index 75b36be2..3fb81a6f 100644 --- a/examples/tune_1p.jl +++ b/examples/tune_1p.jl @@ -15,9 +15,9 @@ using KiteControllers, KiteModels, BayesOpt, ControlPlots kcu::KCU = KCU(set) kps::KPS3 = KPS3(kcu) -wcs::WCSettings = WCSettings(); wcs.dt = 1/set.sample_freq -fcs::FPCSettings = FPCSettings(dt=wcs.dt) -fpps::FPPSettings = FPPSettings() +wcs::WCSettings = WCSettings(true, dt = 1/set.sample_freq) +fcs::FPCSettings = FPCSettings(true, dt=wcs.dt) +fpps::FPPSettings = FPPSettings(true) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) diff --git a/examples/tune_1p_glb.jl b/examples/tune_1p_glb.jl index 7d06f366..e4cc13f2 100644 --- a/examples/tune_1p_glb.jl +++ b/examples/tune_1p_glb.jl @@ -15,9 +15,9 @@ using KiteControllers, KiteModels, ControlPlots, BlackBoxOptim kcu::KCU = KCU(set) kps::KPS3 = KPS3(kcu) -wcs::WCSettings = WCSettings(); wcs.dt = 1/set.sample_freq -fcs::FPCSettings = FPCSettings(dt=wcs.dt) -fpps::FPPSettings = FPPSettings() +wcs::WCSettings = WCSettings(true, dt = 1/set.sample_freq) +fcs::FPCSettings = FPCSettings(true, dt=wcs.dt) +fpps::FPPSettings = FPPSettings(true) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) diff --git a/examples/tune_4p.jl b/examples/tune_4p.jl index 2c08391d..924136af 100644 --- a/examples/tune_4p.jl +++ b/examples/tune_4p.jl @@ -1,38 +1,37 @@ # activate the test environment if needed using Pkg -if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies)) +if ! ("BayesOpt" ∈ keys(Pkg.project().dependencies)) using TestEnv; TestEnv.activate() Pkg.add("BayesOpt") end using KiteUtils set = deepcopy(load_settings("system.yaml")) -set.abs_tol=0.00006 -set.rel_tol=0.0001 +set.abs_tol=0.0006 +set.rel_tol=0.001 using KiteControllers, KiteModels, BayesOpt, ControlPlots plt.close("all") kcu::KCU = KCU(set) kps4::KPS4 = KPS4(kcu) -wcs::WCSettings = WCSettings(); wcs.dt = 1/set.sample_freq -fcs::FPCSettings = FPCSettings(dt=wcs.dt) -fpps::FPPSettings = FPPSettings() +wcs::WCSettings = WCSettings(true, dt = 1/set.sample_freq) +fcs::FPCSettings = FPCSettings(true, dt=wcs.dt) +fpps::FPPSettings = FPPSettings(true) u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower -ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) +ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d, v_wind = set.v_wind) dt::Float64 = wcs.dt # the following values can be changed to match your interest -MAX_TIME::Float64 = 60 -TIME_LAPSE_RATIO = 1 +MAX_TIME::Float64 = 120 MAX_ITER = 40 SHOW_KITE = false # end of user parameter section # LAST_RES = 1e10 -if ! @isdefined T; const T = zeros(Int64(MAX_TIME/dt)); end -if ! @isdefined AZIMUTH; const AZIMUTH = zeros(Int64(MAX_TIME/dt)); end +T::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) +AZIMUTH::Vector{Float64} = zeros(Int64(MAX_TIME/dt)) function simulate(integrator) i=1 @@ -47,7 +46,7 @@ function simulate(integrator) # disturbance if time < 20 steering = 0.0 - elseif time < 21 + elseif time < 20.5 steering = 0.1 end set_depower_steering(kps4.kcu, depower, steering) @@ -64,10 +63,9 @@ function simulate(integrator) return 1 end -function calc_res(az, suppress_overshoot_factor, t1=20, t2=60) - n1=Int(t1/dt) - n2=Int(t2/dt) - n = length(az) +function calc_res(az, suppress_overshoot_factor, t1=20) + n1 = Int(t1/dt) + n = length(az) res = sum(abs2.(rad2deg.(az[n1:n])))/40 overshoot = zeros(n-n1) for i in 1:n-n1 @@ -76,7 +74,6 @@ function calc_res(az, suppress_overshoot_factor, t1=20, t2=60) end end res += sum(abs2.(rad2deg.(overshoot)))/40 * suppress_overshoot_factor - # if res > 500 res = 500.0 end res end @@ -87,7 +84,7 @@ function test_parking(suppress_overshoot_factor = 3.0) clear!(kps4) KitePodModels.init_kcu!(kcu, set) AZIMUTH .= zeros(Int64(MAX_TIME/dt)) - integrator = KiteModels.init_sim!(kps4, delta=0.1, stiffness_factor=0.5) + integrator = KiteModels.init_sim!(kps4, delta=0.1, stiffness_factor=1) simulate(integrator) res = calc_res(AZIMUTH, suppress_overshoot_factor) if res < LAST_RES @@ -104,8 +101,8 @@ end function f(x) fcs.p = x[1] - fcs.i = x[2] - fcs.d = x[3] + fcs.i = 0.04 + fcs.d = x[2] println("x: ", x) test_parking() end @@ -120,15 +117,19 @@ function tune_4p() set_kernel!(config, "kMaternARD5") println(config.noise) # println(config.n_inner_iterations) - lowerbound = [10., 0., 0.]; upperbound = [120., 4.0, 50.] + lowerbound = [0.1, 7.]; upperbound = [1.5, 16.] optimizer, optimum = bayes_optimization(f, lowerbound, upperbound, config) - println("Opimal parameters: p = $(optimizer[1]), i = $(optimizer[2]), d = $(optimizer[3])") + println("Opimal parameters: p = $(optimizer[1]), d = $(optimizer[2])") println("Optimum value : $(optimum)") end -fcs.p=65 -fcs.i=1.37 -fcs.d=50 +# fcs.p=2.255470121692552*0.7 +# fcs.i=0.0 +# fcs.d=38.724898029839586 +fcs.p=1.2 +fcs.i=0.04 +fcs.d=13.25*0.95 +fcs.use_chi = false println(test_parking()) show_result() diff --git a/examples_3d/Project.toml b/examples_3d/Project.toml new file mode 100644 index 00000000..4bb26c84 --- /dev/null +++ b/examples_3d/Project.toml @@ -0,0 +1,2 @@ +[deps] +KiteModels = "b94af626-7959-4878-9336-2adc27959007" diff --git a/src/KiteControllers.jl b/src/KiteControllers.jl index b1bcfbd4..9aa6148a 100644 --- a/src/KiteControllers.jl +++ b/src/KiteControllers.jl @@ -1,7 +1,7 @@ module KiteControllers using KiteUtils, WinchModels, Parameters, Observables, StaticArrays, NLsolve, Printf -using YAML, StructTypes +using YAML, StructTypes, StatsBase import Base.reset import JLD2 @@ -26,7 +26,7 @@ export calc_v_set, get_set_force, get_status # methods export on_autopilot, on_parking, on_reelin, on_stop, on_new_systate # methods of SystemStateControl export on_winchcontrol, get_depower # methods of SystemStateControl export ssParking, ssPowerProduction, ssReelIn, ssManualOperation -export update, observe! +export update, observe!, moving_average abstract type AbstractForceController end const AFC = AbstractForceController diff --git a/src/flightpathplanner2.jl b/src/flightpathplanner2.jl index 6637174b..1efa897a 100644 --- a/src/flightpathplanner2.jl +++ b/src/flightpathplanner2.jl @@ -27,12 +27,12 @@ function FlightPathPlanner(fpps::FPPSettings, fpca::FlightPathCalculator) end # Start automated power production; Precondition: The kite is parking at a high elevation angle. -function start(fpp::FlightPathPlanner) +function start(fpp::FlightPathPlanner, v_wind) if fpp.fpca._sys_state == ssManualOperation || fpp.fpca._sys_state == ssParking # see: Table 5.3 _switch(fpp, POWER) end - set_v_wind_gnd(fpp.fpca, se().v_wind) + set_v_wind_gnd(fpp.fpca, v_wind) end # Check, if the new flight path planner is active. diff --git a/src/fpc_settings.jl b/src/fpc_settings.jl index 69b48ab2..7b59713f 100644 --- a/src/fpc_settings.jl +++ b/src/fpc_settings.jl @@ -54,4 +54,13 @@ function update(fcs::FPCSettings) dict = YAML.load_file(config_file) sec_dict = Dict(Symbol(k) => v for (k, v) in dict["fpc_settings"]) StructTypes.constructfrom!(fcs, sec_dict) +end + +function FPCSettings(update; dt) + fcs = FPCSettings(; dt) + if update + KiteControllers.update(fcs) + end + fcs.dt = dt + fcs end \ No newline at end of file diff --git a/src/fpp_settings.jl b/src/fpp_settings.jl index 4dd1f0e2..0a2109e7 100644 --- a/src/fpp_settings.jl +++ b/src/fpp_settings.jl @@ -42,4 +42,12 @@ function update(fpps::FPPSettings) dict = YAML.load_file(config_file) sec_dict = Dict(Symbol(k) => v for (k, v) in dict["fpp_settings"]) StructTypes.constructfrom!(fpps, sec_dict) +end + +function FPPSettings(update) + fpp = FPPSettings() + if update + KiteControllers.update(fpp) + end + fpp end \ No newline at end of file diff --git a/src/systemstatecontrol.jl b/src/systemstatecontrol.jl index 8ddff802..0b11b82d 100644 --- a/src/systemstatecontrol.jl +++ b/src/systemstatecontrol.jl @@ -10,13 +10,14 @@ sys_state::Union{Nothing, SysState} = nothing state::Observable(SystemState)[] = ssParking tether_length::Union{Nothing, Float64} = nothing + v_wind end -function SystemStateControl(wcs::WCSettings, fcs::FPCSettings, fpps::FPPSettings; u_d0, u_d) +function SystemStateControl(wcs::WCSettings, fcs::FPCSettings, fpps::FPPSettings; u_d0, u_d, v_wind) fpc = FlightPathController(fcs; u_d0, u_d) fpca = FlightPathCalculator(fpc, fpps) fpp = FlightPathPlanner(fpps, fpca) - res = SystemStateControl(wc=WinchController(wcs), fpp=fpp) + res = SystemStateControl(wc=WinchController(wcs), fpp=fpp, v_wind=v_wind) attractor = zeros(2) attractor[end] = deg2rad(80.0) # beta_set @@ -71,13 +72,17 @@ function calc_v_set(ssc::SystemStateControl) v_set end -function calc_steering(ssc::SystemStateControl, manual_steering = 0.0) +function calc_steering(ssc::SystemStateControl, manual_steering = 0.0; heading = nothing) if isnothing(ssc.sys_state) return 0.0 end phi = ssc.sys_state.azimuth beta = ssc.sys_state.elevation - psi = ssc.sys_state.heading + if isnothing(heading) + psi = ssc.sys_state.heading + else + psi = heading + end v_a = ssc.sys_state.v_app chi = ssc.sys_state.course u_d = ssc.sys_state.depower @@ -104,7 +109,7 @@ function switch(ssc::SystemStateControl, state) ssc.state = state # publish new system state if state == ssPowerProduction - start(ssc.fpp) + start(ssc.fpp, ssc.v_wind) end if state == ssParking on_new_system_state(ssc.fpp.fpca, state, true) diff --git a/src/utils.jl b/src/utils.jl index 8b9fe676..097df5ac 100644 --- a/src/utils.jl +++ b/src/utils.jl @@ -1,4 +1,4 @@ - """ +""" A collection of control functions for discrete control Functions: @@ -53,3 +53,14 @@ function merge_angles(alpha, beta, 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 \ No newline at end of file diff --git a/src/wc_settings.jl b/src/wc_settings.jl index cb2b991c..ee02489d 100644 --- a/src/wc_settings.jl +++ b/src/wc_settings.jl @@ -3,7 +3,7 @@ Settings of the WinchController """ @with_kw mutable struct WCSettings @deftype Float64 "timestep of the winch controller" - dt = 0.02 + dt "set to true for running the unit tests" test::Bool = false "factor for I and P of lower force controller" @@ -93,3 +93,12 @@ function update(wcs::WCSettings) sec_dict = Dict(Symbol(k) => v for (k, v) in dict["wc_settings"]) StructTypes.constructfrom!(wcs, sec_dict) end + +function WCSettings(update; dt) + wcs = WCSettings(; dt) + if update + KiteControllers.update(wcs) + end + wcs.dt = dt + wcs +end diff --git a/test/runtests.jl b/test/runtests.jl index c64d3071..84a88616 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -46,7 +46,7 @@ end end @testset "WinchController" begin - wcs = WCSettings() + wcs = WCSettings(dt=0.02) cvi = CalcVSetIn(wcs) force = wcs.f_low v_ro = calc_vro(wcs, force) @@ -133,7 +133,7 @@ end end @testset "SpeedController" begin - wcs = WCSettings() + wcs = WCSettings(dt=0.02) sc = SpeedController(wcs) @test sc.wcs.dt == 0.02 set_tracking(sc, 1.0) @@ -164,7 +164,7 @@ end end @testset "Winch" begin - wcs = WCSettings() + wcs = WCSettings(dt=0.02) set = se() @test set.winch_model == "AsyncMachine" w = Winch(wcs, set) @@ -181,7 +181,7 @@ end # using KiteControllers, Test function test_winch() - wcs = WCSettings() + wcs = WCSettings(dt=0.02) set = se() w = Winch(wcs, set) v_set = 4.0 @@ -194,7 +194,7 @@ function test_winch() end @testset "LowerForceController" begin - wcs = WCSettings() + wcs = WCSettings(dt=0.02) lfc = LowerForceController(wcs) @test lfc.wcs.dt == 0.02 KiteControllers._set(lfc) @@ -229,7 +229,7 @@ end end @testset "UpperForceController" begin - wcs = WCSettings() + wcs = WCSettings(dt=0.02) ufc = UpperForceController(wcs) @test ufc.wcs.dt == 0.02 KiteControllers._set(ufc) @@ -264,7 +264,7 @@ end end @testset "WinchController" begin - wcs = WCSettings() + wcs = WCSettings(dt=0.02) wc = WinchController(wcs) v_act = 1.0 force = 500.0 @@ -416,10 +416,10 @@ end end @testset "SystemStateControl" begin - wcs = WCSettings() + wcs = WCSettings(dt=0.05) fcs = FPCSettings(dt=0.05) fpps = FPPSettings() - ssc = SystemStateControl(wcs, fcs, fpps; u_d0=0.01 * se().depower_offset, u_d=0.01 * se().depower) + ssc = SystemStateControl(wcs, fcs, fpps; u_d0=0.01 * se().depower_offset, u_d=0.01 * se().depower, v_wind = se().v_wind) on_parking(ssc) @test ssc.state == ssParking on_autopilot(ssc) @@ -515,7 +515,7 @@ end KiteControllers.on_new_data(fpp, depower, length, heading, height, time) fpp.count = 50 KiteControllers.on_new_data(fpp, depower, length, heading, height, time) - KiteControllers.start(fpp) + KiteControllers.start(fpp, se().v_wind) KiteControllers._switch(fpp, POWER) @test fpp._state == POWER KiteControllers._switch(fpp, POWER) diff --git a/test/test_flightpathcalculator.jl b/test/test_flightpathcalculator.jl index 6eff9dcc..dcd02bae 100644 --- a/test/test_flightpathcalculator.jl +++ b/test/test_flightpathcalculator.jl @@ -11,12 +11,12 @@ set = deepcopy(load_settings("system.yaml")) kcu::KCU = KCU(set) kps4::KPS4 = KPS4(kcu) -wcs::WCSettings = WCSettings(); wcs.dt = 1/set.sample_freq +wcs::WCSettings = WCSettings(dt = 1/set.sample_freq) fcs::FPCSettings = FPCSettings(dt=wcs.dt) fpps::FPPSettings = FPPSettings() u_d0 = 0.01 * set.depower_offset u_d = 0.01 * set.depower -ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d) +ssc::SystemStateControl = SystemStateControl(wcs, fcs, fpps; u_d0, u_d, v_wind=set.v_wind) dt = wcs.dt fpca = ssc.fpp.fpca \ No newline at end of file diff --git a/test/test_flightpathcontroller3.jl b/test/test_flightpathcontroller3.jl index e8e0e62b..b927442c 100644 --- a/test/test_flightpathcontroller3.jl +++ b/test/test_flightpathcontroller3.jl @@ -19,7 +19,7 @@ PROJECT="system_8000.yaml" set = deepcopy(se(PROJECT)) kcu::KCU = KCU(set) kps4::KPS4 = KPS4(kcu) -wcs::WCSettings = WCSettings(); wcs.dt = 1/set.sample_freq +wcs::WCSettings = WCSettings(dt=0.02); wcs.dt = 1/set.sample_freq fcs::FPCSettings = FPCSettings(dt=wcs.dt) fpps::FPPSettings = FPPSettings() dt::Float64 = wcs.dt diff --git a/test/test_forcespeedcontroller1.jl b/test/test_forcespeedcontroller1.jl index 47d432fb..5c9cf869 100644 --- a/test/test_forcespeedcontroller1.jl +++ b/test/test_forcespeedcontroller1.jl @@ -12,7 +12,7 @@ using KiteControllers, KiteUtils, ControlPlots, BenchmarkTools set = deepcopy(load_settings("system.yaml")) -wcs = WCSettings() +wcs = WCSettings(dt=0.02) wcs.test = true wcs.f_low = 1500 wcs.fac = 1.0 diff --git a/test/test_forcespeedcontroller2.jl b/test/test_forcespeedcontroller2.jl index a43e2515..5bcb3617 100644 --- a/test/test_forcespeedcontroller2.jl +++ b/test/test_forcespeedcontroller2.jl @@ -12,7 +12,7 @@ using KiteControllers, ControlPlots, BenchmarkTools set = deepcopy(load_settings("system.yaml")) -wcs = WCSettings() +wcs = WCSettings(dt=0.02) wcs.test = true wcs.f_low = 350 wcs.fac = 1.0 diff --git a/test/test_fpc_low_right.jl b/test/test_fpc_low_right.jl index 0f503170..834fff96 100644 --- a/test/test_fpc_low_right.jl +++ b/test/test_fpc_low_right.jl @@ -21,9 +21,9 @@ PROJECT="system_8000.yaml" set = deepcopy(se(PROJECT)) kcu::KCU = KCU(set) kps4::KPS4 = KPS4(kcu) -wcs::WCSettings = WCSettings(); wcs.dt = 1/set.sample_freq -fcs::FPCSettings = FPCSettings(dt = wcs.dt) -fpps::FPPSettings = FPPSettings() +wcs::WCSettings = WCSettings(true, dt = 1/set.sample_freq) +fcs::FPCSettings = FPCSettings(true, dt = wcs.dt) +fpps::FPPSettings = FPPSettings(true) dt::Float64 = wcs.dt attractor=[55.73, 56.95] # attractor=[0, 90] # zenith diff --git a/test/test_lower_force1.jl b/test/test_lower_force1.jl index 68b30e08..5109713d 100644 --- a/test/test_lower_force1.jl +++ b/test/test_lower_force1.jl @@ -31,7 +31,7 @@ ACC = zeros(SAMPLES) D_F_ERR = zeros(SAMPLES) # create and initialize winch controller -wcs = WCSettings() +wcs = WCSettings(dt=0.02) update(wcs) wcs.dt = 0.05 lfc = LowerForceController(wcs) diff --git a/test/test_speedcontroller1.jl b/test/test_speedcontroller1.jl index 1075959d..8a755575 100644 --- a/test/test_speedcontroller1.jl +++ b/test/test_speedcontroller1.jl @@ -8,10 +8,10 @@ using Timers; tic() # Test the speed controller. # Input: A varying wind speed. Implements the simulink block diagram, shown in # docs/speed_controller_test1.png -using KiteControllers, ControlPlots, BenchmarkTools +using KiteControllers, ControlPlots, BenchmarkTools, KiteUtils set = deepcopy(load_settings("system.yaml")) -wcs = WCSettings() +wcs = WCSettings(dt=0.02) DURATION = 10.0 SAMPLES = Int(DURATION / wcs.dt + 1) diff --git a/test/test_speedcontroller2.jl b/test/test_speedcontroller2.jl index f28b2899..d526f46b 100644 --- a/test/test_speedcontroller2.jl +++ b/test/test_speedcontroller2.jl @@ -12,7 +12,7 @@ using Timers; tic() using KiteControllers, ControlPlots, BenchmarkTools set = deepcopy(load_settings("system.yaml")) -wcs = WCSettings() +wcs = WCSettings(dt=0.02) DURATION = 10.0 SAMPLES = Int(DURATION / wcs.dt + 1) diff --git a/test/test_winchcontroller.jl b/test/test_winchcontroller.jl index 16f902c0..b7e7010e 100644 --- a/test/test_winchcontroller.jl +++ b/test/test_winchcontroller.jl @@ -11,7 +11,7 @@ using Timers; tic() using KiteControllers, KiteUtils, ControlPlots, BenchmarkTools set = deepcopy(load_settings("system.yaml")) -wcs = WCSettings() +wcs = WCSettings(dt=0.02) wcs.test = true wcs.f_low = 350 wcs.fac = 1.0