diff --git a/FS25_ADSmartPickup/adHitch.lua b/FS25_ADSmartPickup/adHitch.lua index 3836de2..517431e 100644 --- a/FS25_ADSmartPickup/adHitch.lua +++ b/FS25_ADSmartPickup/adHitch.lua @@ -27,6 +27,8 @@ ADHitch.ACT_TIMEOUT_MS = 8000 -- så længe forsøges kobling efter ankomst ADHitch.DETACH_TIMEOUT_MS = 20000 -- afkobling: redskabet skal måske foldes ud først (plov: "unfold the tool") ADHitch.TICK_MS = 250 ADHitch.ROUTE_WAIT_MS = 10000 -- pause ved almindelige markører midt i en tur +ADHitch.ROOMY_ATTACH_DISTANCE = 3 -- m: koblingsrækkevidde når spillets egen ikke rækker +ADHitch.CREPT_EXTRA_DISTANCE = 8 -- m: ekstra ankomstafstand når traktoren har bakket hen til vognen -- ---- ren logik ---- @@ -91,9 +93,13 @@ function ADHitch.findMarker(markers, name) return nil end --- state = {adActive, distance, speed} +-- state = {adActive, distance, speed, arriveDistance?} (arriveDistance: udvidet, når traktoren har bakket hen til vognen) +local function arriveDistanceOf(state) + return state.arriveDistance or ADHitch.ARRIVE_DISTANCE +end + function ADHitch.hasArrived(state) - return not state.adActive and state.distance <= ADHitch.ARRIVE_DISTANCE and state.speed <= ADHitch.STILL_SPEED + return not state.adActive and state.distance <= arriveDistanceOf(state) and state.speed <= ADHitch.STILL_SPEED end function ADHitch.newJob(markerId, markerName, action) @@ -110,7 +116,7 @@ function ADHitch.step(job, state, dt, succeeded) job.seenActive = true elseif ADHitch.hasArrived(state) then job.state = "acting" - elseif state.distance > ADHitch.ARRIVE_DISTANCE and (job.seenActive or job.elapsed >= ADHitch.START_GRACE_MS) then + elseif state.distance > arriveDistanceOf(state) and (job.seenActive or job.elapsed >= ADHitch.START_GRACE_MS) then job.state, job.reason = "failed", "stoppedAway" end elseif job.state == "acting" then @@ -181,9 +187,14 @@ function ADHitch.tryAttach(vehicle) if spec == nil or AttacherJoints == nil or AttacherJoints.updateVehiclesInAttachRange == nil then return false, "noJoints" end + -- spillets egen rækkevidde først, så en rummeligere (spillet retter selv vognen ind ved koblingen) AttacherJoints.updateVehiclesInAttachRange(vehicle, AttacherJoints.MAX_ATTACH_DISTANCE_SQ, AttacherJoints.MAX_ATTACH_ANGLE, true) local info = spec.attachableInfo + if (info == nil or info.attachable == nil) and ADHitch.ROOMY_ATTACH_DISTANCE ^ 2 > AttacherJoints.MAX_ATTACH_DISTANCE_SQ then + AttacherJoints.updateVehiclesInAttachRange(vehicle, ADHitch.ROOMY_ATTACH_DISTANCE ^ 2, AttacherJoints.MAX_ATTACH_ANGLE, true) + info = spec.attachableInfo + end if info == nil or info.attachable == nil then return false, "noAttachable" end local attacher = info.attacherVehicle or vehicle local farmId = vehicle.getActiveFarm ~= nil and vehicle:getActiveFarm() or vehicle:getOwnerFarmId() @@ -272,6 +283,13 @@ local function start(adEnv, vehicle, markerId, action, onDone) return job end +-- Kørende kobl-på-job for traktoren (til bakningen de sidste meter), ellers nil. +function ADHitch.pendingAttach(vehicle) + local job = jobs[vehicle] + if job ~= nil and job.action == "attach" and job.state == "driving" then return job end + return nil +end + -- Afbryd et hente-/koble-job (Stop alt, høsten slut, spilleren overtager): intet kobles på bagefter, og -- onDone kaldes ikke. -> true hvis der var et job. function ADHitch.cancel(vehicle) @@ -410,7 +428,9 @@ function ADHitch.tick(adEnv, dt) succeeded, detail = ACTIONS[job.action](vehicle) if not succeeded then job.lastReason = detail end end - ADHitch.step(job, vehicleState(adEnv, vehicle, job.markerId), elapsed, succeeded) + local state = vehicleState(adEnv, vehicle, job.markerId) + if job.crept then state.arriveDistance = ADHitch.ARRIVE_DISTANCE + ADHitch.CREPT_EXTRA_DISTANCE end + ADHitch.step(job, state, elapsed, succeeded) if job.state == "done" then finish(vehicle, job, true, detail) elseif job.state == "failed" then diff --git a/FS25_ADSmartPickup/adHitchCreep.lua b/FS25_ADSmartPickup/adHitchCreep.lua new file mode 100644 index 0000000..1c2aea9 --- /dev/null +++ b/FS25_ADSmartPickup/adHitchCreep.lua @@ -0,0 +1,193 @@ +-- AD Hitch Creep +-- Bak de sidste meter hen til vognen: når AD "Kør til" når en Connect-markør, og vognens trækøje står 3-8 m +-- lige bag traktorens krog, lægges en lille bakke-opgave i AD's kø før stop. Traktoren bakker i krybefart og +-- standser, så snart spillet ser vognen inden for koblingsrækkevidde; ADHitch kobler derefter på som før. +-- AD's egen bak-kollisionssensor bruges ikke: den ser vognen selv som forhindring. + +ADHitchCreep = {} +ADHitchCreep.LOG_PREFIX = "ADSmartPickup: kobling: " +ADHitchCreep.ROOMY_ATTACH_DISTANCE = 3 -- m: herinden for kobler ADHitch direkte (samme som ADHitch.ROOMY_ATTACH_DISTANCE) +ADHitchCreep.CREEP_MAX_DISTANCE = 8 -- m: længere væk end det bakkes ikke — markøren ligger forkert +ADHitchCreep.CREEP_MAX_LATERAL = 1.5 -- m til siden: mere end det rammer en lige bakning ikke trækøjet +ADHitchCreep.CREEP_TIMEOUT_MS = 25000 +ADHitchCreep.CREEP_SPEED = 2 -- km/t +ADHitchCreep.AWAY_TOLERANCE = 1 -- m: længere væk end ved start = den bakker forkert +ADHitchCreep.RANGE_CHECK_MS = 250 + +-- ---- ren logik ---- + +-- offset = {x, z}: vognens trækøje i traktorens rum set fra krogen (z < 0 = bagved) -> "attach" | "creep" | "none" +function ADHitchCreep.plan(offset) + if offset == nil then return "none" end + local distance = math.sqrt(offset.x ^ 2 + offset.z ^ 2) + if distance <= ADHitchCreep.ROOMY_ATTACH_DISTANCE then return "attach" end + if offset.z >= 0 or distance > ADHitchCreep.CREEP_MAX_DISTANCE or math.abs(offset.x) > ADHitchCreep.CREEP_MAX_LATERAL then + return "none" + end + return "creep" +end + +-- s = {inRange, distance, startDistance, elapsedMs, passed} -> "done" | "drive" | "giveUp" +function ADHitchCreep.step(s) + if s.inRange then return "done" end + if (s.elapsedMs or 0) >= ADHitchCreep.CREEP_TIMEOUT_MS then return "giveUp" end + if s.passed then return "giveUp" end + if s.distance > s.startDistance + ADHitchCreep.AWAY_TOLERANCE then return "giveUp" end + return "drive" +end + +-- ---- spil ---- + +local function worldPos(node) + local x, y, z = getWorldTranslation(node) + return {x = x, y = y, z = z} +end + +local function vehicleName(vehicle) + return vehicle ~= nil and vehicle.getName ~= nil and vehicle:getName() or "?" +end + +-- Traktorens ledige bagerste krog-noder. +local function freeRearJoints(vehicle) + local spec = vehicle.spec_attacherJoints + if spec == nil or spec.attacherJoints == nil or vehicle.rootNode == nil then return {} end + local used = {} + for _, implement in ipairs(spec.attachedImplements or {}) do + if implement.jointDescIndex ~= nil then used[implement.jointDescIndex] = true end + end + local joints = {} + for index, joint in ipairs(spec.attacherJoints) do + if not used[index] and joint.jointTransform ~= nil then + local p = worldPos(joint.jointTransform) + local _, _, lz = worldToLocal(vehicle.rootNode, p.x, p.y, p.z) + if lz < 0 then table.insert(joints, joint.jointTransform) end + end + end + return joints +end + +-- Vognens trækøje i traktorens rum set fra krogen. +local function offsetOf(vehicle, jointNode, targetNode) + local j, t = worldPos(jointNode), worldPos(targetNode) + local x, _, z = worldDirectionToLocal(vehicle.rootNode, t.x - j.x, 0, t.z - j.z) + return {x = x, z = z} +end + +-- Nærmeste frakoblede redskab (samme gård) med et trækøje bag en ledig krog. -> {wagon, joint, target, offset, plan} | nil +function ADHitchCreep.findTarget(vehicle) + local joints = freeRearJoints(vehicle) + if #joints == 0 or g_currentMission == nil or g_currentMission.vehicleSystem == nil then return nil end + local farmId = vehicle.getOwnerFarmId ~= nil and vehicle:getOwnerFarmId() or nil + local best, bestDistance = nil, math.huge + for _, object in pairs(g_currentMission.vehicleSystem.vehicles or {}) do + local attachable = object.spec_attachable + if object ~= vehicle and attachable ~= nil and object.spec_motorized == nil and attachable.inputAttacherJoints ~= nil + and (object.getAttacherVehicle == nil or object:getAttacherVehicle() == nil) + and (farmId == nil or object.getOwnerFarmId == nil or object:getOwnerFarmId() == farmId) then + for _, input in ipairs(attachable.inputAttacherJoints) do + if input.node ~= nil then + for _, joint in ipairs(joints) do + local offset = offsetOf(vehicle, joint, input.node) + local distance = math.sqrt(offset.x ^ 2 + offset.z ^ 2) + if distance < bestDistance and distance <= ADHitchCreep.CREEP_MAX_DISTANCE + 2 then + bestDistance = distance + best = {wagon = object, joint = joint, target = input.node, offset = offset} + end + end + end + end + end + end + if best ~= nil then best.plan = ADHitchCreep.plan(best.offset) end + return best +end + +local function isInRange(vehicle, wagon) + local spec = vehicle.spec_attacherJoints + if spec == nil or AttacherJoints == nil or AttacherJoints.updateVehiclesInAttachRange == nil then return false end + AttacherJoints.updateVehiclesInAttachRange(vehicle, ADHitchCreep.ROOMY_ATTACH_DISTANCE ^ 2, AttacherJoints.MAX_ATTACH_ANGLE, true) + local info = spec.attachableInfo + return info ~= nil and info.attachable == wagon +end + +-- AD-opgave (samme metoder som AD's AbstractTask). +local CreepTask = {} +CreepTask.__index = CreepTask + +function CreepTask.new(adEnv, vehicle, target) + return setmetatable({adEnv = adEnv, vehicle = vehicle, target = target, elapsedMs = 0, checkMs = 0, inRange = false}, CreepTask) +end + +function CreepTask:setUp() + local offset = offsetOf(self.vehicle, self.target.joint, self.target.target) + self.startDistance = math.sqrt(offset.x ^ 2 + offset.z ^ 2) + Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' bakker %.1f m hen til '%s' (%.1f m til siden)", vehicleName(self.vehicle), + self.startDistance, vehicleName(self.target.wagon), offset.x) +end + +function CreepTask:finish(action, distance) + local sdm = self.vehicle.ad.specialDrivingModule + if sdm ~= nil and sdm.stopVehicle ~= nil then sdm:stopVehicle() end + if action == "done" then + Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' står ved '%s' — kobler på", vehicleName(self.vehicle), vehicleName(self.target.wagon)) + else + Logging.warning(ADHitchCreep.LOG_PREFIX .. "'%s' opgav at bakke hen til '%s' (%.1f m tilbage efter %d s)", vehicleName(self.vehicle), + vehicleName(self.target.wagon), distance, math.floor(self.elapsedMs / 1000)) + end + local taskModule = self.adEnv.ADTaskModule + self.vehicle.ad.taskModule:setCurrentTaskFinished(taskModule ~= nil and taskModule.DONT_PROPAGATE or 1) +end + +function CreepTask:update(dt) + self.elapsedMs = self.elapsedMs + dt + self.checkMs = self.checkMs + dt + if self.target.wagon.getAttacherVehicle ~= nil and self.target.wagon:getAttacherVehicle() ~= nil then + return self:finish("giveUp", 0) -- en anden har taget vognen + end + local offset = offsetOf(self.vehicle, self.target.joint, self.target.target) + local distance = math.sqrt(offset.x ^ 2 + offset.z ^ 2) + if self.checkMs >= ADHitchCreep.RANGE_CHECK_MS then + self.checkMs = 0 + self.inRange = isInRange(self.vehicle, self.target.wagon) + end + local action = ADHitchCreep.step({inRange = self.inRange, distance = distance, startDistance = self.startDistance, + elapsedMs = self.elapsedMs, passed = offset.z > 0.5}) + if action ~= "drive" then return self:finish(action, distance) end + + local autoDrive, vehicle = self.adEnv.AutoDrive, self.vehicle + local p = worldPos(self.target.target) + local lx, lz = autoDrive.getDriveDirection(vehicle, p.x, p.y, p.z) + local sdm = vehicle.ad.specialDrivingModule + if sdm ~= nil and sdm.releaseVehicle ~= nil then sdm:releaseVehicle() end + if vehicle.startMotor ~= nil and not vehicle:getIsMotorStarted() and vehicle:getCanMotorRun() then vehicle:startMotor() end + -- som AD's ADSpecialDrivingModule:driveReverse (ikke-guidet): retning spejles ved bakning + autoDrive.driveInDirection(vehicle, dt, 30, 0.3, 0.2, 20, true, false, -lx, -lz, ADHitchCreep.CREEP_SPEED, 1) +end + +function CreepTask:abort() end +function CreepTask:finished() end +function CreepTask:getInfoText() return "Bakker hen til vognen" end +function CreepTask:getI18nInfo() return "" end +function CreepTask:getExcludedVehiclesForCollisionCheck() return {self.target.wagon} end + +local isInstalled = false + +-- DriveToMode:handleFinishedTask lægger StopAndDisableADTask i køen; bakningen lægges ind lige før. +function ADHitchCreep.install(adEnv) + local mode = adEnv ~= nil and adEnv.DriveToMode or nil + if isInstalled or mode == nil or mode.handleFinishedTask == nil or ADHitch == nil or ADHitch.pendingAttach == nil then return end + isInstalled = true + local original = mode.handleFinishedTask + mode.handleFinishedTask = function(self) + local ok, err = pcall(function() + local job = self.driveToDestinationTask ~= nil and ADHitch.pendingAttach(self.vehicle) or nil + if job == nil or job.markerId ~= self.destinationID then return end + local target = ADHitchCreep.findTarget(self.vehicle) + if target == nil or target.plan ~= "creep" then return end + job.crept = true + self.vehicle.ad.taskModule:addTask(CreepTask.new(adEnv, self.vehicle, target)) + end) + if not ok then Logging.warning(ADHitchCreep.LOG_PREFIX .. "bakning til vognen: %s", tostring(err)) end + return original(self) + end +end diff --git a/FS25_ADSmartPickup/adSmartPickup.lua b/FS25_ADSmartPickup/adSmartPickup.lua index f42d516..2ebfeb7 100644 --- a/FS25_ADSmartPickup/adSmartPickup.lua +++ b/FS25_ADSmartPickup/adSmartPickup.lua @@ -1138,6 +1138,9 @@ local function installHook(adEnv) if ADOutbound ~= nil and ADOutbound.install ~= nil then ADOutbound.install(adEnv) end + if ADHitchCreep ~= nil and ADHitchCreep.install ~= nil then + ADHitchCreep.install(adEnv) + end Logging.info(ADSmartPickup.LOG_PREFIX .. "aktiv (mindst fyldte silo med varen; flere valgte varer = behovsstyret forsyning; Wait-punkter for rest-læs; udkørsel til silo/salg)") end diff --git a/FS25_ADSmartPickup/gui/SmartPickupFrame.xml b/FS25_ADSmartPickup/gui/SmartPickupFrame.xml index a854cad..1461b2f 100644 --- a/FS25_ADSmartPickup/gui/SmartPickupFrame.xml +++ b/FS25_ADSmartPickup/gui/SmartPickupFrame.xml @@ -126,20 +126,20 @@ - - - - - + + + + + - - - - - + + + + + diff --git a/FS25_ADSmartPickup/modDesc.xml b/FS25_ADSmartPickup/modDesc.xml index b9049d3..b344582 100644 --- a/FS25_ADSmartPickup/modDesc.xml +++ b/FS25_ADSmartPickup/modDesc.xml @@ -1,7 +1,7 @@ masterdraco - 1.52.1.0 + 1.53.0.0 <en>AD Smart Pickup</en> @@ -47,6 +47,7 @@ + diff --git a/tests/test_adHitch.lua b/tests/test_adHitch.lua index 041ec7b..be266f8 100644 --- a/tests/test_adHitch.lua +++ b/tests/test_adHitch.lua @@ -30,6 +30,10 @@ check("ankomst: stoppet ved markør", H.hasArrived({adActive = false, distance = check("ankomst: AD kører stadig", H.hasArrived({adActive = true, distance = 0.5, speed = 0}), false) check("ankomst: for langt væk", H.hasArrived({adActive = false, distance = 9, speed = 0}), false) check("ankomst: ruller stadig", H.hasArrived({adActive = false, distance = 1, speed = 3}), false) +-- efter at have bakket hen til vognen står traktoren længere fra markøren +check("ankomst: bakket til vognen, udvidet afstand", H.hasArrived({adActive = false, distance = 9, speed = 0, arriveDistance = 13}), true) +local crept = H.step(H.newJob(1, "Connect x", "attach"), {adActive = false, distance = 9, speed = 3, arriveDistance = 13}, 4000, false) +check("vagt: bakket, ruller lidt -> ikke 'stoppedAway'", crept.state, "driving") -- vagten: efter ankomst forsøges koblingen et stykke tid, derefter opgives den local job = H.newJob(25, "Connect Bredal", "attach") check("job: venter på AD", job.state, "driving") diff --git a/tests/test_adHitchCreep.lua b/tests/test_adHitchCreep.lua new file mode 100644 index 0000000..03fef0c --- /dev/null +++ b/tests/test_adHitchCreep.lua @@ -0,0 +1,26 @@ +-- Kør: luajit tests/test_adHitchCreep.lua (fra repo-roden) +dofile("FS25_ADSmartPickup/adHitchCreep.lua") +local C = ADHitchCreep +local failures = 0 +local function check(name, actual, expected) + if actual == expected then print("OK " .. name) else + failures = failures + 1 + print(string.format("FAIL %s: forventede %s, fik %s", name, tostring(expected), tostring(actual))) + end +end +-- plan: vognens trækøje i traktorens rum set fra krogen (x = til siden, z < 0 = bagved) +check("plan: inden for rummelig rækkevidde -> kobl direkte", C.plan({x = 0.4, z = -2.0}), "attach") +check("plan: 5 m lige bagved -> bak", C.plan({x = 0.3, z = -5.0}), "creep") +check("plan: lidt til siden -> bak", C.plan({x = -1.2, z = -6.0}), "creep") +check("plan: for langt til siden -> intet", C.plan({x = 2.5, z = -5.0}), "none") +check("plan: for langt bagved -> intet", C.plan({x = 0, z = -12.0}), "none") +check("plan: foran traktoren -> intet", C.plan({x = 0, z = 5.0}), "none") +check("plan: ingen vogn -> intet", C.plan(nil), "none") +-- skridt under bakningen +check("skridt: vognen inden for rækkevidde -> færdig", C.step({inRange = true, distance = 2.5, startDistance = 6, elapsedMs = 3000}), "done") +check("skridt: på vej -> kør", C.step({inRange = false, distance = 4, startDistance = 6, elapsedMs = 3000}), "drive") +check("skridt: for længe -> giv op", C.step({inRange = false, distance = 4, startDistance = 6, elapsedMs = C.CREEP_TIMEOUT_MS}), "giveUp") +check("skridt: kører væk fra vognen -> giv op", C.step({inRange = false, distance = 7.5, startDistance = 6, elapsedMs = 3000}), "giveUp") +check("skridt: forbi vognen uden kobling -> giv op", C.step({inRange = false, distance = 0.3, startDistance = 6, elapsedMs = 3000, passed = true}), "giveUp") +print(failures == 0 and "\nALLE TESTS OK" or ("\n" .. failures .. " FEJL")) +os.exit(failures == 0 and 0 or 1)