diff --git a/.gitignore b/.gitignore index 67eeded6..0a269278 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,8 @@ *.xopp *.so *.so.bak +polars.csv +*.dat /Manifest.toml /Manifest.toml.1.7 bin/kps-image-1.8-initial.so diff --git a/Project.toml b/Project.toml index 21f48a13..0b369872 100644 --- a/Project.toml +++ b/Project.toml @@ -5,9 +5,9 @@ version = "0.6.6" [deps] AtmosphericModels = "c59cac55-771d-4f45-b14d-1c681463a295" -DataInterpolations = "82cc6244-b520-54b8-b5a6-8a565e85f1d0" Dierckx = "39dd38d3-220a-591b-8e3c-4c3a8c710a94" DiffEqBase = "2b5f629d-d688-5b77-993f-72d75c75574e" +Distributed = "8ba89e20-285c-5b6f-9357-94700520ee1b" DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" KitePodModels = "9de5dc81-f971-414a-927b-652b2f41c539" KiteUtils = "90980105-b163-44e5-ba9f-8b1c83bb0533" @@ -29,12 +29,12 @@ Sundials = "c3572dad-4567-51f8-b174-8c6c989267f4" SymbolicIndexingInterface = "2efcf032-c050-4f8e-a9bb-153293bab1f5" Timers = "21f18d07-b854-4dab-86f0-c15a3821819a" WinchModels = "7dcfa46b-7979-4771-bbf4-0aee0da42e1f" +Xfoil = "19641d66-a62d-11e8-2441-8f57a969a9c4" [compat] AtmosphericModels = "0.2" BenchmarkTools = "1.2, 1.3" ControlPlots = "0.2.0" -DataInterpolations = "6.2.0" Dierckx = "0.5" DiffEqBase = "6.152.2" DocStringExtensions = "0.8, 0.9" diff --git a/data/naca2412.dat b/data/naca2412.dat new file mode 100644 index 00000000..a1e27fbb --- /dev/null +++ b/data/naca2412.dat @@ -0,0 +1,35 @@ +1.0000 0.0013 +0.9500 0.0114 +0.9000 0.0208 +0.8000 0.0375 +0.7000 0.0518 +0.6000 0.0636 +0.5000 0.0724 +0.4000 0.0780 +0.3000 0.0788 +0.2500 0.0767 +0.2000 0.0726 +0.1500 0.0661 +0.1000 0.0563 +0.0750 0.0496 +0.0500 0.0413 +0.0250 0.0299 +0.0125 0.0215 +0.0000 0.0000 +0.0125 -0.0165 +0.0250 -0.0227 +0.0500 -0.0301 +0.0750 -0.0346 +0.1000 -0.0375 +0.1500 -0.0410 +0.2000 -0.0423 +0.2500 -0.0422 +0.3000 -0.0412 +0.4000 -0.0380 +0.5000 -0.0334 +0.6000 -0.0276 +0.7000 -0.0214 +0.8000 -0.0150 +0.9000 -0.0082 +0.9500 -0.0048 +1.0000 -0.0013 diff --git a/data/settings_3l.yaml b/data/settings_3l.yaml index fc8c875a..0d1bb621 100644 --- a/data/settings_3l.yaml +++ b/data/settings_3l.yaml @@ -11,7 +11,7 @@ system: initial: l_tether: 50.0 # initial tether length [m] - elevation: 70.8 # initial elevation angle [deg] + elevation: 85.0 # initial elevation angle [deg] v_reel_out: 0.0 # initial reel out speed [m/s] depower: 25.0 # initial depower settings [%] @@ -37,6 +37,8 @@ depower: kite: model: "data/kite.obj" # 3D model of the kite + foil_file: "data/MH82.dat" # filename for the foil shape + polar_file: "data/polars.csv" # filename for the polars physical_model: "KPS4_3L" # name of the kite model to use (KPS3 or KPS4) version: 2 # version of the model to use mass: 0.9 # kite mass [kg] @@ -63,6 +65,7 @@ kps4_3l: min_steering_line_distance: 1.0 width: 4.1 # width of the kite [m] aero_surfaces: 5 # number of aerodynamic surfaces in 3 line model + flap_height: 0.044 # height at the start of the flap of a normalized kite segment kcu: kcu_mass: 8.4 # mass of the kite control unit [kg] @@ -81,7 +84,7 @@ bridle: rel_damping: 6.0 # relative damping of the kite spring (relative to main tether) tether: - d_tether: 1 # tether diameter [mm] + d_tether: 2 # tether diameter [mm] cd_tether: 0.958 # drag coefficient of the tether damping: 473.0 # unit damping coefficient [Ns] c_spring: 614600.0 # unit spring constant coefficient [N] @@ -96,7 +99,7 @@ winch: v_ro_min: -8.0 # minimal reel-out speed (=max reel-in speed) [m/s] drum_radius: 0.110 # radius of the drum [m] gear_ratio: 1.0 # gear ratio of the winch [-] - inertia_total: 0.104 # total inertia, as seen from the motor/generator [kgm²] + inertia_total: 0.024 # total inertia, as seen from the motor/generator [kgm²] f_coulomb: 122.0 # coulomb friction [N] c_vf: 30.6 # coefficient for the viscous friction [Ns/m] diff --git a/examples/3l_kite_environment.jl b/examples/3l_kite_environment.jl index e6046bbd..cf30af51 100644 --- a/examples/3l_kite_environment.jl +++ b/examples/3l_kite_environment.jl @@ -1,7 +1,8 @@ module Environment -using KiteModels, StaticArrays, LinearAlgebra, OrdinaryDiffEqCore, OrdinaryDiffEqBDF, OrdinaryDiffEqSDIRK, Parameters -export reset, step, render +using KiteModels, StaticArrays, LinearAlgebra, Parameters +import OrdinaryDiffEqCore: ODEIntegrator +export reset, step, render, Env const StateVec = MVector{11, Float32} @@ -11,7 +12,7 @@ const StateVec = MVector{11, Float32} max_render_length::Int = 10000 i::Int = 1 logger::Logger = Logger(s.num_A, max_render_length) - integrator::OrdinaryDiffEq.ODEIntegrator = KiteModels.init_sim!(s; stiffness_factor=0.04, prn=false, mtk=true, torque_control=true) + integrator::ODEIntegrator = KiteModels.init_sim!(s; prn=false, torque_control=true) sys_state::SysState = SysState(s) state::StateVec = zeros(StateVec) state_d::StateVec = zeros(StateVec) @@ -25,27 +26,26 @@ const StateVec = MVector{11, Float32} rotation::Float64 = 0.0 end -const e = Env() - -function step(reel_out_torques; prn=false) - reel_out_torques = Vector{Float64}(reel_out_torques) .* 100 +function step(e::Env, reel_out_torques; prn=false) + reel_out_torques = Vector{Float64}(reel_out_torques) old_heading = calc_heading(e.s) if prn - KiteModels.next_step!(e.s, e.integrator; set_values=reel_out_torques, torque_control=false) + KiteModels.next_step!(e.s, e.integrator; set_values=reel_out_torques) else - redirect_stdout(devnull) do - KiteModels.next_step!(e.s, e.integrator; set_values=reel_out_torques, torque_control=true) + redirect_stderr(devnull) do + redirect_stdout(devnull) do + KiteModels.next_step!(e.s, e.integrator; set_values=reel_out_torques) + end end end - println(e.s.pos[end]) - _calc_rotation(old_heading, calc_heading(e.s)) + _calc_rotation(e, old_heading, calc_heading(e.s)) update_sys_state!(e.sys_state, e.s) e.i += 1 - return _calc_state(e.s) + return (e.integrator.last_stepfail, _calc_state(e, e.s)) end -function reset(name="sim_log", elevation=0.0, azimuth=0.0, tether_length=50.0, force=5000.0, log=false) +function reset(e::Env, name="sim_log", elevation=0.0, azimuth=0.0, tether_length=50.0, force=5000.0, log=false) e.wanted_elevation = Float32(elevation) e.wanted_azimuth = Float32(azimuth) e.wanted_tether_length = Float32(tether_length) @@ -56,24 +56,22 @@ function reset(name="sim_log", elevation=0.0, azimuth=0.0, tether_length=50.0, f save_log(e.logger, basename(name)) end update_settings() - @time e.logger = Logger(e.s.num_A, e.max_render_length) - println(e.integrator) - @time e.integrator = KiteModels.reset_sim!(e.s; stiffness_factor=0.04) + e.logger = Logger(e.s.num_A, e.max_render_length) + e.integrator = KiteModels.init_sim!(e.s; prn=false, torque_control=true) e.sys_state = SysState(e.s) e.i = 1 - return _calc_state(e.s) + return _calc_state(e, e.s) end -function render() +function render(e::Env) if(e.i <= e.max_render_length) log!(e.logger, e.sys_state) end end -function _calc_state(s::KPS4_3L) - _calc_reward(s) +function _calc_state(e::Env, s::KPS4_3L) e.state .= vcat( - _calc_reward(s), # length 1 + _calc_speed_reward(e,s), # length 1 calc_orient_quat(s), # length 4 s.tether_lengths, # length 3 # normalize to min and max @@ -96,27 +94,57 @@ function _calc_state(s::KPS4_3L) return vcat(e.state, e.state_d, e.state_dd) end -function _calc_reward(s::KPS4_3L) - if (KiteModels.calc_tether_elevation(s) < e.wanted_elevation || +function _calc_reward(e::Env, s::KPS4_3L) + if s.vel_kite ⋅ s.e_x < 0 || + (KiteModels.calc_tether_elevation(s) < e.wanted_elevation || !(-2*π < e.rotation < 2*π) || - s.tether_lengths[1] > e.wanted_tether_length*1.5 || - s.tether_lengths[1] < e.wanted_tether_length*0.95 || + s.tether_lengths[3] > e.wanted_tether_length*1.5 || + s.tether_lengths[3] < e.wanted_tether_length*0.95 || sum(winch_force(s)) > e.max_force) return 0.0 end - force_component = _calc_force_component(s) + force_component = _calc_force_component(e,s) reward = clamp(force_component / e.max_force, 0.0, 1.0) # range [-1, 1] clamped to [0, 1] because 0 is physical minimum return reward end -function _calc_force_component(s::KPS4_3L) +function _calc_speed_reward(e::Env, s::KPS4_3L) + speed = s.vel_kite ⋅ s.e_x + if (KiteModels.calc_tether_elevation(s) < e.wanted_elevation || + !(-2*π < e.rotation < 2*π) || + s.tether_lengths[3] > e.wanted_tether_length*1.5 || + s.tether_lengths[3] < e.wanted_tether_length*0.95 || + sum(winch_force(s)) > e.max_force) + return 0.0 + end + # wanted_minus_z = [cos(e.wanted_elevation)*cos(e.wanted_azimuth), cos(e.wanted_elevation)*-sin(e.wanted_azimuth), sin(e.wanted_elevation)] + # reward = speed * (-s.e_z ⋅ wanted_minus_z) + reward = speed + return reward +end + +function _calc_direction_reward(e::Env, s::KPS4_3L) + if s.vel_kite ⋅ s.e_x < 0.0 || + (KiteModels.calc_tether_elevation(s) < e.wanted_elevation || + !(-2*π < e.rotation < 2*π) || + s.tether_lengths[3] > e.wanted_tether_length*1.5 || + s.tether_lengths[3] < e.wanted_tether_length*0.95 || + sum(winch_force(s)) > e.max_force) + return 0.01 + end + wanted_direction = [cos(e.wanted_elevation)*cos(e.wanted_azimuth), cos(e.wanted_elevation)*-sin(e.wanted_azimuth), sin(e.wanted_elevation)] + reward = normalize(s.pos[6]) ⋅ wanted_direction + 1.0 + return reward +end + +function _calc_force_component(e::Env, s::KPS4_3L) wanted_force_vector = [cos(e.wanted_elevation)*cos(e.wanted_azimuth), cos(e.wanted_elevation)*-sin(e.wanted_azimuth), sin(e.wanted_elevation)] - tether_force = sum(s.forces[1:3]) + tether_force = sum(s.winch_forces) force_component = tether_force ⋅ wanted_force_vector return force_component end -function _calc_rotation(old_heading, new_heading) +function _calc_rotation(e::Env, old_heading, new_heading) d_rotation = new_heading - old_heading if d_rotation > 1 d_rotation -= 2*pi diff --git a/examples/simple_3l_speed_control.jl b/examples/simple_3l_speed_control.jl new file mode 100644 index 00000000..13a8ed2f --- /dev/null +++ b/examples/simple_3l_speed_control.jl @@ -0,0 +1,105 @@ +using KiteModels, OrdinaryDiffEqCore, OrdinaryDiffEqBDF, OrdinaryDiffEqSDIRK, LinearAlgebra, Timers, Statistics +using Base: summarysize +tic() + +using Pkg +if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies)) + using TestEnv; TestEnv.activate() +end +using ControlPlots + +set = deepcopy(load_settings("system_3l.yaml")) +# set.elevation = 71 +dt = 0.05 +total_time = 8.0 + +steps = Int(round(total_time / dt)) +logger = Logger(3*set.segments + 6, steps) + +if !@isdefined s; s = KPS4_3L(KCU(set)); end +s.set = update_settings() +s.set.abs_tol = 0.0006 +s.set.rel_tol = 0.001 +s.set.l_tether = 50.0 +s.set.damping = 473 +s.set.elevation = 85 +println("init sim") +@time KiteModels.init_sim!(s; prn=true, torque_control=false) +# @time next_step!(s; set_values=[0.0, 0.0, 0.0], dt=2.0) +println("vel ", mean(norm.(s.integrator[s.simple_sys.force]))) +sys_state = KiteModels.SysState(s) +if sys_state.heading > pi + sys_state.heading -= 2*pi +end + +println("stepping") +total_step_time = 0.0 +toc() +steering = [0.0, 0.0, 0.0] +amount = 2.0 +sign = 1 +for i in 1:steps + time = (i-1) * dt + @show time + # println("vel ", norm(s.integrator[s.simple_sys.vel])) + global total_step_time, sys_state, steering + # steering = [0.0,0.0,1000.0] # left right middle + sign = ifelse(mod(floor(time-0.5), 2) == 0, 1, -1) + steering[1] -= sign * dt * amount + steering[2] += sign * dt * amount + + if sys_state.heading > pi + sys_state.heading -= 2*pi + end + sys_state.var_01 = rad2deg(s.flap_angle[1]) - 10 + sys_state.var_02 = rad2deg(s.flap_angle[2]) - 10 + sys_state.var_03 = s.pos[s.num_C][1] + sys_state.var_04 = s.pos[s.num_D][1] + # sys_state.var_03 = s.tether_lengths[1] + # sys_state.var_04 = s.tether_lengths[2] + sys_state.var_05 = s.tether_lengths[3] + sys_state.var_06 = rad2deg(s.integrator[s.simple_sys.seg_flap_angle[div(s.set.aero_surfaces, 2)]] - s.integrator[s.simple_sys.aoa[div(s.set.aero_surfaces, 2)]]) + sys_state.var_07 = clamp(rad2deg(s.integrator[s.simple_sys.flap_vel[1]]), -20, 20) + sys_state.var_08 = norm(s.D_C) + sys_state.var_09 = norm(s.D_D) + sys_state.var_10 = (s.integrator[s.simple_sys.vel[:, s.num_E-3]]) ⋅ s.e_z + sys_state.var_11 = norm(s.integrator[s.simple_sys.vel[:, s.num_E-3]] .- (s.integrator[s.simple_sys.vel[:, s.num_E-3]]) ⋅ s.e_z) + # sys_state.var_09 = norm(s.D_C + s.D_D) + + # @show s.integrator[s.simple_sys.aoa[div(s.set.aero_surfaces, 2)]] + + step_time = @elapsed next_step!(s; set_values=steering, dt=dt) + if time > total_time/2 + total_step_time += step_time + end + + KiteModels.update_sys_state!(sys_state, s) + if sys_state.heading > pi + sys_state.heading -= 2*pi + end + log!(logger, sys_state) + l = s.set.l_tether+10 + plot2d(s.pos, time; zoom=true, front=false, xlim=(-l/2, l/2), ylim=(0, l)) +end + +times_reltime = (total_time/2) / total_step_time +println("times realtime MTK model: ", times_reltime) +# println("avg steptime MTK model: ", total_step_time/steps) + +p=plotx(logger.time_vec, + [logger.var_01_vec, logger.var_02_vec], + [logger.var_03_vec, logger.var_04_vec], + rad2deg.(logger.heading_vec), + [logger.var_06_vec, logger.var_07_vec], + [logger.var_08_vec, logger.var_09_vec], + [logger.var_10_vec, logger.var_11_vec]; + ylabels=["Steering", "Pos", "heading [deg]", "Angle / Force", "Force", "Vel"], + labels=[ + ["Steering Pos C", "Steering Pos D"], + ["X left", "X right"], + "heading", + ["Flap angle", "Flap vel"] , + ["Drag C", "Drag D"], + ["Vel par", "Vel perp"]], + fig="Steering and heading MTK model") +display(p) \ No newline at end of file diff --git a/examples/simple_3l_torque_control.jl b/examples/simple_3l_torque_control.jl deleted file mode 100644 index 12107a40..00000000 --- a/examples/simple_3l_torque_control.jl +++ /dev/null @@ -1,85 +0,0 @@ -using KiteModels, OrdinaryDiffEqCore, OrdinaryDiffEqBDF, OrdinaryDiffEqSDIRK, LinearAlgebra, Timers -using Base: summarysize -tic() - -using Pkg -if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies)) - using TestEnv; TestEnv.activate() -end -using ControlPlots - -set = deepcopy(load_settings("system_3l.yaml")) -# set.elevation = 71 -steps = 50 -dt = 1/set.sample_freq -tspan = (0.0, dt) - -logger = Logger(3*set.segments + 6, steps) - -steering = [5,5,-30.0] - -if !@isdefined mtk_kite; mtk_kite = KPS4_3L(KCU(set)); end -mtk_kite.set.abs_tol = 0.0006 -mtk_kite.set.rel_tol = 0.001 -mtk_kite.set.l_tether = 50.1 -println("init sim") -@time mtk_integrator = KiteModels.init_sim!(mtk_kite; prn=true, torque_control=true) -println("acc ", norm(mtk_integrator[mtk_kite.simple_sys.acc])) - -println("compiling") -total_new_time = 0.0 -for i in 1:5 - global total_new_time += @elapsed next_step!(mtk_kite, mtk_integrator; set_values=steering) -end -sys_state = KiteModels.SysState(mtk_kite) -if sys_state.heading > pi - sys_state.heading -= 2*pi -end -log!(logger, sys_state) - -println("stepping") -total_old_time = 0.0 -total_new_time = 0.0 -toc() -for i in 1:steps - global total_new_time, sys_state, steering - if i == 1 - steering = [5,5,-26.0] # left right middle - end - if i == 20 - steering = [0,10,-33] - end - if i == 40 - steering = [0,0,-20] - end - if i == 60 - steering = [0,0,-30] - end - - if sys_state.heading > pi - sys_state.heading -= 2*pi - end - sys_state.var_01 = mtk_kite.steering_pos[1] - sys_state.var_02 = mtk_kite.steering_pos[2] - sys_state.var_03 = mtk_kite.reel_out_speeds[1] - sys_state.var_04 = mtk_kite.reel_out_speeds[2] - - total_new_time += @elapsed next_step!(mtk_kite, mtk_integrator; set_values=steering) - - KiteModels.update_sys_state!(sys_state, mtk_kite) - if sys_state.heading > pi - sys_state.heading -= 2*pi - end - log!(logger, sys_state) -end - -new_time = (dt*steps) / total_new_time -println("times realtime MTK model: ", new_time) -println("avg steptime MTK model: ", total_new_time/steps) - -p=plotx(logger.time_vec, [logger.var_01_vec, logger.var_02_vec], [logger.var_03_vec, logger.var_04_vec], - rad2deg.(logger.heading_vec); - ylabels=["Steering", "Reelout speed", "Heading [deg]"], - labels=[["Steering Pos 1", "Steering Pos 2"], ["v_ro 1", "v_ro 2"], "Heading"], - fig="Steering and Heading MTK model") -display(p) diff --git a/scripts/create_polars.jl b/scripts/create_polars.jl new file mode 100644 index 00000000..37f53997 --- /dev/null +++ b/scripts/create_polars.jl @@ -0,0 +1,243 @@ +using Distributed, Timers +using Xfoil +using Pkg +if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies)) + using TestEnv; TestEnv.activate() +end +# using ControlPlots +using KiteUtils +tic() +const procs = addprocs() +se::Settings = KiteUtils.se("system_3l.yaml") + +function normalize!(x, y) + x_min = minimum(x) + x_max = maximum(x) + for i in eachindex(x) + x[i] = (x[i] - x_min) / (x_max - x_min) + y[i] = (y[i] - x_min) / (x_max - x_min) + end +end + +@everywhere begin + using Xfoil, Statistics + + function turn_flap!(angle, x, y, lower_turn, upper_turn) + theta = deg2rad(angle) + x_turn = 0.75 + turn_distance = upper_turn - lower_turn + smooth_idx = [] + rm_idx = [] + + sign = theta > 0 ? 1 : -1 + y_turn = theta > 0 ? upper_turn : lower_turn + for i in eachindex(x) + if x_turn - turn_distance < x[i] < x_turn + turn_distance && sign * y[i] > 0 + append!(smooth_idx, i) + elseif sign * y[i] < 0 && x_turn > x[i] > x_turn - turn_distance + append!(rm_idx, i) + end + if x[i] > x_turn + x_rel = x[i] - x_turn + y_rel = y[i] - y_turn + x[i] = x_turn + x_rel * cos(theta) + y_rel * sin(theta) + y[i] = y_turn - x_rel * sin(theta) + y_rel * cos(theta) + if theta > 0 && x[i] < x_turn - turn_distance/2 && y[i] > lower_turn + append!(rm_idx, i) + elseif theta < 0 && x[i] < x_turn - turn_distance/2 && y[i] < upper_turn + append!(rm_idx, i) + end + end + end + + #TODO: lower and upper is slightly off because of smoothing + lower_i, upper_i = minimum(smooth_idx), maximum(smooth_idx) + for i in smooth_idx + window = min(i - lower_i + 1, upper_i - i + 1) + x[i] = mean(x[i-window:i+window]) + end + deleteat!(x, rm_idx) + deleteat!(y, rm_idx) + nothing + end + + function calculate_constants(d_flap_angle_, x_, y_, cp, lower, upper) + d_flap_angle = deg2rad(d_flap_angle_) + x = deepcopy(x_) + y = deepcopy(y_) + c_te = 0.0 + if d_flap_angle > 0 + x_ref, y_ref = 0.75, upper + else + x_ref, y_ref = 0.75, lower + end + + # straighten out the flap in order to find the trailing edge torque constant + for i in eachindex(x) + x_rel = x[i] - x_ref + y_rel = y[i] - y_ref + x[i] = x_ref + x_rel * cos(-d_flap_angle) + y_rel * sin(-d_flap_angle) + y[i] = y_ref - x_rel * sin(-d_flap_angle) + y_rel * cos(-d_flap_angle) + end + + x2 = [] + y2 = [] + for i in 1:(length(x)-1) + if x[i] > x_ref && lower < y[i] < upper + push!(x2, x[i]) + push!(y2, y[i]) + dx = x[i+1] - x[i] + cp_avg = (cp[i] + cp[i+1]) / 2 + c_te -= dx * cp_avg * (x[i] - x_ref) / (1 - x_ref) # clockwise flap force at trailing edge + end + end + return c_te + end + + function solve_alpha(alphas, d_flap_angle, re, x_, y_, lower, upper, kite_speed, speed_of_sound) + polars = [] + x = deepcopy(x_) + y = deepcopy(y_) + turn_flap!(d_flap_angle, x, y, lower, upper) + Xfoil.set_coordinates(x, y) + x, y = Xfoil.pane(npan=140) + times_not_converged = 0 + @show d_flap_angle + reinit = true + for alpha in alphas + converged = false + cl = 0.0 + cd = 0.0 + # Solve for the given angle of attack + cl, cd, _, _, converged = Xfoil.solve_alpha(alpha, re; iter=50, reinit=reinit, mach=kite_speed/speed_of_sound, ncrit=5) + reinit = false + times_not_converged += !converged + if times_not_converged > 20 + break + end + if converged + times_not_converged = 0 + _, cp = Xfoil.cpdump() + c_te = calculate_constants(d_flap_angle, x, y, cp, lower, upper) + push!(polars, (alpha, d_flap_angle, cl, cd, c_te)) + end + end + return polars + end + + function run_solve_alpha(alphas, d_flap_angle, re, x_, y_, lower, upper, kite_speed, speed_of_sound) + neg_alphas = sort(alphas[findall(alphas .< 0.0)], rev = true) + pos_alphas = sort(alphas[findall(alphas .>= 0.0)]) + polars = solve_alpha(neg_alphas, d_flap_angle, re, x_, y_, lower, upper, kite_speed, speed_of_sound) + polars = append!(polars, solve_alpha(pos_alphas, d_flap_angle, re, x_, y_, lower, upper, kite_speed, speed_of_sound)) + return polars + end +end + +function get_lower_upper(x, y) + lower_flap = 0.0 + upper_flap = 0.0 + min_lower_distance = Inf + min_upper_distance = Inf + for (xi, yi) in zip(x, y) + if yi < 0 + lower_distance = abs(xi - 0.75) + if lower_distance < min_lower_distance + min_lower_distance = lower_distance + lower_flap = yi + end + else + upper_distance = abs(xi - 0.75) + if upper_distance < min_upper_distance + min_upper_distance = upper_distance + upper_flap = yi + end + end + end + return lower_flap, upper_flap +end + +function create_polars(foil_file=se.foil_file, polar_file=se.polar_file) + println("Creating polars") + if !endswith(polar_file, ".csv") + polar_file *= ".csv" + end + if !endswith(foil_file, ".dat") + foil_file *= ".dat" + end + polar_file = joinpath(get_data_path(), polar_file) + foil_file = joinpath(get_data_path(), foil_file) + + alphas = -180:0.5:180 + d_flap_angles = -90:0.5:90 + + kite_speed = se.v_wind + speed_of_sound = 343 + reynolds_number = kite_speed * (se.middle_length + se.tip_length)/2 / 1.460e-5 + + # Read airfoil coordinates from a file. + x, y = open(foil_file, "r") do f + x = Float64[] + y = Float64[] + for line in eachline(f) + entries = split(chomp(line)) + try + push!(x, parse(Float64, entries[1])) + push!(y, parse(Float64, entries[2])) + catch ArgumentError + end + end + x, y + end + normalize!(x, y) + Xfoil.set_coordinates(x, y) + x, y = Xfoil.pane(npan=140) + lower, upper = get_lower_upper(x, y) + + polars = nothing + try + polars = @distributed (append!) for d_flap_angle in d_flap_angles + return run_solve_alpha(alphas, d_flap_angle, reynolds_number, x, y, lower, upper, kite_speed, speed_of_sound) + end + finally + println("removing processes") + rmprocs(procs) + end + + println("Alpha\t\tFlap Angle\tCl\t\tCd\t\tc_te") + for (alpha, d_flap_angle, cl, cd, c_te) in polars + println("$alpha\t$d_flap_angle\t$(cl)\t$(cd)\t$(c_te)") + end + + println("Relative flap height: ", upper - lower) + println("Reynolds number for flying speed of $kite_speed is $reynolds_number") + + csv_content = "alpha,d_flap_angle,cl,cd,c_te\n" + for (alpha, d_flap_angle, cl, cd, c_te) in polars + csv_content *= string( + alpha, ",", + d_flap_angle, ",", + cl, ",", + cd, ",", + c_te, "\n" + ) + end + open(polar_file, "w") do f + write(f, csv_content) + end + open(foil_file, "r+") do f + lines = readlines(f) + if any(isletter, chomp(lines[1])) + lines[1] *= " - polars created" + else + pushfirst!(lines, "polars created") + end + seek(f, 0) + for line in lines + println(f, line) + end + end +end + +create_polars() +toc() \ No newline at end of file diff --git a/scripts/load_polars.jl b/scripts/load_polars.jl new file mode 100644 index 00000000..3ff2e3db --- /dev/null +++ b/scripts/load_polars.jl @@ -0,0 +1,83 @@ +using Dierckx, Statistics +using Pkg +if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies)) + using TestEnv; TestEnv.activate() +end +using ControlPlots, KiteUtils + +# Load the csv file +function read_csv(filename) + data = Dict{String, Vector{Float64}}() + open(filename, "r") do f + header = split(chomp(readline(f)), ",") + for col in header + data[col] = Float64[] + end + for line in eachline(f) + values = split(chomp(line), ",") + for (i, col) in enumerate(header) + push!(data[col], parse(Float64, values[i])) + end + end + end + return data +end + +polars = read_csv("data/polars.csv") +alphas = deg2rad.(polars["alpha"]) +d_flap_angles = deg2rad.(polars["d_flap_angle"]) +cl_values = polars["cl"] +cd_values = polars["cd"] +c_te_values = polars["c_te"] + +rm_idx = [] +dist = 0.02 +for i in 2:length(alphas)-1 + if d_flap_angles[i-1] == d_flap_angles[i+1] && abs(cd_values[i-1] - cd_values[i]) > dist && abs(cd_values[i+1] - cd_values[i]) > dist + push!(rm_idx, i) + end +end +deleteat!(alphas, rm_idx) +deleteat!(d_flap_angles, rm_idx) +deleteat!(cl_values, rm_idx) +deleteat!(cd_values, rm_idx) +deleteat!(c_te_values, rm_idx) + +wd = 2.0 .- abs.(cd_values ./ argmax(abs, cd_values)) +order = 2 +println("1") +cl_spline = Spline2D(alphas, d_flap_angles, cl_values; kx=order, ky=order, s=20.0) +println("2") +cd_spline = Spline2D(alphas, d_flap_angles, cd_values; w=wd, kx=order, ky=order, s=10.0) +println("3") +c_te_spline = Spline2D(alphas, d_flap_angles, c_te_values; kx=order, ky=order, s=1.0) + +function plot_values(spline, values, name) + fig = plt.figure() + ax = fig.add_subplot(projection="3d") + # alpha = 0.6396476065600847 + plot_alphas = 0.0:0.04:deg2rad(60) + plot_flap_angles = -1.5:0.1:deg2rad(60) + + idx = reduce(vcat, [findall(x -> abs(x - alpha) < deg2rad(0.3), alphas) for alpha in plot_alphas]) + + spl_values = reduce(vcat, [reduce(vcat, [spline(alpha, flap_angle) for flap_angle in plot_flap_angles]) for alpha in plot_alphas]) + extended_plot_alphas = reduce(vcat, [reduce(vcat, [alpha for _ in plot_flap_angles]) for alpha in plot_alphas]) + extended_plot_flap_angles = reduce(vcat, [reduce(vcat, [flap_angle for flap_angle in plot_flap_angles]) for _ in plot_alphas]) + + ax.scatter(d_flap_angles[idx], alphas[idx], values[idx]) + ax.scatter(extended_plot_flap_angles, extended_plot_alphas, spl_values) + plt.xlabel("Flap angle") + plt.ylabel("Alpha") + plt.zlabel("$name values") + plt.title("$name for different d_flap and angle") + plt.legend() + plt.grid(true) + plt.show() +end + +plot_values(cd_spline, cd_values, "Cd") +plot_values(cl_spline, cl_values, "Cl") +plot_values(c_te_spline, c_te_values, "C_te") + +cd_spline(0,0)*π \ No newline at end of file diff --git a/src/KPS4_3L.jl b/src/KPS4_3L.jl index 4135ec74..95404ce0 100644 --- a/src/KPS4_3L.jl +++ b/src/KPS4_3L.jl @@ -43,7 +43,7 @@ const SPRINGS_INPUT_3L = [1. 4. -1. # s1: E, A # E = 1, C = 2, D = 3, A = 4 # E = segments*3+1, C = segments*3+2, D = segments*3+3, A = segments*3+4 - +# TODO: add line multiplier: multiple lines doing same thing const KITE_SPRINGS_3L = 6 const KITE_PARTICLES_3L = 4 @@ -75,12 +75,20 @@ $(TYPEDFIELDS) last_set_hash::UInt64 = 0 "Reference to the atmospheric model as implemented in the package AtmosphericModels" am::AtmosphericModel = AtmosphericModel() - "Function for calculation the lift coefficent, using a spline based on the provided value pairs." - calc_cl = Spline1D(se().alpha_cl, se().cl_list) - "Function for calculation the drag coefficent, using a spline based on the provided value pairs." - calc_cd = Spline1D(se().alpha_cd, se().cd_list) + "Function for calculating the lift coefficent, using a spline based on the provided value pairs." + cl_spline::Union{Spline2D, Nothing} = nothing + "Function for calculating the drag coefficent, using a spline based on the provided value pairs." + cd_spline::Union{Spline2D, Nothing} = nothing + "Function for calculating the trailing edge force coefficient, using a spline based on the provided value pairs." + c_te_spline::Union{Spline2D, Nothing} = nothing + "Min and max cl value in polars" + cl_bounds::MVector{2, S} = zeros(MVector{2, S}) + "Min and max cd value in polars" + cd_bounds::MVector{2, S} = zeros(MVector{2, S}) + "Min and max c_te value in polars" + c_te_bounds::MVector{2, S} = zeros(MVector{2, S}) "Reference to the motor models as implemented in the package WinchModels. index 1: middle motor, index 2: left motor, index 3: right motor" - motors::SVector{3, AbstractWinchModel} + motors::SizedArray{Tuple{3}, AbstractWinchModel} "Iterations, number of calls to the function residual!" iter:: Int64 = 0 "wind vector at the height of the kite" @@ -101,6 +109,7 @@ $(TYPEDFIELDS) pos::SVector{P, T} = zeros(SVector{P, T}) vel::SVector{P, T} = zeros(SVector{P, T}) veld::SVector{P, T} = zeros(SVector{P, T}) + flap_acc::MVector{2, S} = zeros(MVector{2, S}) "velocity vector of the kite" vel_kite::T = zeros(S, 3) "unstressed segment lengths of the three tethers [m]" @@ -114,15 +123,19 @@ $(TYPEDFIELDS) "unstretched tether length" tether_lengths::T = zeros(S, 3) "lengths of the connections of the steering tethers to the kite" - steering_pos::MVector{2, S} = zeros(S, 2) + flap_angle::MVector{2, S} = zeros(S, 2) "air density at the height of the kite" rho::S = 0.0 - "multiplier for the stiffniss of tether and bridle" - stiffness_factor::S = 1.0 + "multiplier for the damping of all movement" + damping_coeff::S = 0.0 "current masses, depending on the total tether length" masses::MVector{P, S} = zeros(P) "vector of the springs, defined as struct" springs::MVector{Q, SP} = zeros(SP, Q) + "unit spring coefficient" + c_spring::S = zero(S) + "unit damping coefficient" + damping::S = zero(S) "vector of the forces, acting on the particles" forces::SVector{P, T} = zeros(SVector{P, T}) "synchronous speed or torque of the motor/ generator" @@ -135,6 +148,10 @@ $(TYPEDFIELDS) e_y::T = zeros(S, 3) "z vector of kite reference frame" e_z::T = zeros(S, 3) + "Point number of C flap connection point" + num_flap_C::Int64 = 0 + "Point number of D flap connection point" + num_flap_D::Int64 = 0 "Point number of E" num_E::Int64 = 0 "Point number of C" @@ -143,10 +160,14 @@ $(TYPEDFIELDS) num_D::Int64 = 0 "Point number of A" num_A::Int64 = 0 - "Angle of right tip" - α_l::SimFloat = 0.0 "Angle of left tip" + α_l::SimFloat = 0.0 + "Angle of right tip" α_r::SimFloat = 0.0 + "Angle of point C" + α_C::SimFloat = 0.0 + "Kite length at point C" + kite_length_C::SimFloat = 0.0 "Lift of point C" L_C::T = zeros(S, 3) "Lift of point D" @@ -162,12 +183,11 @@ $(TYPEDFIELDS) set_values_idx::Union{ModelingToolkit.ParameterIndex, Nothing} = nothing v_wind_gnd_idx::Union{ModelingToolkit.ParameterIndex, Nothing} = nothing - stiffness_factor_idx::Union{ModelingToolkit.ParameterIndex, Nothing} = nothing v_wind_idx::Union{ModelingToolkit.ParameterIndex, Nothing} = nothing prob::Union{OrdinaryDiffEqCore.ODEProblem, Nothing} = nothing get_pos::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing - get_steering_pos::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing - get_line_acc::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing + get_flap_angle::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing + get_flap_acc::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing get_kite_vel::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing get_winch_forces::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing get_tether_lengths::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing @@ -176,6 +196,8 @@ $(TYPEDFIELDS) get_L_D::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing get_D_C::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing get_D_D::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing + integrator::Union{Sundials.CVODEIntegrator, Nothing} = nothing + u0:: Vector{Float64} = [0.0] end """ @@ -201,13 +223,15 @@ function clear!(s::KPS4_3L) s.e_y .= 0.0 s.e_z .= 0.0 s.set_values .= 0.0 - s.steering_pos .= 0.0 + s.flap_angle .= 0.0 s.vel_kite .= 0.0 [s.winch_forces[i] .= 0.0 for i in 1:3] s.tether_lengths .= [s.set.l_tether for _ in 1:3] s.α_l = π/2 - s.set.min_steering_line_distance/(2*s.set.radius) s.α_r = π/2 + s.set.min_steering_line_distance/(2*s.set.radius) s.segment_lengths .= s.tether_lengths ./ s.set.segments + s.num_flap_C = s.set.segments*3+3-2 + s.num_flap_D = s.set.segments*3+3-1 s.num_E = s.set.segments*3+3 s.num_C = s.set.segments*3+3+1 s.num_D = s.set.segments*3+3+2 @@ -217,12 +241,13 @@ function clear!(s::KPS4_3L) s.veld[i] .= 0.0 end s.rho = s.set.rho_0 + s.c_spring = s.set.e_tether * (s.set.d_tether/2000.0)^2 * pi + s.damping = (s.set.damping / s.set.c_spring) * s.c_spring init_masses!(s) init_springs!(s) - s.calc_cl = Spline1D(s.set.alpha_cl, s.set.cl_list) - s.calc_cd = Spline1D(s.set.alpha_cd, s.set.cd_list) end +# include(joinpath(@__DIR__, "CreatePolars.jl")) function KPS4_3L(kcu::KCU) set = kcu.set if set.winch_model == "TorqueControlledMachine" @@ -230,11 +255,43 @@ function KPS4_3L(kcu::KCU) else s = KPS4_3L{SimFloat, KVec3, set.segments*3+2+KITE_PARTICLES_3L, set.segments*3+KITE_SPRINGS_3L, SP}(set=kcu.set, motors=[AsyncMachine(set) for _ in 1:3]) end - s.num_E = s.set.segments*3+3 - s.num_C = s.set.segments*3+3+1 - s.num_D = s.set.segments*3+3+2 - s.num_A = s.set.segments*3+3+3 + open(joinpath(dirname(get_data_path()), s.set.foil_file), "r") do f + lines = readlines(f) + if !endswith(chomp(lines[1]), "polars created") + error("No polars created for $(s.set.foil_file). Run scripts/create_polars.jl to create a polars file.") + end + end clear!(s) + + polars = read_csv(s.set.polar_file) + alphas = deg2rad.(polars["alpha"]) + d_flap_angles = deg2rad.(polars["d_flap_angle"]) + cl_values = polars["cl"] + cd_values = polars["cd"] + c_te_values = polars["c_te"] + + rm_idx = [] + dist = 0.02 + for i in 2:length(alphas)-1 + if d_flap_angles[i-1] == d_flap_angles[i+1] && abs(cd_values[i-1] - cd_values[i]) > dist && abs(cd_values[i+1] - cd_values[i]) > dist + push!(rm_idx, i) + end + end + deleteat!(alphas, rm_idx) + deleteat!(d_flap_angles, rm_idx) + deleteat!(cl_values, rm_idx) + deleteat!(cd_values, rm_idx) + deleteat!(c_te_values, rm_idx) + + wd = 2.0 .- abs.(cd_values ./ argmax(abs, cd_values)) + order = 2 + s.cl_spline = Spline2D(alphas, d_flap_angles, cl_values; kx=order, ky=order, s=20.0) + s.cd_spline = Spline2D(alphas, d_flap_angles, cd_values; w=wd, kx=order, ky=order, s=10.0) + s.c_te_spline = Spline2D(alphas, d_flap_angles, c_te_values; kx=order, ky=order, s=1.0) + + s.cl_bounds = (minimum(cl_values), maximum(cl_values)) + s.cd_bounds = (minimum(cd_values), maximum(cd_values)) + s.c_te_bounds = (minimum(c_te_values), maximum(c_te_values)) return s end @@ -272,8 +329,8 @@ function update_sys_state!(ss::SysState, s::KPS4_3L, zoom=1.0) ss.v_app = norm(s.v_apparent) ss.l_tether = s.tether_lengths[3] ss.v_reelout = s.reel_out_speeds[3] - ss.depower = 100 - ((s.steering_pos[1] + s.steering_pos[2])/2) / ((s.set.middle_length + s.set.tip_length)/2) * 100 - ss.steering = (s.steering_pos[2] - s.steering_pos[1]) / ((s.set.middle_length + s.set.tip_length)/2) * 100 + ss.depower = rad2deg(s.flap_angle[1] + s.flap_angle[2]) + ss.steering = rad2deg(s.flap_angle[2] - s.flap_angle[1]) ss.vel_kite .= s.vel_kite nothing end @@ -298,8 +355,8 @@ function SysState(s::KPS4_3L, zoom=1.0) course = calc_course(s) v_app_norm = norm(s.v_apparent) t_sim = 0 - depower = 100 - ((s.steering_pos[1] + s.steering_pos[2])/2) / ((s.set.middle_length + s.set.tip_length)/2) * 100 - steering = (s.steering_pos[1] - s.steering_pos[2]) / ((s.set.middle_length + s.set.tip_length)/2) * 100 + depower = rad2deg(s.flap_angle[1] + s.flap_angle[2]) + steering = rad2deg(s.flap_angle[2] - s.flap_angle[1]) KiteUtils.SysState{P}(s.t_0, t_sim, 0, 0, orient, elevation, azimuth, s.tether_lengths[3], s.reel_out_speeds[3], forces[3], depower, steering, heading, course, v_app_norm, s.vel_kite, X, Y, Z, 0, 0, 0, 0, @@ -307,95 +364,117 @@ function SysState(s::KPS4_3L, zoom=1.0) end +function calc_heading(s::KPS4_3L) + # turn s.e_x by -azimuth around global z-axis and then by elevation around global y-axis + vec = rotate_in_xz(rotate_in_yx(s.e_x, -calc_azimuth(s)), -calc_elevation(s)) + heading = atan(-vec[2], vec[3]) + return heading +end + +function calc_heading_stable(s::KPS4_3L) + +end + + """ - init_sim!(s; t_end=1.0, stiffness_factor=1.0, prn=false) + init_sim!(s; damping_coeff=1.0, prn=false, torque_control=true) Initialises the integrator of the model. Parameters: -- s: an instance of an abstract kite model -- t_end: end time of the simulation; normally not needed -- stiffness_factor: factor applied to the tether stiffness during initialisation +- s: an instance of a 3 line kite model +- damping_coeff: amount of damping in the first steps - prn: if set to true, print the detailed solver results -- steady_state_history: an instance of SteadyStateHistory containing old pairs of AKM objects and integrators +- torque_control: wether or not to use torque control +- ss: an instance of a sys state Returns: An instance of a DAE integrator. """ -function init_sim!(s::KPS4_3L; t_end=1.0, stiffness_factor=1.0, prn=false, - torque_control=true) +function init_sim!(s::KPS4_3L; damping_coeff=100.0, prn=false, + torque_control=true) # TODO: add sysstate init ability clear!(s) change_control_mode = s.torque_control != torque_control - s.stiffness_factor = stiffness_factor s.torque_control = torque_control - dt = 1/s.set.sample_freq + if s.torque_control + [s.motors[i] = TorqueControlledMachine(s.set) for i in 1:3] + else + [s.motors[i] = AsyncMachine(s.set) for i in 1:3] + end + + dt = 1/s.set.sample_freq*2 tspan = (0.0, dt) - solver = KenCarp4(autodiff=false) # TRBDF2, Rodas4P, Rodas5P, Kvaerno5, KenCarp4, radau, QNDF + # solver = KenCarp4(autodiff=false) # TRBDF2, Rodas4P, Rodas5P, Kvaerno5, KenCarp4, radau, QNDF + solver = CVODE_BDF() # 2 times faster + s.damping_coeff = damping_coeff new_inital_conditions = (s.last_init_elevation != s.set.elevation || s.last_init_tether_length != s.set.l_tether) s.set_hash = settings_hash(s.set) - steady_tol = 1e-6 - if isnothing(s.prob) || change_control_mode || s.last_set_hash != s.set_hash - if prn; println("initializing with new model and new steady state"); end + init_new_model = isnothing(s.prob) || change_control_mode || s.last_set_hash != s.set_hash + init_new_pos = new_inital_conditions && !isnothing(s.set_values_idx) + + if init_new_model + if prn; println("initializing with new model and new pos"); end pos = init_pos(s) - model!(s, pos; torque_control=s.torque_control) + model!(s, pos) s.prob = ODEProblem(s.simple_sys, nothing, tspan) - steady_prob = SteadyStateProblem(s.prob) - s.steady_sol = solve(steady_prob, DynamicSS(solver; tspan=tspan), abstol=steady_tol, reltol=steady_tol) - s.prob = remake(s.prob; u0=s.steady_sol.u) - elseif new_inital_conditions - if prn; println("initializing with last model and new steady state"); end + s.integrator = OrdinaryDiffEqCore.init(s.prob, solver; dt, abstol=s.set.abs_tol, reltol=s.set.rel_tol, save_on=false, dtmin=1e-7) + next_step!(s; set_values=zeros(3), dt=2.0) # step 2 sec to get stable state + s.u0 = deepcopy(s.integrator.u) + OrdinaryDiffEqCore.reinit!(s.integrator, s.u0) + elseif init_new_pos + if prn; println("initializing with last model and new pos"); end pos = init_pos(s) - s.prob = ODEProblem(s.simple_sys, [s.simple_sys.pos => pos, s.simple_sys.tether_length => s.tether_lengths], tspan) - steady_prob = SteadyStateProblem(s.prob) - s.steady_sol = solve(steady_prob, DynamicSS(solver; tspan=tspan), abstol=steady_tol, reltol=steady_tol) - s.prob = remake(s.prob; u0=s.steady_sol.u) + s.prob = ODEProblem( + s.simple_sys, + [s.simple_sys.pos => pos, s.simple_sys.tether_length => s.tether_lengths, s.simple_sys.damping_coeff => s.damping_coeff], + tspan, [s.simple_sys.damping_coeff => s.damping_coeff] + ) + OrdinaryDiffEqCore.reinit!(s.integrator, s.prob.u0) + # next_step!(s; set_values=zeros(3), dt=2.0) # step 2 sec to get stable state + s.u0 = deepcopy(s.integrator.u) + # OrdinaryDiffEqCore.reinit!(s.integrator, s.u0) else - if prn; println("initializing with last model and last steady state"); end + if prn; println("initializing with last model and last pos"); end + OrdinaryDiffEqCore.reinit!(s.integrator, s.u0) end + s.last_init_elevation = s.set.elevation - s.last_init_tether_length = s.set.l_tether + s.last_init_tether_length = s.set.l_tether s.last_set_hash = s.set_hash - integrator = OrdinaryDiffEqCore.init(s.prob, solver; dt, abstol=s.set.abs_tol, reltol=s.set.rel_tol, save_on=false) - if isnothing(s.set_values_idx) - s.set_values_idx = parameter_index(integrator.f, :set_values) - s.v_wind_gnd_idx = parameter_index(integrator.f, :v_wind_gnd) - s.v_wind_idx = parameter_index(integrator.f, :v_wind) - s.stiffness_factor_idx = parameter_index(integrator.f, :stiffness_factor) - s.get_pos = getu(integrator.sol, s.simple_sys.pos[:,:]) - s.get_steering_pos = getu(integrator.sol, s.simple_sys.steering_pos) - s.get_line_acc = getu(integrator.sol, s.simple_sys.acc[:,s.num_E-2]) - s.get_kite_vel = getu(integrator.sol, s.simple_sys.vel[:,s.num_A]) - s.get_winch_forces = getu(integrator.sol, s.simple_sys.force[:,1:3]) - s.get_L_C = getu(integrator.sol, s.simple_sys.L_C) - s.get_L_D = getu(integrator.sol, s.simple_sys.L_D) - s.get_D_C = getu(integrator.sol, s.simple_sys.D_C) - s.get_D_D = getu(integrator.sol, s.simple_sys.D_D) - s.get_tether_lengths = getu(integrator.sol, s.simple_sys.tether_length) - s.get_tether_speeds = getu(integrator.sol, s.simple_sys.tether_speed) - end - update_pos!(s, integrator) - return integrator + update_pos!(s) + return nothing end -function next_step!(s::KPS4_3L, integrator; set_values=zeros(KVec3), v_wind_gnd=s.set.v_wind, wind_dir=0.0, dt=1/s.set.sample_freq) + +function next_step!(s::KPS4_3L; set_values=zeros(KVec3), v_wind_gnd=s.set.v_wind, wind_dir=0.0, dt=1/s.set.sample_freq) s.iter = 0 set_v_wind_ground!(s, calc_height(s), v_wind_gnd, wind_dir) - s.set_values .= set_values - integrator.ps[s.set_values_idx] .= s.set_values - integrator.ps[s.v_wind_gnd_idx] .= s.v_wind_gnd - integrator.ps[s.v_wind_idx] .= s.v_wind - integrator.ps[s.stiffness_factor_idx] = s.stiffness_factor - s.t_0 = integrator.t - OrdinaryDiffEqCore.step!(integrator, dt, true) - update_pos!(s, integrator) - if s.stiffness_factor < 1.0 - s.stiffness_factor+=0.01 - if s.stiffness_factor > 1.0 - s.stiffness_factor = 1.0 - end + if isnothing(s.set_values_idx) + s.set_values_idx = parameter_index(s.integrator.f, :set_values) + s.v_wind_gnd_idx = parameter_index(s.integrator.f, :v_wind_gnd) + s.v_wind_idx = parameter_index(s.integrator.f, :v_wind) + s.get_pos = getu(s.integrator.sol, s.simple_sys.pos[:,:]) + s.get_flap_angle = getu(s.integrator.sol, s.simple_sys.flap_angle) + s.get_flap_acc = getu(s.integrator.sol, s.simple_sys.flap_acc) + s.get_kite_vel = getu(s.integrator.sol, s.simple_sys.vel[:,s.num_A]) + s.get_winch_forces = getu(s.integrator.sol, s.simple_sys.force[:,1:3]) + s.get_L_C = getu(s.integrator.sol, s.simple_sys.L_C) + s.get_L_D = getu(s.integrator.sol, s.simple_sys.L_D) + s.get_D_C = getu(s.integrator.sol, s.simple_sys.D_C) + s.get_D_D = getu(s.integrator.sol, s.simple_sys.D_D) + s.get_tether_lengths = getu(s.integrator.sol, s.simple_sys.tether_length) + s.get_tether_speeds = getu(s.integrator.sol, s.simple_sys.tether_speed) end - integrator.t + s.set_values .= set_values + s.integrator.p[s.set_values_idx] .= s.set_values + s.integrator.p[s.v_wind_gnd_idx] .= s.v_wind_gnd + s.integrator.p[s.v_wind_idx] .= s.v_wind + s.t_0 = s.integrator.t + OrdinaryDiffEqCore.step!(s.integrator, dt, true) + @assert s.integrator.sol.retcode == ReturnCode.Success + update_pos!(s) + s.integrator.t end function calc_pre_tension(s::KPS4_3L) @@ -405,7 +484,7 @@ function calc_pre_tension(s::KPS4_3L) avg_force += forces[i] end avg_force /= s.num_A - res = avg_force/s.set.c_spring + res = avg_force/s.c_spring if res < 0.0 res = 0.0 end if isnan(res) res = 0.0 end return res + 1.0 @@ -425,7 +504,7 @@ Calculate and return the real, stretched tether lenght. """ function tether_length(s::KPS4_3L) length = 0.0 - for i in 3:3:s.num_E-3 + for i in 3:3:s.num_flap_C-1 length += norm(s.pos[i+3] - s.pos[i]) end return length @@ -440,7 +519,7 @@ end Determine the height of the topmost kite particle above ground. """ -function calc_height(s::KPS4_3L)isnothing(s.prob) || +function calc_height(s::KPS4_3L) pos_kite(s)[3] end @@ -474,16 +553,21 @@ function winch_force(s::KPS4_3L) norm.(s.winch_forces) end # Implementation of the three-line model using ModellingToolkit.jl -# issue: still uses settings getter function -function calc_acc_speed(tether_speed::SimFloat, norm_::SimFloat, set_speed::SimFloat) - calc_acceleration(AsyncMachine(se("system_3l.yaml")), tether_speed, norm_; set_speed, set_torque=nothing, use_brake=false) +function calc_acc_speed(motor::AsyncMachine, tether_speed, norm_, set_speed) + calc_acceleration(motor, tether_speed, norm_; set_speed, set_torque=nothing, use_brake=true) +end +@register_symbolic calc_acc_speed(motor::AsyncMachine, tether_speed, norm_, set_speed) + +function calc_acc_torque(motor::TorqueControlledMachine, tether_speed, norm_, set_torque) + calc_acceleration(motor, tether_speed, norm_; set_speed=nothing, set_torque, use_brake=false) end -@register_symbolic calc_acc_speed(tether_speed, norm_, set_speed) +@register_symbolic calc_acc_torque(motor::TorqueControlledMachine, tether_speed, norm_, set_torque) -function calc_acc_torque(tether_speed::SimFloat, norm_::SimFloat, set_torque::SimFloat) - calc_acceleration(TorqueControlledMachine(se("system_3l.yaml")), tether_speed, norm_; set_speed=nothing, set_torque, use_brake=false) +function sym_spline(spline::Spline2D, aoa, flap_angle) + return spline(aoa, flap_angle-aoa) end -@register_symbolic calc_acc_torque(tether_speed, norm_, set_torque) +@register_symbolic sym_spline(spline::Spline2D, aoa, flap_angle) + """ calc_aero_forces!(s::KPS4_3L, eqs2, force_eqs, force, pos, vel, t, e_x, e_y, e_z, rho) @@ -497,10 +581,9 @@ Parameters: Updates the vector s.forces of the first parameter. """ -function calc_aero_forces_mtk!(s::KPS4_3L, eqs2, force_eqs, force, pos, vel, t, e_x, e_y, e_z, rho, v_wind, steering_pos) +function calc_aero_forces_mtk!(s::KPS4_3L, eqs2, force_eqs, force, pos, vel, t, e_x, e_y, e_z, E_C, rho, v_wind, flap_angle) n = s.set.aero_surfaces @variables begin - E_c(t)[1:3] v_cx(t)[1:3] v_dx(t)[1:3] v_dy(t)[1:3] @@ -513,8 +596,6 @@ function calc_aero_forces_mtk!(s::KPS4_3L, eqs2, force_eqs, force, pos, vel, t, eqs2 = [ eqs2 - # in the aero calculations, E_c is the center of the circle shape on which the kite lies - E_c ~ pos[:, s.num_E] + e_z * (-s.set.bridle_center_distance + s.set.radius) v_cx ~ (vel[:, s.num_C] ⋅ e_x) * e_x v_dx ~ (vel[:, s.num_D] ⋅ e_x) * e_x v_dy ~ (vel[:, s.num_D] ⋅ e_y) * e_y @@ -525,31 +606,39 @@ function calc_aero_forces_mtk!(s::KPS4_3L, eqs2, force_eqs, force, pos, vel, t, y_ld ~ -norm(pos[:, s.num_D] - 0.5 * (pos[:, s.num_C] + pos[:, s.num_D])) ] - # integrating loop variables + # integrating loop variables, iterating over 2n segments @variables begin F(t)[1:3, 1:2n] e_r(t)[1:3, 1:2n] + e_te(t)[1:3, 1:2n] # clockwise trailing edge of flap vector y_l(t)[1:2n] v_kite(t)[1:3, 1:2n] v_a(t)[1:3, 1:2n] e_drift(t)[1:3, 1:2n] v_a_xr(t)[1:3, 1:2n] aoa(t)[1:n*2] - dL_dα(t)[1:3, 1:2n] - dD_dα(t)[1:3, 1:2n] + cl_seg(t)[1:n*2] + cd_seg(t)[1:n*2] + L_seg(t)[1:3, 1:2n] + D_seg(t)[1:3, 1:2n] + F_te_seg(t)[1:3, 1:2n] + seg_flap_angle(t)[1:2n] # flap angle relative to -e_x, e_z + ram_force(t)[1:2n] + te_force(t)[1:2n] L_C(t)[1:3] L_D(t)[1:3] D_C(t)[1:3] D_D(t)[1:3] - F_steering_c(t)[1:3] - F_steering_d(t)[1:3] - d(t)[1:2n] + F_te_C(t)[1:3] # trailing edge clockwise force + F_te_D(t)[1:3] end l_c_eq = SizedArray{Tuple{3}, Symbolics.Equation}(collect(L_C .~ 0)) l_d_eq = SizedArray{Tuple{3}, Symbolics.Equation}(collect(L_D .~ 0)) d_c_eq = SizedArray{Tuple{3}, Symbolics.Equation}(collect(D_C .~ 0)) d_d_eq = SizedArray{Tuple{3}, Symbolics.Equation}(collect(D_D .~ 0)) - kite_length = zeros(MVector{2n, SimFloat}) + f_te_c_eq = SizedArray{Tuple{3}, Symbolics.Equation}(collect(F_te_C .~ 0)) + f_te_d_eq = SizedArray{Tuple{3}, Symbolics.Equation}(collect(F_te_D .~ 0)) + kite_length = zero(SimFloat) α = zero(SimFloat) α_0 = zero(SimFloat) α_middle = zero(SimFloat) @@ -558,52 +647,66 @@ function calc_aero_forces_mtk!(s::KPS4_3L, eqs2, force_eqs, force, pos, vel, t, α_0 = π/2 - s.set.width/2/s.set.radius α_middle = π/2 dα = (α_middle - α_0) / n + ram_range = 1.0 # TODO: do experiment to find out what value is right here for i in 1:n*2 if i <= n α = α_0 + -dα/2 + i * dα + kite_length = s.set.tip_length + (s.set.middle_length-s.set.tip_length) * (α - α_0) / (π/2 - α_0) # TODO: kite length gets less with flap turning else α = pi - (α_0 + -dα/2 + (i-n) * dα) + kite_length = s.set.tip_length + (s.set.middle_length-s.set.tip_length) * (π - α_0 - α) / (π/2 - α_0) end - + seg_flap_height = kite_length * s.set.flap_height eqs2 = [ eqs2 - F[:, i] ~ E_c + e_y * cos(α) * s.set.radius - e_z * sin(α) * s.set.radius - e_r[:, i] ~ (E_c - F[:, i]) / norm(E_c - F[:, i]) + F[:, i] ~ E_C + e_y * cos(α) * s.set.radius - e_z * sin(α) * s.set.radius + e_r[:, i] ~ (E_C - F[:, i]) / norm(E_C - F[:, i]) y_l[i] ~ cos(α) * s.set.radius α < π/2 ? v_kite[:, i] ~ ((v_cx - v_dx) / (y_lc - y_ld) * (y_l[i] - y_ld) + v_dx) + v_cy + v_cz : v_kite[:, i] ~ ((v_cx - v_dx) / (y_lc - y_ld) * (y_l[i] - y_ld) + v_dx) + v_dy + v_dz - v_a[:,i] ~ v_wind .- v_kite[:,i] + v_a[:, i] ~ v_wind .- v_kite[:, i] e_drift[:, i] ~ (e_r[:, i] × e_x) v_a_xr[:, i] ~ v_a[:, i] .- (v_a[:, i] ⋅ e_drift[:, i]) .* e_drift[:, i] - ] - if α < π/2 - kite_length[i] = (s.set.tip_length + (s.set.middle_length-s.set.tip_length) * α - * s.set.radius/(0.5*s.set.width)) - else - kite_length[i] = (s.set.tip_length + (s.set.middle_length-s.set.tip_length) * (π-α) - * s.set.radius/(0.5*s.set.width)) - end - eqs2 = [ - eqs2 + α < s.α_l ? - d[i] ~ steering_pos[1] : + seg_flap_angle[i] ~ flap_angle[1] : α > s.α_r ? - d[i] ~ steering_pos[2] : - d[i] ~ (steering_pos[2] - steering_pos[1]) / (s.α_r - s.α_l) * (α - s.α_l) + (steering_pos[1]) - aoa[i] ~ -asin((v_a_xr[:, i] / norm(v_a_xr[:, i])) ⋅ e_r[:, i]) + - asin(clamp(d[i] / kite_length[i], -1.0, 1.0)) - dL_dα[:, i] ~ 0.5 * rho * (norm(v_a_xr[:, i]))^2 * s.set.radius * kite_length[i] * rad_cl_mtk(aoa[i]) * + seg_flap_angle[i] ~ flap_angle[2] : + seg_flap_angle[i] ~ ((flap_angle[2] - flap_angle[1]) / (s.α_r - s.α_l) * (α - s.α_l) + (flap_angle[1])) + + aoa[i] ~ -asin((v_a_xr[:, i] / norm(v_a_xr[:, i])) ⋅ e_r[:, i]) + deg2rad(s.set.alpha_zero) + cl_seg[i] ~ clamp(sym_spline(s.cl_spline, aoa[i], seg_flap_angle[i]), s.cl_bounds[1], s.cl_bounds[2]) + cd_seg[i] ~ clamp(sym_spline(s.cd_spline, aoa[i], seg_flap_angle[i]), s.cd_bounds[1], s.cd_bounds[2]) + + L_seg[:, i] ~ 0.5 * rho * (norm(v_a_xr[:, i]))^2 * s.set.radius * dα * kite_length * cl_seg[i] * ((v_a_xr[:, i] × e_drift[:, i]) / norm(v_a_xr[:, i] × e_drift[:, i])) - dD_dα[:, i] ~ 0.5 * rho * norm(v_a_xr[:, i]) * s.set.radius * kite_length[i] * rad_cd_mtk(aoa[i]) * - v_a_xr[:,i] # the sideways drag cannot be calculated with the C_d formula + D_seg[:, i] ~ 0.5 * rho * norm(v_a_xr[:, i]) * s.set.radius * dα * kite_length * cd_seg[i] * + v_a_xr[:, i] + + + e_te[:, i] ~ e_x * sin(seg_flap_angle[i]) + e_r[:, i] * cos(seg_flap_angle[i]) + ram_force[i] ~ ifelse( + seg_flap_angle[i] > aoa[i], + -rho * norm(v_a[:, i])^2 * seg_flap_height * s.set.radius * dα * (seg_flap_height/2) / (kite_length/4) * + min((seg_flap_angle[i] - aoa[i])/deg2rad(ram_range), 1.0), + rho * norm(v_a[:, i])^2 * seg_flap_height * s.set.radius * dα * (seg_flap_height/2) / (kite_length/4) * + min((aoa[i] - seg_flap_angle[i])/deg2rad(ram_range), 1.0), + ) + te_force[i] ~ 0.5 * rho * (norm(v_a_xr[:, i]))^2 * s.set.radius * dα * kite_length * + clamp(sym_spline(s.c_te_spline, aoa[i], seg_flap_angle[i]), s.c_te_bounds[1], s.c_te_bounds[2]) + F_te_seg[:, i] ~ (ram_force[i] + te_force[i]) * e_te[:, i] ] + + # TODO: correct for extra torque in wingtips (add to c substract from d) if i <= n - [l_c_eq[j] = (L_C[j] ~ l_c_eq[j].rhs + dL_dα[j, i] * dα) for j in 1:3] - [d_c_eq[j] = (D_C[j] ~ d_c_eq[j].rhs + dD_dα[j, i] * dα) for j in 1:3] + [l_c_eq[j] = (L_C[j] ~ l_c_eq[j].rhs + L_seg[j, i]) for j in 1:3] + [d_c_eq[j] = (D_C[j] ~ d_c_eq[j].rhs + D_seg[j, i]) for j in 1:3] + [f_te_c_eq[j] = (F_te_C[j] ~ f_te_c_eq[j].rhs + F_te_seg[j, i]) for j in 1:3] else - [l_d_eq[j] = (L_D[j] ~ l_d_eq[j].rhs + dL_dα[j, i] * dα) for j in 1:3] - [d_d_eq[j] = (D_D[j] ~ d_d_eq[j].rhs + dD_dα[j, i] * dα) for j in 1:3] + [l_d_eq[j] = (L_D[j] ~ l_d_eq[j].rhs + L_seg[j, i]) for j in 1:3] + [d_d_eq[j] = (D_D[j] ~ d_d_eq[j].rhs + D_seg[j, i]) for j in 1:3] + [f_te_d_eq[j] = (F_te_D[j] ~ f_te_d_eq[j].rhs + F_te_seg[j, i]) for j in 1:3] end end @@ -613,14 +716,24 @@ function calc_aero_forces_mtk!(s::KPS4_3L, eqs2, force_eqs, force, pos, vel, t, d_c_eq l_d_eq d_d_eq - F_steering_c ~ ((0.2 * (L_C ⋅ -e_z)) * -e_z) - F_steering_d ~ ((0.2 * (L_D ⋅ -e_z)) * -e_z) + f_te_c_eq + f_te_d_eq ] + + # longtitudinal force + # F_inside_flap = P * A + # F_inside_flap = rho * norm(v)^2 * flap_height * width + # F_inside_flap = rho * norm(v)^2 * flap_height * radius * dα + # F_trailing_edge = -F_inside_flap * (flap_height/2) / (kite_length/4) if flap_angle > 0 clockwise force + # F_trailing_edge = F_inside_flap * (flap_height/2) / (kite_length/4) if flap_angle < 0 clockwise force + # dF_te_dα = rho * norm(v)^2 * flap_height * radius + # flap_height = height_middle * kite_length / middle_length - force_eqs[:,s.num_C] .= (force[:,s.num_C] .~ (L_C + D_C) - F_steering_c) - force_eqs[:,s.num_D] .= (force[:,s.num_D] .~ (L_D + D_D) - F_steering_d) - force_eqs[:,s.num_E-2] .= (force[:,s.num_E-2] .~ F_steering_c) - force_eqs[:,s.num_E-1] .= (force[:,s.num_E-1] .~ F_steering_d) + # TODO: check if the right forces are added + force_eqs[:,s.num_C] .= (force[:,s.num_C] .~ L_C + D_C) + force_eqs[:,s.num_D] .= (force[:,s.num_D] .~ L_D + D_D) + force_eqs[:,s.num_flap_C] .= (force[:,s.num_flap_C] .~ F_te_C) + force_eqs[:,s.num_flap_D] .= (force[:,s.num_flap_D] .~ F_te_D) return eqs2, force_eqs end @@ -634,27 +747,29 @@ and distribute it equally on the two particles, that are attached to the segment The result is stored in the array s.forces. """ function calc_particle_forces_mtk!(s::KPS4_3L, eqs2, force_eqs, force, pos1, pos2, vel1, vel2, length, c_spring, - damping, rho, i, l_0, k, c, segment, rel_vel, av_vel, norm1, unit_vector, k1, k2, c1, spring_vel, - spring_force, v_apparent, v_wind_tether, area, v_app_perp, half_drag_force, stiffness_factor) + damping, rho, i, l_0, k, c, segment, rel_vel, av_vel, norm1, unit_vector, k1, k2, c1, c2, spring_vel, perp_vel, + spring_force, v_apparent, v_wind_tether, area, v_app_perp, half_drag_force) d_tether = s.set.d_tether/1000.0 eqs2 = [ eqs2 i <= s.set.segments*3 ? l_0 ~ length[(i-1) % 3 + 1] : l_0 ~ s.springs[i].length # Unstressed length - i <= s.set.segments*3 ? k ~ c_spring[(i-1) % 3 + 1] * stiffness_factor : - k ~ s.springs[i].c_spring * stiffness_factor # Spring constant + i <= s.set.segments*3 ? k ~ c_spring[(i-1) % 3 + 1] : + k ~ s.springs[i].c_spring # Spring constant i <= s.set.segments*3 ? c ~ damping[(i-1) % 3 + 1] : c ~ s.springs[i].damping # Damping coefficient - segment .~ pos1 - pos2 + segment .~ pos1 - pos2 # TODO: all segments have same length and tension rel_vel .~ vel1 - vel2 av_vel .~ 0.5 * (vel1 + vel2) norm1 ~ norm(segment) unit_vector .~ segment / norm1 - k1 ~ 0.25 * k # compression stiffness kite segments + k1 ~ 1.0 * k # compression stiffness kite segments k2 ~ 0.1 * k # compression stiffness tether segments c1 ~ 6.0 * c # damping kite segments - spring_vel .~ rel_vel ⋅ unit_vector + c2 ~ 0.05 * c # damping perpendicular + spring_vel ~ rel_vel ⋅ unit_vector + perp_vel .~ rel_vel .- spring_vel * unit_vector ] - if i >= s.num_E-2 # kite springs + if i >= Base.length(s.springs) - KITE_SPRINGS_3L + 1 # kite springs for j in 1:3 eqs2 = [ eqs2 @@ -671,8 +786,8 @@ function calc_particle_forces_mtk!(s::KPS4_3L, eqs2, force_eqs, force, pos1, pos eqs2 spring_force[j] ~ ifelse( (norm1 - l_0) > 0.0, - (k * (l_0 - norm1) - c * spring_vel) * unit_vector[j], - (k2 * (l_0 - norm1) - c * spring_vel) * unit_vector[j] + (k * (l_0 - norm1) - c * spring_vel) * unit_vector[j] - c2 * perp_vel[j], + (k2 * (l_0 - norm1) - c * spring_vel) * unit_vector[j] - c2 * perp_vel[j] ) ] end @@ -680,16 +795,18 @@ function calc_particle_forces_mtk!(s::KPS4_3L, eqs2, force_eqs, force, pos1, pos eqs2 = [ eqs2 v_apparent ~ v_wind_tether - av_vel - area ~ norm1 * d_tether - v_app_perp ~ v_apparent - v_apparent ⋅ unit_vector * unit_vector + i >= s.num_flap_C ? + area ~ norm1 * d_tether * 10 : + area ~ norm1 * d_tether + v_app_perp ~ v_apparent - (v_apparent ⋅ unit_vector) * unit_vector half_drag_force .~ (0.25 * rho * s.set.cd_tether * norm(v_app_perp) * area) .* v_app_perp ] for j in 1:3 force_eqs[j, s.springs[i].p1] = - (force[j,s.springs[i].p1] ~ force_eqs[j, s.springs[i].p1].rhs + (half_drag_force[j] + spring_force[j])) + (force[j, s.springs[i].p1] ~ force_eqs[j, s.springs[i].p1].rhs + (half_drag_force[j] + spring_force[j])) force_eqs[j, s.springs[i].p2] = - (force[j,s.springs[i].p2] ~ force_eqs[j, s.springs[i].p2].rhs + (half_drag_force[j] - spring_force[j])) + (force[j, s.springs[i].p2] ~ force_eqs[j, s.springs[i].p2].rhs + (half_drag_force[j] - spring_force[j])) end return eqs2, force_eqs @@ -706,7 +823,7 @@ Output:length - s.forces - s.v_wind_tether """ -@inline function inner_loop_mtk!(s::KPS4_3L, eqs2, force_eqs, t, force, pos, vel, length, c_spring, damping, v_wind_gnd, stiffness_factor) +@inline function inner_loop_mtk!(s::KPS4_3L, eqs2, force_eqs, t, force, pos, vel, length, c_spring, damping, v_wind_gnd) @variables begin height(t)[eachindex(s.springs)] rho(t)[eachindex(s.springs)] @@ -715,7 +832,7 @@ Output:length l_0(t)[eachindex(s.springs)] k(t)[eachindex(s.springs)] c(t)[eachindex(s.springs)] - segment(t)[1:3,eachindex(s.springs)] + segment(t)[1:3, eachindex(s.springs)] rel_vel(t)[1:3, eachindex(s.springs)] av_vel(t)[1:3, eachindex(s.springs)] norm1(t)[eachindex(s.springs)] @@ -723,7 +840,9 @@ Output:length k1(t)[eachindex(s.springs)] k2(t)[eachindex(s.springs)] c1(t)[eachindex(s.springs)] + c2(t)[eachindex(s.springs)] spring_vel(t)[eachindex(s.springs)] + perp_vel(t)[1:3, eachindex(s.springs)] spring_force(t)[1:3, eachindex(s.springs)] v_apparent(t)[1:3, eachindex(s.springs)] area(t)[eachindex(s.springs)] @@ -741,60 +860,64 @@ Output:length v_wind_tether[:, i] ~ calc_wind_factor(s.am, height[i]) * v_wind_gnd ] - # TODO: @assert height > 0 eqs2, force_eqs = calc_particle_forces_mtk!(s, eqs2, force_eqs, force, pos[:, p1], pos[:, p2], vel[:, p1], vel[:, p2], length, c_spring, damping, rho[i], i, l_0[i], k[i], c[i], segment[:, i], - rel_vel[:, i], av_vel[:, i], norm1[i], unit_vector[:, i], k1[i], k2[i], c1[i], spring_vel[i], - spring_force[:, i], v_apparent[:,i], v_wind_tether[:, i], area[i], v_app_perp[:, i], - half_drag_force[:, i], stiffness_factor) + rel_vel[:, i], av_vel[:, i], norm1[i], unit_vector[:, i], k1[i], k2[i], c1[i], c2[i], spring_vel[i], + perp_vel[:, i], spring_force[:, i], v_apparent[:, i], v_wind_tether[:, i], area[i], v_app_perp[:, i], + half_drag_force[:, i]) end return eqs2, force_eqs end -function update_pos!(s, integrator) - pos = s.get_pos(integrator) - s.steering_pos .= s.get_steering_pos(integrator) - [s.pos[i] .= pos[:,i] for i in 1:s.num_A] - s.veld[s.num_E-2] .= s.get_line_acc(integrator) - s.vel_kite .= s.get_kite_vel(integrator) - winch_forces = s.get_winch_forces(integrator) - [s.winch_forces[i] .= (winch_forces[:,i]) for i in 1:3] - s.tether_lengths .= s.get_tether_lengths(integrator) - s.reel_out_speeds .= s.get_tether_speeds(integrator) - s.L_C = s.get_L_C(integrator) - s.L_D = s.get_L_D(integrator) - s.D_C = s.get_D_C(integrator) - s.D_D = s.get_D_D(integrator) +function update_pos!(s) + pos = s.get_pos(s.integrator) + s.flap_angle .= s.get_flap_angle(s.integrator) + s.flap_acc .= s.get_flap_acc(s.integrator) + [s.pos[i] .= pos[:, i] for i in 1:s.num_A] + s.vel_kite .= s.get_kite_vel(s.integrator) + winch_forces = s.get_winch_forces(s.integrator) + [s.winch_forces[i] .= (winch_forces[:, i]) for i in 1:3] + s.tether_lengths .= s.get_tether_lengths(s.integrator) + s.reel_out_speeds .= s.get_tether_speeds(s.integrator) + s.L_C = s.get_L_C(s.integrator) + s.L_D = s.get_L_D(s.integrator) + s.D_C = s.get_D_C(s.integrator) + s.D_D = s.get_D_D(s.integrator) calc_kite_ref_frame!(s, s.pos[s.num_E], s.pos[s.num_C], s.pos[s.num_D]) - @assert all(abs.(s.steering_pos) .<= s.set.tip_length) + # @assert all(abs.(s.flap_angle) .<= deg2rad(70)) nothing end -function model!(s::KPS4_3L, pos_; torque_control=false) +function model!(s::KPS4_3L, pos_) @parameters begin set_values[1:3] = s.set_values v_wind_gnd[1:3] = s.v_wind_gnd v_wind[1:3] = s.v_wind - stiffness_factor = s.stiffness_factor end @variables begin pos(t)[1:3, 1:s.num_A] = pos_ # left right middle vel(t)[1:3, 1:s.num_A] = zeros(3, s.num_A) # left right middle acc(t)[1:3, 1:s.num_A] = zeros(3, s.num_A) # left right middle + damping_coeff(t) = s.damping_coeff tether_length(t)[1:3] = s.tether_lengths - steering_pos(t)[1:2] = s.steering_pos - steering_vel(t)[1:2] = zeros(2) - steering_acc(t)[1:2] = zeros(2) + flap_angle(t)[1:2] = zeros(2) # angle + flap_vel(t)[1:2] = zeros(2) # angular vel + flap_acc(t)[1:2] = zeros(2) # angular acc tether_speed(t)[1:3] = zeros(3) # left right middle segment_length(t)[1:3] = zeros(3) # left right middle mass_tether_particle(t)[1:3] # left right middle - damping(t)[1:3] = s.set.damping ./ s.tether_lengths ./ s.set.segments # left right middle - c_spring(t)[1:3] = s.set.c_spring ./ s.tether_lengths ./ s.set.segments # left right middle + damping(t)[1:3] = s.damping ./ (s.tether_lengths ./ s.set.segments) # left right middle + c_spring(t)[1:3] = s.c_spring ./ (s.tether_lengths ./ s.set.segments) # left right middle P_c(t)[1:3] = 0.5 .* (s.pos[s.num_C] + s.pos[s.num_D]) + E_C(t)[1:3] e_x(t)[1:3] e_y(t)[1:3] e_z(t)[1:3] + e_r_C(t)[1:3] + e_r_D(t)[1:3] + e_te_C(t)[1:3] + e_te_D(t)[1:3] force(t)[1:3, 1:s.num_A] = zeros(3, s.num_A) # left right middle rho_kite(t) = 0.0 end @@ -807,20 +930,20 @@ function model!(s::KPS4_3L, pos_; torque_control=false) mass_per_meter = s.set.rho_tether * π * (s.set.d_tether/2000.0)^2 [eqs1 = vcat(eqs1, D.(pos[:, i]) .~ 0.0) for i in 1:3] - [eqs1 = vcat(eqs1, D.(pos[:, i]) .~ vel[:,i]) for i in 4:s.num_E-3] - eqs1 = [eqs1; D.(steering_pos) .~ steering_vel] - [eqs1 = vcat(eqs1, D.(pos[:, i]) .~ vel[:,i]) for i in s.num_E:s.num_A] + [eqs1 = vcat(eqs1, D.(pos[:, i]) .~ vel[:, i]) for i in 4:s.num_flap_C-1] + eqs1 = [eqs1; D(flap_angle) ~ flap_vel] + [eqs1 = vcat(eqs1, D.(pos[:, i]) .~ vel[:, i]) for i in s.num_E:s.num_A] [eqs1 = vcat(eqs1, D.(vel[:, i]) .~ 0.0) for i in 1:3] - [eqs1 = vcat(eqs1, D.(vel[:, i]) .~ acc[:,i]) for i in 4:s.num_E-3] - eqs1 = [eqs1; D.(steering_vel) .~ steering_acc] - [eqs1 = vcat(eqs1, D.(vel[:, i]) .~ acc[:,i]) for i in s.num_E:s.num_A] + [eqs1 = vcat(eqs1, D.(vel[:, i]) .~ acc[:, i]) for i in 4:s.num_flap_C-1] + eqs1 = [eqs1; D(flap_vel) ~ flap_acc] + [eqs1 = vcat(eqs1, D.(vel[:, i]) .~ acc[:, i]) for i in s.num_E:s.num_A] eqs1 = vcat(eqs1, D.(tether_length) .~ tether_speed) - if torque_control - eqs1 = vcat(eqs1, D.(tether_speed) .~ [calc_acc_torque(tether_speed[i], norm(force[:, (i-1) % 3 + 1]), + if s.torque_control + eqs1 = vcat(eqs1, D.(tether_speed) .~ [calc_acc_torque(s.motors[i], tether_speed[i], norm(force[:, (i-1) % 3 + 1]), set_values[i]) for i in 1:3]) else - eqs1 = vcat(eqs1, D.(tether_speed) .~ [calc_acc_speed(tether_speed[i], norm(force[:,(i-1) % 3 + 1]), + eqs1 = vcat(eqs1, D.(tether_speed) .~ [calc_acc_speed(s.motors[i], tether_speed[i], norm(force[:,(i-1) % 3 + 1]), set_values[i]) for i in 1:3]) end @@ -828,57 +951,75 @@ function model!(s::KPS4_3L, pos_; torque_control=false) force_eqs = SizedArray{Tuple{3, s.num_A}, Symbolics.Equation}(undef) force_eqs[:, :] .= (force[:, :] .~ 0) + flap_length = s.kite_length_C/4 eqs2 = [ - pos[:, s.num_E-2] ~ pos[:, s.num_C] + e_z * steering_pos[1] - pos[:, s.num_E-1] ~ pos[:, s.num_D] + e_z * steering_pos[2] - vel[:, s.num_E-2] ~ vel[:, s.num_C] + e_z * steering_vel[1] - vel[:, s.num_E-1] ~ vel[:, s.num_D] + e_z * steering_vel[2] - acc[:, s.num_E-2] ~ acc[:, s.num_C] + e_z * steering_acc[1] - acc[:, s.num_E-1] ~ acc[:, s.num_D] + e_z * steering_acc[2] - segment_length ~ tether_length ./ s.set.segments - mass_tether_particle ~ mass_per_meter .* segment_length - damping ~ s.set.damping ./ segment_length - c_spring ~ s.set.c_spring ./ segment_length - P_c ~ 0.5 * (pos[:, s.num_C] + pos[:, s.num_D]) - e_y ~ (pos[:, s.num_C] - pos[:, s.num_D]) / norm(pos[:, s.num_C] - pos[:, s.num_D]) - e_z ~ (pos[:, s.num_E] - P_c) / norm(pos[:, s.num_E] - P_c) - e_x ~ cross(e_y, e_z) + pos[:, s.num_flap_C] ~ pos[:, s.num_C] - e_x * flap_length * cos(flap_angle[1]) + e_r_C * flap_length * sin(flap_angle[1]) + pos[:, s.num_flap_D] ~ pos[:, s.num_D] - e_x * flap_length * cos(flap_angle[2]) + e_r_D * flap_length * sin(flap_angle[2]) + vel[:, s.num_flap_C] ~ vel[:, s.num_C] - e_x * flap_length * cos(flap_vel[1]) + e_r_C * flap_length * sin(flap_vel[1]) + vel[:, s.num_flap_D] ~ vel[:, s.num_D] - e_x * flap_length * cos(flap_vel[2]) + e_r_D * flap_length * sin(flap_vel[2]) + acc[:, s.num_flap_C] ~ acc[:, s.num_C] - e_x * flap_length * cos(flap_acc[1]) + e_r_C * flap_length * sin(flap_acc[1]) + acc[:, s.num_flap_D] ~ acc[:, s.num_D] - e_x * flap_length * cos(flap_acc[2]) + e_r_D * flap_length * sin(flap_acc[2]) + segment_length ~ tether_length ./ s.set.segments + mass_tether_particle ~ mass_per_meter .* segment_length + damping ~ s.damping ./ segment_length + c_spring ~ s.c_spring ./ segment_length + P_c ~ 0.5 * (pos[:, s.num_C] + pos[:, s.num_D]) + e_y ~ (pos[:, s.num_C] - pos[:, s.num_D]) / norm(pos[:, s.num_C] - pos[:, s.num_D]) + e_z ~ (pos[:, s.num_E] - P_c) / norm(pos[:, s.num_E] - P_c) + e_x ~ cross(e_y, e_z) + e_r_C ~ (E_C - pos[:, s.num_C]) / norm(E_C - pos[:, s.num_C]) + e_r_D ~ (E_C - pos[:, s.num_D]) / norm(E_C - pos[:, s.num_D]) + e_te_C ~ e_x * sin(flap_angle[1]) + e_r_C * cos(flap_angle[1]) + e_te_D ~ e_x * sin(flap_angle[2]) + e_r_D * cos(flap_angle[2]) + # E_C is the center of the circle shape of the front view of the kite + E_C ~ pos[:, s.num_E] + e_z * (-s.set.bridle_center_distance + s.set.radius) rho_kite ~ calc_rho(s.am, pos[3,s.num_A]) + damping_coeff ~ max(1.0 - t, 0.0) * s.damping_coeff ] - eqs2, force_eqs = calc_aero_forces_mtk!(s, eqs2, force_eqs, force, pos, vel, t, e_x, e_y, e_z, rho_kite, v_wind, steering_pos) + eqs2, force_eqs = calc_aero_forces_mtk!(s, eqs2, force_eqs, force, pos, vel, t, e_x, e_y, e_z, E_C, rho_kite, v_wind, flap_angle) eqs2, force_eqs = inner_loop_mtk!(s, eqs2, force_eqs, t, force, pos, vel, segment_length, c_spring, damping, - v_wind_gnd, stiffness_factor) + v_wind_gnd) for i in 1:3 eqs2 = vcat(eqs2, vcat(force_eqs[:, i])) eqs2 = vcat(eqs2, acc[:, i] .~ 0) end - for i in 4:s.num_E-3 - eqs2 = vcat(eqs2, vcat(force_eqs[:,i])) - eqs2 = vcat(eqs2, acc[:, i] .~ [0.0; 0.0; -G_EARTH] .+ (force[:,i] ./ mass_tether_particle[(i-1)%3+1])) - end - for i in s.num_E-2:s.num_E-1 - flap_resistance = [50.0 * ((vel[:,i]-vel[:, s.num_C]) ⋅ e_z) * e_z[j] for j in 1:3] - [force_eqs[j,i] = force[j,i] ~ force_eqs[j,i].rhs + [0.0; 0.0; -G_EARTH][j] + flap_resistance[j] for j in 1:3] - tether_rhs = [force_eqs[j, i].rhs for j in 1:3] - kite_rhs = [force_eqs[j, i+3].rhs for j in 1:3] - f_xy = (tether_rhs ⋅ e_z) .* e_z - force_eqs[:,i] .= force[:, i] .~ tether_rhs .- f_xy - force_eqs[:,i+3] .= force[:, i+3] .~ kite_rhs .+ f_xy - eqs2 = vcat(eqs2, vcat(force_eqs[:, i])) - eqs2 = vcat(eqs2, steering_acc[i-s.num_E+3] ~ (force[:,i] ./ mass_tether_particle[(i-1) % 3 + 1]) ⋅ - e_z - (acc[:, i+3] ⋅ e_z)) + for i in 4:s.num_flap_C-1 + eqs2 = vcat(eqs2, vcat(force_eqs[:, i])) + eqs2 = vcat(eqs2, acc[:, i] .~ [0.0; 0.0; -G_EARTH] .+ (force[:, i] ./ mass_tether_particle[(i-1)%3+1]) .- damping_coeff * vel[:, i]) end + + # torque = I * flap_acc + # flap_acc = torque / (1/3 * (kite_mass/8) * kite_length_c^2) + # torque = force[:, i] * kite_length_c + # flap_acc = force[:, i] * kite_length_c / (1/3 * (kite_mass/8) * kite_length_c^2) + + # 1. add all flap + spring + drag forces to flap_C point + # 2. remove forces not in e_flap_c direction + # 3. substract forces on point flap_C from point C + # 4. calculate acceleration from force flap c in e_flap_c direction + [force_eqs[j, s.num_C] = force[j, s.num_C] ~ force_eqs[j, s.num_C].rhs - force_eqs[j, s.num_flap_C].rhs for j in 1:3] + [force_eqs[j, s.num_D] = force[j, s.num_D] ~ force_eqs[j, s.num_D].rhs - force_eqs[j, s.num_flap_D].rhs for j in 1:3] + eqs2 = [ + eqs2 + vcat(force_eqs[:, s.num_flap_C]) + vcat(force_eqs[:, s.num_flap_D]) + flap_acc[1] ~ ((force[:, s.num_flap_C] + [0.0, 0.0, -G_EARTH]) ⋅ e_te_C - s.damping * 0.25 * flap_vel[1]) * + flap_length / (1/3 * (s.set.mass/8) * flap_length^2) - (damping_coeff*200) * flap_vel[1] + flap_acc[2] ~ ((force[:, s.num_flap_D] + [0.0, 0.0, -G_EARTH]) ⋅ e_te_D - s.damping * 0.25 * flap_vel[2]) * + flap_length / (1/3 * (s.set.mass/8) * flap_length^2) - (damping_coeff*200) * flap_vel[2] + ] + for i in s.num_E:s.num_A - eqs2 = vcat(eqs2, vcat(force_eqs[:,i])) - eqs2 = vcat(eqs2, acc[:, i] .~ [0.0; 0.0; -G_EARTH] .+ (force[:, i] ./ s.masses[i])) + eqs2 = vcat(eqs2, vcat(force_eqs[:, i])) + eqs2 = vcat(eqs2, acc[:, i] .~ [0.0; 0.0; -G_EARTH] .+ (force[:, i] ./ s.masses[i]) .- (damping_coeff) * vel[:, i]) end eqs = vcat(eqs1, eqs2) @named sys = ODESystem(Symbolics.scalarize.(reduce(vcat, Symbolics.scalarize.(eqs))), t) - s.simple_sys = structural_simplify(sys) + s.simple_sys = structural_simplify(sys; simplify=true) nothing end @@ -894,4 +1035,31 @@ function settings_hash(st) h = hash(field_value, h) end return h +end + +function read_csv(filename="polars.csv") + if !endswith(filename, ".csv") + filename *= ".csv" + end + filename = joinpath(dirname(get_data_path()), filename) + data = Dict{String, Vector{Float64}}() + try + open(filename, "r") do f + header = split(chomp(readline(f)), ",") + for col in header + data[col] = Float64[] + end + for line in eachline(f) + values = split(chomp(line), ",") + for (i, col) in enumerate(header) + push!(data[col], parse(Float64, values[i])) + end + end + end + catch e + println("Could not open csv file.") + println(e) + return nothing + end + return data end \ No newline at end of file diff --git a/src/KiteModels.jl b/src/KiteModels.jl index 09fc83cb..90d1976c 100644 --- a/src/KiteModels.jl +++ b/src/KiteModels.jl @@ -35,7 +35,7 @@ module KiteModels using PrecompileTools: @setup_workload, @compile_workload using Dierckx, StaticArrays, Rotations, LinearAlgebra, Parameters, NLsolve, DocStringExtensions, OrdinaryDiffEqCore, - OrdinaryDiffEqBDF, OrdinaryDiffEqSDIRK, Serialization, DataInterpolations + OrdinaryDiffEqBDF, OrdinaryDiffEqSDIRK, Sundials import Sundials using Reexport, Pkg @reexport using KitePodModels @@ -91,10 +91,8 @@ Basic 3-dimensional vector, stack allocated, immutable. """ const SVec3 = SVector{3, SimFloat} -# the following two definitions speed up the function residual! from 940ns to 540ns -# disadvantage: changing the cl and cd curves requires a restart of the program -const rad_cl_mtk = CubicSpline(se().cl_list, deg2rad.(se().alpha_cl); extrapolate=true) -const rad_cd_mtk = CubicSpline(se().cd_list, deg2rad.(se().alpha_cd); extrapolate=true) +# const rad_cl_mtk = CubicSpline(se().cl_list, deg2rad.(se().alpha_cl); extrapolate=true) +# const rad_cd_mtk = CubicSpline(se().cd_list, deg2rad.(se().alpha_cd); extrapolate=true) """ abstract type AbstractKiteModel @@ -618,14 +616,14 @@ end set.kcu_diameter = 0 kps4_::KPS4 = KPS4(KCU(set)) kps3_::KPS3 = KPS3(KCU(se("system.yaml"))) - kps4_3l_::KPS4_3L = KPS4_3L(KCU(se(SYS_3L))) + # kps4_3l_::KPS4_3L = KPS4_3L(KCU(se(SYS_3L))) # TODO: add back @assert ! isnothing(kps4_.wm) @compile_workload begin # all calls in this block will be precompiled, regardless of whether # they belong to your package or not (on Julia 1.8 and higher) integrator = KiteModels.init_sim!(kps3_; stiffness_factor=0.035, prn=false) integrator = KiteModels.init_sim!(kps4_; delta=0.03, stiffness_factor=0.05, prn=false) - integrator = KiteModels.init_sim!(kps4_3l_) + # integrator = KiteModels.init_sim!(kps4_3l_) nothing end end diff --git a/src/init.jl b/src/init.jl index 85a6b437..df5ede56 100644 --- a/src/init.jl +++ b/src/init.jl @@ -30,48 +30,13 @@ function init_springs!(s::KPS4) s.springs end -# "get the azimuth of the steering tethers" -# function get_tether_azimuth(width, radius, tip_length, middle_length) -# α_0 = pi/2 - width/2/radius -# α_c = α_0 + width*(-2*tip_length + sqrt(2*middle_length^2 + 2*tip_length^2))/(4*(middle_length - tip_length)) / radius -# distance_c = cos(α_c)*radius -# α_tether = atan(distance_c/l_tether) -# return α_tether -# end - -# implemented -function get_particles(width, radius, middle_length, tip_length, bridle_center_distance, pos_kite = [ 75., 0., 129.90381057], - vec_c=[-15., 0., -25.98076211], v_app=[10.4855, 0, -3.08324]) - # inclination angle of the kite; beta = atan(-pos_kite[2], pos_kite[1]) ??? - beta = pi/2.0 - - e_z = normalize(vec_c) # vec_c is the direction of the last two particles - e_y = normalize(cross(v_app, e_z)) - e_x = normalize(cross(e_y, e_z)) - - α_0 = pi/2 - width/2/radius - α_c = α_0 + width*(-2*tip_length + sqrt(2*middle_length^2 + 2*tip_length^2))/(4*(middle_length - tip_length)) / radius - α_d = π - α_c - - E = pos_kite - E_c = pos_kite + e_z * (-bridle_center_distance + radius) # E at center of circle on which the kite shape lies - C = E_c + e_y*cos(α_c)*radius - e_z*sin(α_c)*radius - D = E_c + e_y*cos(α_d)*radius - e_z*sin(α_d)*radius - - length(α) = α < π/2 ? - (tip_length + (middle_length-tip_length)*α*radius/(0.5*width)) : - (tip_length + (middle_length-tip_length)*(π-α)*radius/(0.5*width)) - P_c = (C+D)./2 - A = P_c - e_x*(length(α_c)*(3/4 - 1/4)) - - [E, C, D, A] # important to have the order E = 1, C = 2, D = 3, A = 4 -end - # implemented - looks good function init_springs!(s::KPS4_3L) l_0 = s.set.l_tether / s.set.segments - particles = get_particles(s.set.width, s.set.radius, s.set.middle_length, s.set.tip_length, s.set.bridle_center_distance) + E, C, D, A, _, _ = KiteUtils.get_particles_3l(s.set.width, s.set.radius, + s.set.middle_length, s.set.tip_length, s.set.bridle_center_distance) + particles = [E, C, D, A] # build the tether segments of the three tethers k = s.set.e_tether * (s.set.d_tether/2000.0)^2 * pi / l_0 # Spring stiffness for this spring [N/m] @@ -85,8 +50,8 @@ function init_springs!(s::KPS4_3L) p0, p1 = SPRINGS_INPUT_3L[i, 1], SPRINGS_INPUT_3L[i, 2] # bridle points l_0 = norm(particles[Int(p1)] - particles[Int(p0)]) * PRE_STRESS k = s.set.e_tether * (s.set.d_line/2000.0)^2 * pi / l_0 - p0 += s.num_E-1 # correct the index for the start and end particles of the bridle - p1 += s.num_E-1 + p0 += s.num_flap_D # correct the index for the start and end particles of the bridle + p1 += s.num_flap_D c = s.set.damping/ l_0 s.springs[i+s.set.segments*3] = SP(Int(p0), Int(p1), l_0, k, c) end @@ -120,11 +85,11 @@ function init_masses!(s::KPS4_3L) for i in 4:s.set.segments*3 s.masses[i] += l_0 * mass_per_meter end - [s.masses[i] += 0.5 * l_0 * mass_per_meter for i in s.num_E-2:s.num_E] + [s.masses[i] += 0.5 * l_0 * mass_per_meter for i in s.num_flap_C:s.num_E] s.masses[s.num_E] += 0.5 * s.set.l_bridle * mass_per_meter - s.masses[s.num_A] += s.set.mass/2 - s.masses[s.num_C] += s.set.mass/4 - s.masses[s.num_D] += s.set.mass/4 + s.masses[s.num_A] += s.set.mass/4 + s.masses[s.num_C] += s.set.mass*3/8 + s.masses[s.num_D] += s.set.mass*3/8 return s.masses end @@ -170,7 +135,7 @@ function init_pos_vel_acc(s::KPS4, X=zeros(2 * (s.set.segments+KITE_PARTICLES)+1 end # implemented -function init_pos_vel_acc(s::KPS4_3L, X=zeros(5*s.set.segments+3); delta = 0.0) +function init_pos_vel_acc(s::KPS4_3L; delta = 0.0) pos = zeros(SVector{s.num_A, KVec3}) vel = zeros(SVector{s.num_A, KVec3}) acc = zeros(SVector{s.num_A, KVec3}) @@ -181,33 +146,39 @@ function init_pos_vel_acc(s::KPS4_3L, X=zeros(5*s.set.segments+3); delta = 0.0) sin_el, cos_el = sin(deg2rad(s.set.elevation)), cos(deg2rad(s.set.elevation)) for (i, j) in enumerate(range(6, step=3, length=s.set.segments)) radius = i * (s.set.l_tether/s.set.segments) - pos[j] .= [cos_el*radius + X[i], delta, sin_el*radius + X[s.set.segments+i]] + pos[j] .= [cos_el*radius, delta, sin_el*radius] end # kite points - vec_c = pos[s.num_E-3] - pos[s.num_E] - particles = get_particles(s.set.width, s.set.radius, s.set.middle_length, s.set.tip_length, s.set.bridle_center_distance, pos[s.num_E], vec_c, s.v_apparent) - pos[s.num_A] .= particles[4] + [X[s.set.segments*2+1], 0, X[s.set.segments*2+2]] - pos[s.num_C] .= particles[2] + X[s.set.segments*2+3 : s.set.segments*2+5] + vec_c = pos[s.num_flap_C-1] - pos[s.num_E] + E, C, D, A, s.α_C, s.kite_length_C = KiteUtils.get_particles_3l(s.set.width, s.set.radius, + s.set.middle_length, s.set.tip_length, s.set.bridle_center_distance, pos[s.num_E], vec_c, s.v_apparent) + + pos[s.num_A] .= A + pos[s.num_C] .= C pos[s.num_D] .= [pos[s.num_C][1], -pos[s.num_C][2], pos[s.num_C][3]] # build tether connection points - e_z = normalize(vec_c) - distance_c_l = 0.0 # distance between c and left steering line + calc_kite_ref_frame!(s, E, C, D) + E_C = pos[s.num_E] + s.e_z * (-s.set.bridle_center_distance + s.set.radius) + e_r_C = (E_C - pos[s.num_C]) / norm(E_C - pos[s.num_C]) + flap_length = s.kite_length_C/4 + angle_flap_c = 0.0 # distance between c and left steering line # distance_c_l = s.set.tip_length/2 # distance between c and left steering line - s.tether_lengths[1] = norm(pos[s.num_C] + e_z .* (X[s.set.segments*2+6] + distance_c_l)) # find the right steering tether length - s.tether_lengths[2] = s.tether_lengths[1] - pos[s.num_E-2] .= pos[s.num_C] + e_z .* (distance_c_l) - pos[s.num_E-1] .= pos[s.num_E-2] .* [1.0, -1.0, 1.0] - + pos[s.num_flap_C] .= pos[s.num_C] - s.e_x * flap_length * cos(angle_flap_c) + e_r_C * flap_length * sin(angle_flap_c) + pos[s.num_flap_D] .= pos[s.num_flap_C] .* [1.0, -1.0, 1.0] + + s.tether_lengths[3] = norm(pos[s.num_E]) + s.tether_lengths[1] = 0.0 # build left and right tether points for (i, j) in enumerate(range(4, step=3, length=s.set.segments-1)) - pos[j] .= pos[s.num_E-2] ./ s.set.segments .* i .+ - [X[2*s.set.segments+6+i], - X[3*s.set.segments+5+i], - X[4*s.set.segments+4+i]] + len = (s.set.segments-1)/2 + middle_distance = (len - abs(i-len))/len + pos[j] .= pos[s.num_flap_C] ./ s.set.segments .* i .+ [(middle_distance)*s.tether_lengths[3]*0.5, 0.0, 0.0] + s.tether_lengths[1] += norm(pos[j] - pos[j-3]) pos[j+1] .= [pos[j][1], -pos[j][2], pos[j][3]] end + s.tether_lengths[2] = s.tether_lengths[1] # set vel and acc for i in 1:s.num_A @@ -231,13 +202,13 @@ function init_pos_vel(s::KPS4, X=zeros(2 * (s.set.segments+KITE_PARTICLES))) pos, vel end -function init_pos_vel(s::KPS4_3L, X=zeros(5*s.set.segments+3)) - pos, vel, _ = init_pos_vel_acc(s, X) +function init_pos_vel(s::KPS4_3L) + pos, vel, _ = init_pos_vel_acc(s) return pos, vel end -function init_pos(s::KPS4_3L, X=zeros(5*s.set.segments+3); delta=0.0) - pos_, _, _ = init_pos_vel_acc(s, X; delta=0.0) +function init_pos(s::KPS4_3L; delta=0.0) + pos_, _, _ = init_pos_vel_acc(s; delta=0.0) pos = zeros(3, s.num_A) [pos[:,i] .= pos_[i] for i in 1:s.num_A] return pos @@ -248,23 +219,23 @@ function init_inner(s::KPS4, X=zeros(2 * (s.set.segments+KITE_PARTICLES-1)+1); o vcat(pos[2:end], vel[2:end]), vcat(vel[2:end], acc[2:end]) end -function init_inner(s::KPS4_3L, X=zeros(5*s.set.segments+3);delta=0.0) - pos_, vel_, acc_ = init_pos_vel_acc(s, X; delta=delta) +function init_inner(s::KPS4_3L; delta=0.0) + pos_, vel_, acc_ = init_pos_vel_acc(s; delta=delta) # remove last left and right tether point and replace them by the length from C and D pos = vcat( - pos_[4:s.num_E-3], + pos_[4:s.num_flap_C-1], pos_[s.num_E:end], ) len = vcat( # connection length - norm(pos_[s.num_C]-pos_[s.num_E-2]), # left line connection distance - norm(pos_[s.num_D]-pos_[s.num_E-1]), # right line connection distance + norm(pos_[s.num_C]-pos_[s.num_flap_C]), # left line connection distance + norm(pos_[s.num_D]-pos_[s.num_flap_D]), # right line connection distance ) vel = vcat( - vel_[4:s.num_E-3], + vel_[4:s.num_flap_C-1], vel_[s.num_E:end], ) acc = vcat( - acc_[4:s.num_E-3], + acc_[4:s.num_flap_C-1], acc_[s.num_E:end], ) vcat(pos, vel, len, [0,0]), vcat(vel, acc, [0,0], [0,0]) @@ -279,8 +250,8 @@ end # implemented -function init(s::KPS4_3L, X=zeros(5*s.set.segments+3); delta=0.0) - y_, yd_ = init_inner(s, X; delta = delta) +function init(s::KPS4_3L; delta=0.0) + y_, yd_ = init_inner(s; delta = delta) y = vcat(reduce(vcat, y_), reduce(vcat,[s.tether_lengths, zeros(3)])) yd = vcat(reduce(vcat, yd_), zeros(6)) MVector{6*(s.num_A-5)+4+6, SimFloat}(y), MVector{6*(s.num_A-5)+4+6, SimFloat}(yd) @@ -295,3 +266,11 @@ function rotate_in_xz(vec, angle) result end +# rotate a 3d vector around the z axis +function rotate_in_yx(vec, angle) + result = similar(vec) + result[1] = cos(angle) * vec[1] + sin(angle) * vec[2] + result[2] = cos(angle) * vec[2] - sin(angle) * vec[1] + result[3] = vec[3] + result +end \ No newline at end of file diff --git a/test/create_sys_image.jl b/test/create_sys_image.jl index 49320f4a..a1282541 100644 --- a/test/create_sys_image.jl +++ b/test/create_sys_image.jl @@ -5,14 +5,14 @@ if ! ("PackageCompiler" ∈ keys(Pkg.project().dependencies)) Pkg.update() end @info "Loading packages ..." -using Dierckx, StaticArrays, LinearAlgebra, Parameters, NLsolve, DocStringExtensions, Sundials, KiteUtils, KitePodModels, AtmosphericModels, OrdinaryDiffEqCore, OrdinaryDiffEqBDF, OrdinaryDiffEqSDIRK, ModelingToolkit, DataInterpolations +using Dierckx, StaticArrays, LinearAlgebra, Parameters, NLsolve, DocStringExtensions, Sundials, KiteUtils, KitePodModels, AtmosphericModels, OrdinaryDiffEqCore, OrdinaryDiffEqBDF, OrdinaryDiffEqSDIRK, ModelingToolkit using PackageCompiler @info "Creating sysimage ..." push!(LOAD_PATH,joinpath(pwd(),"src")) PackageCompiler.create_sysimage( - [:Dierckx, :StaticArrays, :Parameters, :NLsolve, :DocStringExtensions, :Sundials, :KiteUtils, :KitePodModels, :AtmosphericModels, :OrdinaryDiffEqCore, :OrdinaryDiffEqBDF, :OrdinaryDiffEqSDIRK, :ModelingToolkit, :DataInterpolations],; + [:Dierckx, :StaticArrays, :Parameters, :NLsolve, :DocStringExtensions, :Sundials, :KiteUtils, :KitePodModels, :AtmosphericModels, :OrdinaryDiffEqCore, :OrdinaryDiffEqBDF, :OrdinaryDiffEqSDIRK, :ModelingToolkit],; sysimage_path="kps-image_tmp.so", include_transitive_dependencies=true, precompile_execution_file=joinpath("test", "test_for_precompile.jl")