Feat/find mtk steady state

examples/reel_out_4p_3l.jl

@@ -64,7 +64,7 @@ function simulate(integrator, steps, plot=false)
     return iter/steps
 end
 
-integrator = KiteModels.init_sim!(kps4_3l, stiffness_factor=0.3, prn=STATISTIC)
+integrator = KiteModels.init_sim!(kps4_3l)
 kps4_3l.set_speeds = [0.0, 0.0, 0.0]
 
 if PLOT

examples/speed_controlled_mtk.jl

@@ -23,11 +23,13 @@ steering = [0,0,-0.0]
 println("Running models")
 if ! @isdefined mtk_kite; mtk_kite = KPS4_3L(KCU(set)); end
 if ! @isdefined mtk_integrator
-    mtk_integrator = KiteModels.init_sim!(mtk_kite; stiffness_factor=0.1, prn=false, mtk=true, torque_control=false)
+    mtk_integrator = KiteModels.init_sim!(mtk_kite; prn=false, torque_control=false)
 else 
-    mtk_integrator = KiteModels.reset_sim!(mtk_kite; stiffness_factor=0.1)
+    mtk_integrator = KiteModels.reset_sim!(mtk_kite)
 end
 
+println()
+
 println("compiling")
 total_new_time = 0.0
 for i in 1:5

examples/torque_controlled_3l.jl → examples/steady_state_mtk.jl

@@ -13,27 +13,25 @@ set.abs_tol = 0.006
 set.rel_tol = 0.01
 steps = 110
 dt = 1/set.sample_freq
-tspan   = (0.0, dt) 
+tspan   = (0.0, dt)
 
 logger = Logger(3*set.segments + 6, steps)
 
 steering = [5,5,-30.0]
 
 println("Running models")
-s = KPS4_3L(KCU(set))
-integrator = KiteModels.init_sim!(s; stiffness_factor=0.1, prn=false, mtk=false, torque_control=true)
+s::KPS4_3L = KPS4_3L(KCU(set))
+integrator = init_sim!(s; torque_control=true, mtk=true)
+
 sys_state = KiteModels.SysState(s)
+if sys_state.heading > pi
+    sys_state.heading -= 2*pi
+end
 log!(logger, sys_state)
 
-println("compiling")
-total_new_time = 0.0
-for i in 1:5
-    global total_new_time += @elapsed next_step!(s, integrator; set_values=steering)
-end
 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
@@ -43,32 +41,41 @@ for i in 1:steps
     if i == 20
         steering = [0,10,-33]
     end
-    if i == 50
-        steering = [0,0.0,-20]
+    if i == 40
+        steering = [0,0,-10]
     end
-    if i == 70
-        steering = [0,0, -25]
+    if i == 60
+        steering = [0,0,-20]
     end
 
+    if sys_state.heading > pi
+        sys_state.heading -= 2*pi
+    end
     sys_state.var_01 =  s.steering_pos[1]
     sys_state.var_02 =  s.steering_pos[2]
     sys_state.var_03 =  s.reel_out_speeds[1]
     sys_state.var_04 =  s.reel_out_speeds[2]
+
     total_new_time += @elapsed next_step!(s, integrator; set_values=steering)
 
     KiteModels.update_sys_state!(sys_state, s)
     if sys_state.heading > pi
         sys_state.heading -= 2*pi
     end
+    # reltime = i*dt-dt
+    # if mod(i, 5) == 1
+    #     plot2d(s.pos, reltime; zoom=false, front=false, 
+    #                             segments=set.segments, fig="side_view")            
+    # end
     log!(logger, sys_state)
 end
 
 new_time = (dt*steps) / total_new_time
-println("times realtime implicit model: ", new_time)
-println("avg steptime implicit model:   ", total_new_time/steps)
+println("times realtime MTK model: ", new_time)
+println("avg steptime MTK model:   ", total_new_time/steps)
 
 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 implicit model")
+      fig="Steering and Heading MTK model")

examples/torque_controlled_mtk.jl

@@ -9,23 +9,22 @@ end
 using ControlPlots
 
 set = deepcopy(load_settings("system_3l.yaml"))
-set.abs_tol = 0.006
-set.rel_tol = 0.01
-steps = 110
+# 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]
 
-println("Running models")
-if ! @isdefined mtk_kite; mtk_kite = KPS4_3L(KCU(set)); end
-if ! @isdefined mtk_integrator
-    mtk_integrator = KiteModels.init_sim!(mtk_kite; stiffness_factor=0.1, prn=false, mtk=true, torque_control=true)
-else 
-    mtk_integrator = KiteModels.reset_sim!(mtk_kite; stiffness_factor=0.1)
-end
+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
@@ -50,11 +49,11 @@ for i in 1:steps
     if i == 20
         steering = [0,10,-33]
     end
-    if i == 50
-        steering = [0,0.0,-20]
+    if i == 40
+        steering = [0,0,-20]
     end
-    if i == 70
-        steering = [0,0, -25]
+    if i == 60
+        steering = [0,0,-30]
     end
 
     if sys_state.heading > pi