From b59b0d587d72f78c5923aa501038b93269bba271 Mon Sep 17 00:00:00 2001 From: orwell96 Date: Thu, 23 Apr 2020 09:44:17 +0200 Subject: Implement a LZB speed lookup table for the path and rewrite velocity controls --- advtrains/init.lua | 1 + advtrains/lzb.lua | 209 ++++++++++++++++++++++++++--------------------- advtrains/path.lua | 38 ++++++++- advtrains/signals.lua | 4 + advtrains/trainlogic.lua | 201 ++++++++++++++++++++++++++------------------- advtrains/wagons.lua | 2 +- 6 files changed, 273 insertions(+), 182 deletions(-) (limited to 'advtrains') diff --git a/advtrains/init.lua b/advtrains/init.lua index f61701c..3e2177e 100644 --- a/advtrains/init.lua +++ b/advtrains/init.lua @@ -73,6 +73,7 @@ function advtrains.pcall(fun) end end) if not succ then + error("pcall") reload_saves() else return return1, return2, return3, return4 diff --git a/advtrains/lzb.lua b/advtrains/lzb.lua index 8846573..706f825 100644 --- a/advtrains/lzb.lua +++ b/advtrains/lzb.lua @@ -4,15 +4,22 @@ --[[ Documentation of train.lzb table train.lzb = { - trav = Current index that the traverser has advanced so far - oncoming = table containing oncoming signals, in order of appearance on the path + trav_index = Current index that the traverser has advanced so far + checkpoints = table containing oncoming signals, in order of index { pos = position of the point - idx = where this is on the path - spd = speed allowed to pass - fun = function(pos, id, train, index, speed, lzbdata) + index = where this is on the path + speed = speed allowed to pass. nil = no effect + callback = function(pos, id, train, index, speed, lzbdata) -- Function that determines what to do on the train in the moment it drives over that point. + -- When spd==0, called instead when train has stopped in front + -- nil = no effect + lzbdata = {} + -- Table of custom data filled in by approach callbacks + -- Whenever an approach callback inserts an LZB checkpoint with changed lzbdata, + -- all consecutive approach callbacks will see these passed as lzbdata table. } + trav_lzbdata = currently active lzbdata table at traverser index } each step, for every item in "oncoming", we need to determine the location to start braking (+ some safety margin) and, if we passed this point for at least one of the items, initiate brake. @@ -45,6 +52,16 @@ function advtrains.set_lzb_param(par, val) end end +local function resolve_latest_lzbdata(ckp, index) + local i = #ckp + local ckpi + while i>0 do + ckpi = ckp[i] + if ckpi.index <= index and ckpi.lzbdata then + return ckpi.lzbdata + end + end +end local function look_ahead(id, train) @@ -56,29 +73,64 @@ local function look_ahead(id, train) --local aware_i = advtrains.path_get_index_by_offset(train, brake_i, AWARE_ZONE) local lzb = train.lzb - local trav = lzb.trav - - --train.debug = lspd + local trav = lzb.trav_index + -- retrieve latest lzbdata + local lzbdata = lzb.trav_lzbdata + + if lzbdata.off_track then + --previous position was off track, do not scan any further + end while trav <= brake_i do - trav = trav + 1 local pos = advtrains.path_get(train, trav) -- check offtrack - if trav > train.path_trk_f then - table.insert(lzb.oncoming, { - pos = pos, - idx = trav-1, - spd = 0, - }) + if trav - 1 == train.path_trk_f then + lzbdata.off_track = true + advtrains.lzb_add_checkpoint(train, trav - 1, 0, nil, lzbdata) else -- run callbacks -- Note: those callbacks are defined in trainlogic.lua for consistency with the other node callbacks - advtrains.tnc_call_approach_callback(pos, id, train, trav, lzb.data) + advtrains.tnc_call_approach_callback(pos, id, train, trav, lzb.trav_lzbdata) end + trav = trav + 1 + end - lzb.trav = trav + lzb.trav_index = trav + +end + +-- Flood-fills train.path_speed, based on this checkpoint +local function apply_checkpoint_to_path(train, checkpoint) + if not checkpoint.speed then + return + end + -- make sure path exists until checkpoint + local pos = advtrains.path_get(train, checkpoint.index) + + local brake_accel = advtrains.get_acceleration(train, 11) + + -- start with the checkpoint index at specified speed + local index = checkpoint.index + local p_speed -- speed in path_speed + local c_speed = checkpoint.speed -- calculated speed at current index + while true do + p_speed = train.path_speed[index] + if (p_speed and p_speed <= c_speed) or index < train.index then + --we're done. train already slower than wanted at this position + return + end + -- insert calculated target speed + train.path_speed[index] = c_speed + -- calculate c_speed at previous index + advtrains.path_get(train, index-1) + local eldist = train.path_dist[index] - train.path_dist[index-1] + -- Calculate the start velocity the train would have if it had a end velocity of c_speed and accelerating with brake_accel, after a distance of eldist: + -- v0² = v1² - 2*a*s + c_speed = math.sqrt( (c_speed * c_speed) - (2 * brake_accel * eldist) ) + index = index - 1 + end end @@ -90,102 +142,75 @@ s = v0 * ------- + - * | ------- | = ----------- a 2 \ a / 2*a ]] -local function apply_control(id, train) - local lzb = train.lzb - - local i = 1 - while i<=#lzb.oncoming do - if lzb.oncoming[i].idx < train.index then - local ent = lzb.oncoming[i] - if ent.fun then - ent.fun(ent.pos, id, train, ent.idx, ent.spd, lzb.data) - end - - table.remove(lzb.oncoming, i) - else - i = i + 1 - end - end - - for i, it in ipairs(lzb.oncoming) do - local a = advtrains.get_acceleration(train, 1) --should be negative - local v0 = train.velocity - local v1 = it.spd - if v1 and v1 <= v0 then - local s = (v1*v1 - v0*v0) / (2*a) - - local st = s + params.ADD_SLOW - if v0 > 3 then - st = s + params.ADD_FAST - end - if v0<=0 then - st = s + params.ADD_STAND - end - - local i = advtrains.path_get_index_by_offset(train, it.idx, -st) - - --train.debug = dump({v0f=v0*f, aff=a*f*f,v0=v0, v1=v1, f=f, a=a, s=s, st=st, i=i, idx=train.index}) - if i <= train.index then - -- Gotcha! Braking... - train.ctrl.lzb = 1 - --train.debug = train.debug .. "BRAKE!!!" - return - end - - i = advtrains.path_get_index_by_offset(train, i, -params.ZONE_ROLL) - if i <= train.index and v0>1 then - -- roll control - train.ctrl.lzb = 2 - return - end - i = advtrains.path_get_index_by_offset(train, i, -params.ZONE_HOLD) - if i <= train.index and v0>1 then - -- hold speed - train.ctrl.lzb = 3 - return - end - end - end - train.ctrl.lzb = nil -end - -local function invalidate(train) +-- Removes all LZB checkpoints and restarts the traverser at the current train index +function advtrains.lzb_invalidate(train) train.lzb = { - trav = atround(train.index), - data = {}, - oncoming = {}, + trav_index = atround(train.index), + trav_lzbdata = {}, + checkpoints = {}, } end -function advtrains.lzb_invalidate(train) - invalidate(train) +-- LZB part of path_invalidate_ahead. Clears all checkpoints that are ahead of start_idx +-- in contrast to path_inv_ahead, doesn't complain if start_idx is behind train.index, clears everything then +function advtrains.lzb_invalidate_ahead(train, start_idx) + if train.lzb then + local idx = atfloor(start_idx) + local i = 1 + while train.lzb.checkpoints[i] do + if train.lzb.checkpoints[i].idx >= idx then + table.remove(train.lzb.checkpoints, i) + else + i=i+1 + end + end + -- re-apply all checkpoints to path_speed + train.path_speed = {} + for _,ckp in train.lzb.checkpoints do + apply_checkpoint_to_path(train, ckp) + end + end end -- Add LZB control point --- udata: User-defined additional data -function advtrains.lzb_add_checkpoint(train, index, speed, callback, udata) +-- lzbdata: If you modify lzbdata in an approach callback, you MUST add a checkpoint AND pass the (modified) lzbdata into it. +-- If you DON'T modify lzbdata, you MUST pass nil as lzbdata. Always modify the lzbdata table in place, never overwrite it! +function advtrains.lzb_add_checkpoint(train, index, speed, callback, lzbdata) local lzb = train.lzb local pos = advtrains.path_get(train, index) - table.insert(lzb.oncoming, { + local lzbdata_c = nil + if lzbdata then + -- make a shallow copy of lzbdata + lzbdata_c = {} + for k,v in pairs(lzbdata) do lzbdata_c[k] = v end + end + local ckp = { pos = pos, - idx = index, - spd = speed, - fun = callback, - udata = udata, - }) + index = index, + speed = speed, + callback = callback, + lzbdata = lzbdata_c, + } + table.insert(lzb.checkpoints, ckp) + + apply_checkpoint_to_path(train, ckp) end advtrains.te_register_on_new_path(function(id, train) - invalidate(train) + advtrains.lzb_invalidate(train) look_ahead(id, train) end) +advtrains.te_register_on_invalidate_ahead(function(id, train) + advtrains.lzb_invalidate_ahead(train, start_idx) +end) + advtrains.te_register_on_update(function(id, train) if not train.path or not train.lzb then atprint("LZB run: no path on train, skip step") return end look_ahead(id, train) - apply_control(id, train) + --apply_control(id, train) end, true) diff --git a/advtrains/path.lua b/advtrains/path.lua index ee82c06..cd7d94a 100644 --- a/advtrains/path.lua +++ b/advtrains/path.lua @@ -19,6 +19,8 @@ -- When the day comes on that path!=node, these will only be set if this index represents a transition between rail nodes -- path_dist - The total distance of this path element from path element 0 -- path_dir - The direction of this path item's transition to the next path item, which is the angle of conns[path_cn[i]].c +-- path_speed- Populated by the LZB system. The maximum speed (velocity) permitted in the moment this path item is passed. +-- (this saves brake distance calculations every step to determine LZB control). nil means no limit. --Variables: -- path_ext_f/b - how far path[i] is set -- path_trk_f/b - how far the path extends along a track. beyond those values, paths are generated in a straight line. @@ -52,6 +54,8 @@ function advtrains.path_create(train, pos, connid, rel_index) [0] = advtrains.conn_angle_median(conns[mconnid].c, conns[connid].c) } + train.path_speed = { } + train.path_ext_f=0 train.path_ext_b=0 train.path_trk_f=0 @@ -123,6 +127,7 @@ function advtrains.path_invalidate(train, ignore_lock) train.path_cp = nil train.path_cn = nil train.path_dir = nil + train.path_speed = nil train.path_ext_f=0 train.path_ext_b=0 train.path_trk_f=0 @@ -131,6 +136,28 @@ function advtrains.path_invalidate(train, ignore_lock) train.path_req_b=0 train.dirty = true + --atdebug(train.id, "Path invalidated") +end + +-- Keeps the path intact, but invalidates all path nodes from the specified index (inclusive) +-- onwards. This has the advantage that we don't need to recalculate the whole path, and we can do it synchronously. +function advtrains.path_invalidate_ahead(train, start_idx) + + local idx = atfloor(start_idx) + + if(idx <= train.index) then + advtrains.path_print(train, atwarn) + error("Train "+train.id+": Cannot path_invalidate_ahead start_idx="+idx+" as train has already passed!") + end + + local i = idx + while train.path[i] do + advtrains.occ.clear_item(train.id, advtrains.round_vector_floor_y(train.path[i])) + end + train.path_ext_f=idx - 1 + train.path_trk_f=idx - 1 + + advtrains.run_callbacks_invahead(train.id, train, idx) end -- Prints a path using the passed print function @@ -141,12 +168,12 @@ function advtrains.path_print(train, printf) printf("path_print: Path is invalidated/inexistant.") return end - printf("i: CP Position Dir CN Dist") + printf("i: CP Position Dir CN Dist Speed") for i = train.path_ext_b, train.path_ext_f do if i==train.path_trk_b then printf("--Back on-track border here--") end - printf(i,": ",train.path_cp[i]," ",train.path[i]," ",train.path_dir[i]," ",train.path_cn[i]," ",train.path_dist[i],"") + printf(i,": ",train.path_cp[i]," ",train.path[i]," ",train.path_dir[i]," ",train.path_cn[i]," ",train.path_dist[i]," ",train.path_speed[i]) if i==train.path_trk_f then printf("--Front on-track border here--") end @@ -350,6 +377,7 @@ function advtrains.path_clear_unused(train) train.path_ext_b = i + 1 end + --[[ Why exactly are we clearing path from the front? This doesn't make sense! for i = train.path_ext_f,train.path_req_f + PATH_CLEAR_KEEP,-1 do advtrains.occ.clear_item(train.id, advtrains.round_vector_floor_y(train.path[i])) train.path[i] = nil @@ -358,14 +386,16 @@ function advtrains.path_clear_unused(train) train.path_cn[i] = nil train.path_dir[i+1] = nil train.path_ext_f = i - 1 - end + end ]] train.path_trk_b = math.max(train.path_trk_b, train.path_ext_b) - train.path_trk_f = math.min(train.path_trk_f, train.path_ext_f) + --train.path_trk_f = math.min(train.path_trk_f, train.path_ext_f) train.path_req_f = math.ceil(train.index) train.path_req_b = math.floor(train.end_index or train.index) end +-- Scan the path of the train for position, without querying the occupation table +-- returns index, or nil if pos is not on the path function advtrains.path_lookup(train, pos) local cp = advtrains.round_vector_floor_y(pos) for i = train.path_ext_b, train.path_ext_f do diff --git a/advtrains/signals.lua b/advtrains/signals.lua index e144aa6..2bad1d4 100644 --- a/advtrains/signals.lua +++ b/advtrains/signals.lua @@ -63,6 +63,7 @@ for r,f in pairs({on={as="off", ls="green", als="red"}, off={as="on", ls="red", rules=advtrains.meseconrules, ["action_"..f.as] = function (pos, node) advtrains.ndb.swap_node(pos, {name = "advtrains:retrosignal_"..f.as..rotation, param2 = node.param2}, true) + advtrains.interlocking.signal_on_aspect_changed(pos) end }}, on_rightclick=function(pos, node, player) @@ -74,6 +75,7 @@ for r,f in pairs({on={as="off", ls="green", als="red"}, off={as="on", ls="red", advtrains.interlocking.show_ip_form(pos, pname) elseif advtrains.check_turnout_signal_protection(pos, player:get_player_name()) then advtrains.ndb.swap_node(pos, {name = "advtrains:retrosignal_"..f.as..rotation, param2 = node.param2}, true) + advtrains.interlocking.signal_on_aspect_changed(pos) end end, -- new signal API @@ -120,6 +122,7 @@ for r,f in pairs({on={as="off", ls="green", als="red"}, off={as="on", ls="red", rules=advtrains.meseconrules, ["action_"..f.as] = function (pos, node) advtrains.setstate(pos, f.als, node) + advtrains.interlocking.signal_on_aspect_changed(pos) end }}, on_rightclick=function(pos, node, player) @@ -131,6 +134,7 @@ for r,f in pairs({on={as="off", ls="green", als="red"}, off={as="on", ls="red", advtrains.interlocking.show_ip_form(pos, pname) elseif advtrains.check_turnout_signal_protection(pos, player:get_player_name()) then advtrains.setstate(pos, f.als, node) + advtrains.interlocking.signal_on_aspect_changed(pos) end end, -- new signal API diff --git a/advtrains/trainlogic.lua b/advtrains/trainlogic.lua index 76bbb7a..a5da28e 100644 --- a/advtrains/trainlogic.lua +++ b/advtrains/trainlogic.lua @@ -36,6 +36,7 @@ end local t_accel_all={ [0] = -10, [1] = -3, + [11] = -2, -- calculation base for LZB [2] = -0.5, [4] = 0.5, } @@ -43,10 +44,19 @@ local t_accel_all={ local t_accel_eng={ [0] = 0, [1] = 0, + [11] = 0, [2] = 0, [4] = 1.5, } +local VLEVER_EMERG = 0 +local VLEVER_BRAKE = 1 +local VLEVER_LZBCALC = 11 +local VLEVER_ROLL = 2 +local VLEVER_HOLD = 3 +local VLEVER_ACCEL = 4 + + tp_player_tmr = 0 advtrains.mainloop_trainlogic=function(dtime) @@ -190,6 +200,8 @@ end function advtrains.get_acceleration(train, lever) local acc_all = t_accel_all[lever] + if not acc_all then return 0 end + local acc_eng = t_accel_eng[lever] local nwagons = #train.trainparts if nwagons == 0 then @@ -215,14 +227,16 @@ local function mkcallback(name) assertt(func, "function") table.insert(callt, func) end - return callt, function(id, train) + return callt, function(id, train, param1, param2, param3) for _,f in ipairs(callt) do - f(id, train) + f(id, train, param1, param2, param3) end end end local callbacks_new_path, run_callbacks_new_path = mkcallback("new_path") +local callbacks_invahead +callbacks_invahead, advtrains.run_callbacks_invahead = mkcallback("invalidate_ahead") -- (id, train, start_idx) local callbacks_update, run_callbacks_update = mkcallback("update") local callbacks_create, run_callbacks_create = mkcallback("create") local callbacks_remove, run_callbacks_remove = mkcallback("remove") @@ -304,6 +318,10 @@ function advtrains.train_ensure_init(id, train) return true end +local function v_target_apply(v_targets, lever, vel) + v_targets[lever] = v_targets[lever] and math.min(v_targets[lever], vel) or vel +end + function advtrains.train_step_b(id, train, dtime) if train.no_step or train.wait_for_path or not train.path then return end @@ -311,18 +329,29 @@ function advtrains.train_step_b(id, train, dtime) advtrains.path_get(train, atfloor(train.index + 2)) advtrains.path_get(train, atfloor(train.end_index - 1)) + --[[ again, new velocity control: + There are two heterogenous means of control: + -> set a fixed acceleration and ignore speed (user) + -> steer towards a target speed, distance doesn't matter + -> needs to specify the maximum acceleration/deceleration values they are willing to accelerate/brake with + -> Reach a target speed after a certain distance (LZB, handled specially) + + ]]-- + --- 3. handle velocity influences --- + -- Variables for "desired velocities" of various parts of the code + local v_targets = {} --Table keys: VLEVER_* + local train_moves=(train.velocity~=0) - local tarvel_cap = train.speed_restriction if train.recently_collided_with_env then - tarvel_cap=0 if not train_moves then train.recently_collided_with_env=nil--reset status when stopped end + v_target_apply(v_targets, VLEVER_EMERG, 0) end if train.locomotives_in_train==0 then - tarvel_cap=0 + v_target_apply(v_targets, VLEVER_ROLL, 0) end --- 3a. this can be useful for debugs/warnings and is used for check_trainpartload --- @@ -337,27 +366,17 @@ function advtrains.train_step_b(id, train, dtime) local back_off_track=train.end_index=trainvelocity then + if braketar and braketar>=v0 then train.atc_brake_target=nil braketar = nil end @@ -391,87 +410,96 @@ function advtrains.train_step_b(id, train, dtime) end train.ctrl.atc = nil - if train.tarvelocity and train.tarvelocity>trainvelocity then - train.ctrl.atc=4 + if train.tarvelocity and train.tarvelocity>v0 then + v_target_apply(v_targets, VLEVER_ACCEL, train.tarvelocity) end - if train.tarvelocity and train.tarvelocitytarvel_cap then - tmp_lever = 0 + --- 2c. If no tv_lever set, honor the user control --- + local a_lever = tv_lever + if not tv_lever then + a_lever = train.ctrl.user + if not a_lever then + -- default to holding current speed + a_lever = VLEVER_HOLD + end end - train.lever = tmp_lever - - --- 3a. actually calculate new velocity --- - if tmp_lever~=3 then - local accel = advtrains.get_acceleration(train, tmp_lever) - local vdiff = accel*dtime - - -- This should only be executed when we are accelerating - -- I suspect that this causes the braking bugs - if tmp_lever == 4 then - - -- ATC control exception: don't cross tarvelocity if - -- atc provided a target_vel - if train.tarvelocity then - local tvdiff = train.tarvelocity - trainvelocity - if tvdiff~=0 and math.abs(vdiff) > math.abs(tvdiff) then - --applying this change would cross tarvelocity - --atdebug("In Tvdiff condition, clipping",vdiff,"to",tvdiff) - --atdebug("vel=",trainvelocity,"tvel=",train.tarvelocity) - vdiff=tvdiff - end - end - if tarvel_cap and trainvelocity<=tarvel_cap and trainvelocity+vdiff>tarvel_cap then - vdiff = tarvel_cap - train.velocity + train.lever = a_lever + + --- 3a. calculate the acceleration required to reach the speed restriction in path_speed (LZB) --- + -- Iterates over the path nodes we WOULD pass if we were continuing with the speed assumed by actual_lever + -- and determines the MINIMUM of path_speed in this range. + -- Then, determines acceleration so that we can reach this 'overridden' target speed in this step (but short-circuited) + if not a_lever or a_lever > VLEVER_BRAKE then + -- only needs to run if we're not yet braking anyway + local tv_vdiff = advtrains.get_acceleration(train, tv_lever) * dtime + local dst_curr_v = (v0 + tv_vdiff) * dtime + local nindex_curr_v = advtrains.path_get_index_by_offset(train, train.index, dst_curr_v) + local i = atfloor(train.index) + local lzb_target + local psp + while true do + psp = train.path_speed[i] + if psp then + lzb_target = lzb_target and math.min(lzb_target, psp) or psp end - local mspeed = (train.max_speed or 10) - if trainvelocity+vdiff > mspeed then - vdiff = mspeed - trainvelocity + if i > nindex_curr_v then + break end + i = i + 1 end - if trainvelocity+vdiff < 0 then - vdiff = - trainvelocity + local dv + if lzb_target and lzb_target <= v0 then + -- apply to tv_target after the actual calculation happened + a_lever = VLEVER_BRAKE + tv_target = tv_target and math.min(tv_target, lzb_target) or lzb_target end - - - train.acceleration=vdiff - train.velocity=train.velocity+vdiff - --if train.ctrl.user then - -- train.tarvelocity = train.velocity - --end + end + + --- 3b. now that we know tv_target and a_lever, calculate effective new v and change it on train + + local dv = advtrains.get_acceleration(train, a_lever) * dtime + local v1 + local tv_effective = false + if tv_target and (math.abs(dv) > math.abs(tv_target - v0)) then + v1 = tv_target + tv_effective = true else - train.acceleration = 0 + v1 = v0 +dv end + if v1 > train.max_speed then + v1 = train.max_speed + end + if v1 < 0 then + v1 = 0 + end + + train.acceleration = v1 - v0 + train.velocity = v1 + --- 4. move train --- local idx_floor = math.floor(train.index) @@ -479,7 +507,10 @@ function advtrains.train_step_b(id, train, dtime) local distance = (train.velocity*dtime) / pdist --debugging code - --train.debug = atdump(train.ctrl).."step_dist: "..math.floor(distance*1000) + --local debutg = advtrains.print_concat_table({"v0=",v0,"v1=",v1,"a_lever",a_lever,"tv_target",tv_target,"tv_eff",tv_effective}) + --train.debug = debutg + + if advtrains.DFLAG and v1>0 then error("DFLAG") end train.index=train.index+distance diff --git a/advtrains/wagons.lua b/advtrains/wagons.lua index 522b649..ca582c1 100644 --- a/advtrains/wagons.lua +++ b/advtrains/wagons.lua @@ -990,7 +990,7 @@ function wagon:show_bordcom(pname) if advtrains.interlocking and train.lzb and #train.lzb.oncoming > 0 then local i=1 while train.lzb.oncoming[i] do - local oci = train.lzb.oncoming[i] + local oci = train.lzb.oncoming[i] --TODO repair this if oci.udata and oci.udata.signal_pos then if advtrains.interlocking.db.get_sigd_for_signal(oci.udata.signal_pos) then form = form .. "button[4.5,8;5,1;ilrs;Remote Routesetting]" -- cgit v1.2.3