From ef86e733b5a5590f72d53ceea02bab93a6319e12 Mon Sep 17 00:00:00 2001 From: schwiti6190 <58079399+schwiti6190@users.noreply.github.com> Date: Fri, 5 Feb 2021 10:37:56 +0100 Subject: [PATCH] diff changes/fixes mode 9/10 and BunkerSiloManager - reworked shield height control - fixes reverse unloading mode 6 in bunker silo (#6748) - code improvements BunkerSiloManager --- AIDriver.lua | 3 +- BunkersiloManager.lua | 172 ++++++++++++------- FieldworkAIDriver.lua | 7 +- LevelCompactAIDriver.lua | 296 ++++++++++++++++---------------- ShovelModeAIDriver.lua | 31 ++-- UnloadableFieldworkAIDriver.lua | 43 ++--- 6 files changed, 294 insertions(+), 258 deletions(-) diff --git a/AIDriver.lua b/AIDriver.lua index 2a64f41c2..492861652 100644 --- a/AIDriver.lua +++ b/AIDriver.lua @@ -1412,7 +1412,8 @@ end function AIDriver:setOffsetInBGASilo() if self.bunkerSiloManager == nil then - local silo = BunkerSiloManagerUtil.getTargetBunkerSiloByPointOnCourse(self.course,self.ppc:getCurrentWaypointIx()+3) + local ix = self.ppc:getCurrentWaypointIx()+3 + local silo = BunkerSiloManagerUtil.getTargetBunkerSiloAtWaypoint(self.vehicle,self.course,ix) if silo then self.bunkerSiloManager = BunkerSiloManager(self.vehicle, silo,3) end diff --git a/BunkersiloManager.lua b/BunkersiloManager.lua index 6d6f8a013..46a3a4531 100644 --- a/BunkersiloManager.lua +++ b/BunkersiloManager.lua @@ -33,7 +33,6 @@ widthNode --> | | <-- startNode (sx,_,sz) ---@param node targetNode is either the front/back node of the implement used to check if the bestTarget was passed ---@param boolean is the silo a heap ? function BunkerSiloManager:init(vehicle, Silo, width, targetNode,isHeap) - print("BunkerSiloManager: init()") self.siloMap = self:createBunkerSiloMap(vehicle, Silo, width,isHeap) self.silo = Silo self.vehicle = vehicle @@ -229,7 +228,7 @@ function BunkerSiloManager:setOffsetsPerWayPoint(course,bestColumn,ix) local points = {} local foundFirst = 0 for index=ix,course:getNumberOfWaypoints() do - if BunkerSiloManagerUtil.getTargetBunkerSiloByPointOnCourse(course,index)~= nil then + if BunkerSiloManagerUtil.getTargetBunkerSiloAtWaypoint(self.vehicle,course,index)~= nil then local closest,cx,cz = 0,0,0 local leastDistance = math.huge for lineIndex=1,#self.siloMap do @@ -259,6 +258,30 @@ function BunkerSiloManager:setOffsetsPerWayPoint(course,bestColumn,ix) return foundFirst end +---Gets the closest bunker silo map line form a given column to a waypoint ix +---@param course course of the driver +---@param int waypoint ix to get the distance from +---@param int target column of the bunker silo map +---@return float dx,dy,dz closest silo map part position +function BunkerSiloManager:getClosestSiloPartPositionToWaypoint(course,ix,targetColumn) + local closestLine,dx,dz = 0,0,0 + local leastDistance = math.huge + for lineIndex=1,#self.siloMap do + local fillUnit= self.siloMap[lineIndex][targetColumn] + --get the middle of the silo part + local x,z = fillUnit.bx,fillUnit.bz + local distance = course:getDistanceBetweenPointAndWaypoint(x,z, ix) + if leastDistance > distance then + leastDistance = distance + closestLine = lineIndex + dx,dz = x,z + end + end + local dy = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, dx, 1, dz); + return dx,dy,dz +end + + ---have we reached the end ? ---@param siloMapPart bestTarget targeted part ---@return boolean end reached ? @@ -351,9 +374,10 @@ function BunkerSiloManager:updateTarget(bestTarget) end ---drawing the routing of the driver ----@param siloMapPart bestTarget targeted part ----@param tempTarget for driving out of the silo -function BunkerSiloManager:debugRouting(bestTarget,tempTarget) +---@param table bestTarget targeted part +---@param table for driving out of the silo +---@param float target shield height of mode 10 driver +function BunkerSiloManager:debugRouting(bestTarget,tempTarget,targetHeight) if self.siloMap ~= nil and bestTarget ~= nil then local fillUnit = self.siloMap[bestTarget.line][bestTarget.column] @@ -389,6 +413,16 @@ function BunkerSiloManager:debugRouting(bestTarget,tempTarget) cpDebug:drawLine(tx, y, tz, 1, 0, 1, sx, y, sz); cpDebug:drawPoint(tx, y, tz, 1, 1 , 1); end + if targetHeight then + local column = bestTarget.column + local firstFillUnit = self.siloMap[1][column] + local secondFillUnit = self.siloMap[#self.siloMap][column] + local x,z = firstFillUnit.bx, firstFillUnit.bz + local terrainHeight = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, x,1,z) + local height = terrainHeight + targetHeight + local nx,nz = secondFillUnit.bx, secondFillUnit.bz + cpDebug:drawLine(x, height, z, 1, 1, 1, nx, height, nz); + end end end @@ -422,49 +456,32 @@ end BunkerSiloManagerUtil = {} ----get the closest bunkerSilo or heap +---Checks for bunkerSilos between two points ---@param vehicle vehicle of the driver ----@param int forces search around waypointIndex=forcedPoint ----@param boolean is using Heaps allowed, for example mode 9 ----@return targetSilo either the found BunkerSilo or --- a simulated silo by BunkerSiloManagerUtil.getHeapsMinMaxCoords() for Heaps ----@return boolean have we found a heap ? -function BunkerSiloManagerUtil.getTargetBunkerSilo(vehicle,forcedPoint,checkForHeapsActive) - local pointIndex = 0 - if forcedPoint then - pointIndex = forcedPoint; - else - pointIndex = vehicle.cp.driver.shovelFillStartPoint+2 - end - local x,z = vehicle.Waypoints[pointIndex].cx,vehicle.Waypoints[pointIndex].cz - local tx,tz = x,z + 0.50 - local p1x,p1z,p2x,p2z,p1y,p2y = 0,0,0,0,0,0 +---@param int x,z fist point +---@param int tx,tz second point +---return BunkerSilo +function BunkerSiloManagerUtil.getBunkerSilo(vehicle,x,z,tx,tz) if g_currentMission.bunkerSilos ~= nil then for _, bunker in pairs(g_currentMission.bunkerSilos) do local x1,z1 = bunker.bunkerSiloArea.sx,bunker.bunkerSiloArea.sz local x2,z2 = bunker.bunkerSiloArea.wx,bunker.bunkerSiloArea.wz local x3,z3 = bunker.bunkerSiloArea.hx,bunker.bunkerSiloArea.hz - bunker.type = "silo" if MathUtil.hasRectangleLineIntersection2D(x1,z1,x2-x1,z2-z1,x3-x1,z3-z1,x,z,tx-x,tz-z) then + courseplay.debugVehicle(10, vehicle, "Silo was found: %s",nameNum(bunker)) return bunker end end end - --it's not a bunkersSilo, try to find a heap if it is allowed - if checkForHeapsActive then - return BunkerSiloManagerUtil.getHeapCoords(vehicle),true - end end - ----check for heaps and simulate a bunkerSiloMap for the found heap +---Checks for heaps between two points ---@param vehicle vehicle of the driver +---@param int x,z first point +---@param int nx,nz second point ---return targetSilo a simulated bunkerSilo version of the heap -function BunkerSiloManagerUtil.getHeapCoords(vehicle) - local p1x,p1z,p2x,p2z,p1y,p2y = 0,0,0,0,0,0 - - p1x,p1z = vehicle.Waypoints[vehicle.cp.driver.shovelFillStartPoint].cx,vehicle.Waypoints[vehicle.cp.driver.shovelFillStartPoint].cz; - p2x,p2z = vehicle.Waypoints[vehicle.cp.driver.shovelFillEndPoint].cx,vehicle.Waypoints[vehicle.cp.driver.shovelFillEndPoint].cz; +function BunkerSiloManagerUtil.getHeapCoords(vehicle,x,z,nx,nz) + local p1x,p1z,p2x,p2z,p1y,p2y = x,z,nx,nz,0,0 p1y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, p1x, 1, p1z); p2y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, p2x, 1, p2z); local heapFillType = DensityMapHeightUtil.getFillTypeAtLine(p1x, p1y, p1z, p2x, p2y, p2z, 5) @@ -472,7 +489,7 @@ function BunkerSiloManagerUtil.getHeapCoords(vehicle) if not heapFillType or heapFillType == FillType.UNKNOWN then return end - courseplay:debug(string.format("%s: heap with %s found",nameNum(vehicle),tostring(heapFillType)),10) + courseplay.debugVehicle(10, vehicle, "Heap with %s was found",tostring(heapFillType)) --create temp node local point = createTransformGroup("cpTempHeapFindingPoint"); link(g_currentMission.terrainRootNode, point); @@ -484,10 +501,6 @@ function BunkerSiloManagerUtil.getHeapCoords(vehicle) local yRot = MathUtil.getYRotationFromDirection(dx, dz); setRotation(point, 0, yRot, 0); - --debug line vor search area to be sure, the point is set correctly - vehicle.cp.tempMOde9PointX,vehicle.cp.tempMOde9PointY,vehicle.cp.tempMOde9PointZ = getWorldTranslation(point) - vehicle.cp.tempMOde9PointX2,vehicle.cp.tempMOde9PointY2,vehicle.cp.tempMOde9PointZ2 = localToWorld(point,0,0,distance*2) - -- move the line to find out the size of the heap --find maxX @@ -502,7 +515,7 @@ function BunkerSiloManagerUtil.getHeapCoords(vehicle) --print(string.format("fillType:%s distance: %.1f",tostring(fillType),i)) if fillType ~= heapFillType then maxX = i-stepSize - courseplay:debug("maxX= "..tostring(maxX),10) + courseplay.debugVehicle(10, vehicle, "maxX = %.2f",maxX) break end end @@ -517,7 +530,7 @@ function BunkerSiloManagerUtil.getHeapCoords(vehicle) --print(string.format("fillType:%s distance: %.1f",tostring(fillType),i)) if fillType ~= heapFillType then minX = i-stepSize - courseplay:debug("minX= "..tostring(minX),10) + courseplay.debugVehicle(10, vehicle, "minX = %.2f",minX) break end end @@ -533,12 +546,12 @@ function BunkerSiloManagerUtil.getHeapCoords(vehicle) if fillType == heapFillType then foundHeap = true minZ = i-stepSize - courseplay:debug("minZ= "..tostring(minZ),10) + courseplay.debugVehicle(10, vehicle, "minZ = %.2f",minZ) end else if fillType ~= heapFillType then maxZ = i-stepSize+1 - courseplay:debug("maxZ= "..tostring(maxZ),10) + courseplay.debugVehicle(10, vehicle, "maxZ = %.2f",maxZ) break end end @@ -550,36 +563,69 @@ function BunkerSiloManagerUtil.getHeapCoords(vehicle) bunker.bunkerSiloArea.sx,_,bunker.bunkerSiloArea.sz = localToWorld(point,maxX,0,minZ); bunker.bunkerSiloArea.wx,_,bunker.bunkerSiloArea.wz = localToWorld(point,-minX,0,minZ) bunker.bunkerSiloArea.hx,_,bunker.bunkerSiloArea.hz = localToWorld(point,maxX,0,maxZ) - bunker.type = "heap" - -- Clean up the temporary node. unlink(point); delete(point); - - + return bunker end ----get the best column to fill +---Checks if there is a bunker silo in between two waypoints, +---if a bunker was found the return it +---@param vehicle vehicle of the driver +---@param table course of the driver +---@param int first waypoint ix +---@param int last waypoint ix +---@param boolean looking for heaps allowed ? +---@return table bunker silo or simulated heap silo +---@return boolean was a heap found ? +function BunkerSiloManagerUtil.getTargetBunkerSiloBetweenWaypoints(vehicle,course,firstWpIx,lastWpIx,checkForHeapsActive) + local x,_,z = course:getWaypointPosition(firstWpIx) + local nx,_,nz = course:getWaypointPosition(lastWpIx) + + local silo = BunkerSiloManagerUtil.getBunkerSilo(vehicle,x,z,nx,nz) + if silo then + return silo + end + --it's not a bunkersSilo, try to find a heap if it is allowed + if checkForHeapsActive then + return BunkerSiloManagerUtil.getHeapCoords(vehicle,x,z,nx,nz),true + end +end + +---Checks if the waypoint is in a bunker silo, +---if a bunker was found the return it +---@param vehicle vehicle of the driver +---@param table course of the driver +---@param int waypoint ix to look for a silo +---@return table bunker silo +function BunkerSiloManagerUtil.getTargetBunkerSiloAtWaypoint(vehicle,course,wpIx) + local x,_,z = course:getWaypointPosition(wpIx) + local nx,nz = x,z + 0.50 + return BunkerSiloManagerUtil.getBunkerSilo(vehicle,x,z,nx,nz) +end + +---Gets the first waypoint in the silo and the last waypoint +---@param vehicle vehicle of the driver ---@param course course of the driver ----@param int waypointIndex to find the next closest bunkersilo or default is 1 ----@return int best column to fill -function BunkerSiloManagerUtil.getTargetBunkerSiloByPointOnCourse(course,forcedPoint) - local pointIndex = forcedPoint or 1 ; - local x,_,z = course:getWaypointPosition(pointIndex) - local tx,tz = x,z + 0.50 - local p1x,p1z,p2x,p2z,p1y,p2y = 0,0,0,0,0,0 - if g_currentMission.bunkerSilos ~= nil then - for _, bunker in pairs(g_currentMission.bunkerSilos) do - local x1,z1 = bunker.bunkerSiloArea.sx,bunker.bunkerSiloArea.sz - local x2,z2 = bunker.bunkerSiloArea.wx,bunker.bunkerSiloArea.wz - local x3,z3 = bunker.bunkerSiloArea.hx,bunker.bunkerSiloArea.hz - bunker.type = "silo" - if MathUtil.hasRectangleLineIntersection2D(x1,z1,x2-x1,z2-z1,x3-x1,z3-z1,x,z,tx-x,tz-z) then - return bunker +---@param int first waypoint to start checking from +---@return int first waypointIx in silo +---@return int last waypointIx in silo +function BunkerSiloManagerUtil.getFirstAndLastWaypointIxInSilo(vehicle,course,startIx) + local firstIx, lastIx + for ix=startIx,course:getNumberOfWaypoints() do + local x,_,z = course:getWaypointPosition(ix) + local nextIx = math.min(ix,course:getNumberOfWaypoints()) + local nx,_,nz = course:getWaypointPosition(nextIx) + if BunkerSiloManagerUtil.getBunkerSilo(vehicle,x,z,nx,nz) then + if not firstIx then + firstIx = ix end + lastIx = ix + elseif foundFirst then + break end end + return firstIx,lastIx end - diff --git a/FieldworkAIDriver.lua b/FieldworkAIDriver.lua index 3a4f95637..221c5888f 100644 --- a/FieldworkAIDriver.lua +++ b/FieldworkAIDriver.lua @@ -363,7 +363,10 @@ function FieldworkAIDriver:drive(dt) return end elseif self.state == self.states.ON_UNLOAD_OR_REFILL_COURSE then - self:driveUnloadOrRefill(dt) + local giveUpControl = self:driveUnloadOrRefill(dt) + if giveUpControl then + return + end elseif self.state == self.states.RETURNING_TO_FIRST_POINT then self:setSpeed(self:getFieldSpeed()) self.triggerHandler.fillableObject = nil @@ -493,7 +496,7 @@ function FieldworkAIDriver:stopAndRefuel() end ---@return boolean true if unload took over the driving -function FieldworkAIDriver:driveUnloadOrRefill() +function FieldworkAIDriver:driveUnloadOrRefill(dt) if self.course:isTemporary() then -- use the courseplay speed limit until we get to the actual unload course fields (on alignment/temporary) self:setSpeed(self.vehicle.cp.speeds.field) diff --git a/LevelCompactAIDriver.lua b/LevelCompactAIDriver.lua index 51d3ca076..c81c4c156 100644 --- a/LevelCompactAIDriver.lua +++ b/LevelCompactAIDriver.lua @@ -90,7 +90,6 @@ function LevelCompactAIDriver:drive(dt) end if self.levelState == self.states.DRIVE_TO_PARKING then - self:moveShield('up',dt) self.ppc:update() AIDriver.driveCourse(self, dt) elseif self.levelState == self.states.WAITING_FOR_FREE_WAY then @@ -126,6 +125,7 @@ function LevelCompactAIDriver:drive(dt) self:driveSiloCompact(dt) end self:updateInfoText() + self:updateShieldHeight() end ---search for unloaders nearby @@ -209,26 +209,19 @@ function LevelCompactAIDriver:checkShield() local leveler = AIDriverUtil.getImplementWithSpecialization(self.vehicle, Leveler) if leveler then - self:debugSparse("leveler found: %s",nameNum(leveler)) + self:debug("leveler found: %s",nameNum(leveler)) if self:getIsModeFillUp() or self:getIsModeLeveling() then - --record alphaList if not existing - if self.alphaList == nil then - self:setIsAlphaListrecording() - end - if self:getIsAlphaListrecording() then - self:recordAlphaList() - else - return true - end + return true else courseplay:setInfoText(self.vehicle, 'COURSEPLAY_WRONG_TOOL'); - self:debugSparse("fail no working combo found!") + self:debug("fail no working combo found!") end else if self:getIsModeCompact() then return true else - self:debugSparse("fail no working combo found!") + courseplay:setInfoText(self.vehicle, 'COURSEPLAY_WRONG_TOOL'); + self:debug("fail no working combo found!") end end end @@ -278,11 +271,7 @@ function LevelCompactAIDriver:driveSiloLevel(dt) if self.bestTarget == nil then self.bestTarget, self.firstLine, self.targetHeight = self:getBestTargetFillUnitLeveling(self.lastDrivenColumn) end - renderText(0.2,0.395,0.02,"self:drivePush(dt)") - - self:drivePush(dt) - self:moveShield('down',dt,self:getDiffHeightforHeight(self.targetHeight)) - + self:drivePush(dt) if self:isAtEnd() --or self:hasShieldEmpty() or self:isStuck() @@ -298,7 +287,6 @@ function LevelCompactAIDriver:driveSiloLevel(dt) elseif self.fillUpState == self.states.PULLBACK then - self:moveShield('up',dt) if self:isStuck() then self.fillUpState = self.states.PUSH end @@ -315,10 +303,9 @@ function LevelCompactAIDriver:driveSiloFillUp(dt) --initialize first target point if self.bestTarget == nil then self.bestTarget, self.firstLine = self:getBestTargetFillUnitFillUp(self.lastDrivenColumn) - end + end + self.targetHeight = 0 self:drivePush(dt) - self:moveShield('down',dt,0) - --self:moveShield('down',dt,self:getDiffHeightforHeight(0)) if self:lastLineFillLevelChanged() or self:isStuck() --or self:hasShieldEmpty() @@ -332,7 +319,6 @@ function LevelCompactAIDriver:driveSiloFillUp(dt) end end elseif self.fillUpState == self.states.PULLBACK then - self:moveShield('up',dt) if self:drivePull(dt) then self.fillUpState = self.states.PUSH self:deleteBestTargetLeveling() @@ -374,16 +360,18 @@ function LevelCompactAIDriver:drivePull(dt) local fwd = true local refSpeed = math.min(20,self.vehicle.cp.settings.bunkerSpeed:get()) local allowedToDrive = true - local cx,cy,cz = self.course:getWaypointPosition(self.course:getNumberOfWaypoints()) - local lx, lz = AIVehicleUtil.getDriveDirection(self.vehicle.cp.directionNode, cx,cy,cz); + local gx,gy,gz = self.course:waypointLocalToWorld(1,0,0,15) + local lx, lz = AIVehicleUtil.getDriveDirection(self.vehicle.cp.directionNode, gx,gy,gz); self:driveInDirection(dt,lx,lz,fwd,refSpeed,allowedToDrive) --end if I moved over the last way point + self:debugRouting(gx,gz) if lz < 0 then pullDone = true end if self.hasFoundUnloaders then self:changeLevelState(self.states.DRIVE_TO_PARKING) self:deleteBestTarget() + self:raiseImplements() return false end -- self:drawMap() @@ -394,17 +382,10 @@ end ---currently we just drive 10 m ahead and then start normaly drive the buker course function LevelCompactAIDriver:driveToPreStartPosition(dt) local refSpeed = math.min(20,self.vehicle.cp.settings.bunkerSpeed:get()) - self:moveShield('up',dt) - if self.tempTarget == nil then - local gx,gy,gz = localToWorld(self.vehicle.rootNode,0,0,10) - self.tempTarget = {} - self.tempTarget.x = gx - self.tempTarget.y = gy - self.tempTarget.z = gz - end - local lx, lz = AIVehicleUtil.getDriveDirection(self.vehicle.cp.directionNode, self.tempTarget.x,self.tempTarget.y,self.tempTarget.z); - local distX,_,distZ = localToWorld(self.vehicle.rootNode,0,0,0) - if math.abs(distZ-self.tempTarget.z) > 1 then + local gx,gy,gz = self.course:waypointLocalToWorld(1,0,0,15) + local lx, lz = AIVehicleUtil.getDriveDirection(self.vehicle.cp.directionNode, gx,gy,gz); + self:debugRouting(gx,gz) + if lz > 0 then self:driveInDirection(dt,lx,lz,true,refSpeed,true) else self:selectMode() @@ -492,13 +473,19 @@ function LevelCompactAIDriver:hasShieldEmpty() end end +---Is the shield full ? +---@return boolean shield is full +function LevelCompactAIDriver:isShieldFull() + local shield = self.leveler + return shield and shield:getFillUnitFillLevel(1)/shield:getFillUnitCapacity(1) > 0.98 or false +end + function LevelCompactAIDriver:updateTarget() return self.bunkerSiloManager:updateTarget(self.bestTarget) end function LevelCompactAIDriver:isAtEnd() - if not self.bunkerSiloManager then return false end - return self.bunkerSiloManager:isAtEnd(self.bestTarget) + return self.bunkerSiloManager and self.bunkerSiloManager:isAtEnd(self.bestTarget) or false end function LevelCompactAIDriver:deleteBestTarget() @@ -582,15 +569,15 @@ end function LevelCompactAIDriver:checkSilo() if self.bunkerSiloManager == nil then - local silo = BunkerSiloManagerUtil.getTargetBunkerSilo(self.vehicle,1) + local silo = BunkerSiloManagerUtil.getTargetBunkerSiloAtWaypoint(self.vehicle,self.course,1) if silo then + self:debug("silo was found") self.bunkerSiloManager = BunkerSiloManager(self.vehicle,silo,self:getWorkWidth(),self:getValidBackImplement()) + return true + else + courseplay:setInfoText(self.vehicle, courseplay:loc('COURSEPLAY_MODE10_NOSILO')); end - - end - if not self.bunkerSiloManager then - courseplay:setInfoText(self.vehicle, courseplay:loc('COURSEPLAY_MODE10_NOSILO')); - else + else ---silo was already found return true end end @@ -623,104 +610,6 @@ function LevelCompactAIDriver:raiseImplements() self.vehicle:raiseStateChange(Vehicle.STATE_CHANGE_AI_END_LINE) end - -function LevelCompactAIDriver:moveShield(moveDir,dt,fixHeight) - local leveler = self.leveler - local moveFinished = false - if leveler and leveler.spec_attacherJointControl ~= nil then - local spec = leveler.spec_attacherJointControl - local jointDesc = spec.jointDesc - if moveDir == "down" then - - --move attacherJoint down - if spec.heightController.moveAlpha ~= jointDesc.lowerAlpha then - spec.heightTargetAlpha = jointDesc.lowerAlpha - else - local newAlpha = self:getClosestAlpha(fixHeight) - leveler:controlAttacherJoint(spec.controls[2],newAlpha) - moveFinished = true - end - - elseif moveDir == "up" then - if spec.heightController.moveAlpha ~= spec.jointDesc.upperAlpha then - spec.heightTargetAlpha = jointDesc.upperAlpha - if not fixHeight then - leveler:controlAttacherJoint(spec.controls[2], spec.controls[2].moveAlpha + 0.1) - end - else - moveFinished = true - end - end - end; - return moveFinished -end - -function LevelCompactAIDriver:getClosestAlpha(height) - local closestIndex = 99 - local closestValue = 99 - for indexHeight,_ in pairs (self.alphaList) do - --print("try "..tostring(indexHeight)) - local diff = math.abs(height-indexHeight) - if closestValue > diff then - --print(string.format("%s is closer- set as closest",tostring(closestValue))) - closestIndex = indexHeight - closestValue = diff - end - end - return self.alphaList[closestIndex] -end - -function LevelCompactAIDriver:getIsAlphaListrecording() - return self.isAlphaListrecording; -end - -function LevelCompactAIDriver:resetIsAlphaListrecording() - self.isAlphaListrecording = nil -end -function LevelCompactAIDriver:setIsAlphaListrecording() - self.isAlphaListrecording = true - self.alphaList ={} -end -function LevelCompactAIDriver:getDiffHeightforHeight(targetHeight) - local blade = self.leveler - local bladeX,bladeY,bladeZ = getWorldTranslation(self:getLevelerNode(blade)) - local bladeTerrain = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, bladeX,bladeY,bladeZ); - local _,_,offSetZ = worldToLocal(self.vehicle.rootNode,bladeX,bladeY,bladeZ) - local _,projectedTractorY,_ = localToWorld(self.vehicle.rootNode,0,0,offSetZ) - - return targetHeight- (projectedTractorY-bladeTerrain) -end - - -function LevelCompactAIDriver:recordAlphaList() - local blade = self.leveler - local spec = blade.spec_attacherJointControl - local jointDesc = spec.jointDesc - local bladeX,bladeY,bladeZ = getWorldTranslation(self:getLevelerNode(blade)) - local bladeTerrain = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, bladeX,bladeY,bladeZ); - local _,_,offSetZ = worldToLocal(self.vehicle.rootNode,bladeX,bladeY,bladeZ) - local _,projectedTractorY,_ = localToWorld(self.vehicle.rootNode,0,0,offSetZ) - local tractorToGround = courseplay:round(projectedTractorY-bladeTerrain,3) - local bladeToGound = courseplay:round(bladeY-bladeTerrain,3) - - if spec.heightController.moveAlpha ~= jointDesc.lowerAlpha then - spec.heightTargetAlpha = jointDesc.lowerAlpha - blade:controlAttacherJoint(spec.controls[2], spec.controls[2].moveAlpha + 0.1) - else - blade:controlAttacherJoint(spec.controls[2], spec.controls[2].moveAlpha - 0.005) - - --record the related alphas to the alpha list - local alphaEntry = courseplay:round(bladeToGound-tractorToGround,3) - if self.alphaList[alphaEntry] ~= nil then - self:debug("resetIsAlphaListrecording") - self:resetIsAlphaListrecording() - else - self:debug(string.format("self.alphaList[%s] = %s",tostring(alphaEntry),tostring(spec.controls[2].moveAlpha))) - self.alphaList[alphaEntry] = spec.controls[2].moveAlpha - end - end -end - function LevelCompactAIDriver:getLevelerNode(blade) for _, levelerNode in pairs (blade.spec_leveler.nodes) do if levelerNode.node ~= nil then @@ -820,7 +709,7 @@ function LevelCompactAIDriver:getBestTargetFillUnitLeveling(lastDrivenColumn) if siloMap ~= nil then local newColumn = math.ceil(#siloMap[1]/2) if newApproach then - newBestTarget, firstLine = self.bunkerSiloManager:getBestTargetFillUnitFillUp(self.bestTarget) + newBestTarget, firstLine = self.bunkerSiloManager:getBestTargetFillUnitFillUp() self:debug('Best leveling target at line %d, column %d, height %d, first line %d (fist approach)', newBestTarget.line, newBestTarget.column, targetHeight, firstLine) return newBestTarget, firstLine, targetHeight @@ -854,15 +743,24 @@ function LevelCompactAIDriver:getColumnsTargetHeight(newColumn) end totalFillLevel = totalFillLevel + siloMap[i][newColumn].fillLevel end - local newHeight = math.max(0.6,(totalFillLevel/1000)/totalArea) - self:debug("getTargetHeight: totalFillLevel:%s; totalArea:%s Height%s",tostring(totalFillLevel),tostring(totalArea),tostring(newHeight)) + local newHeight = (totalFillLevel/1000)/totalArea + self:debug("getColumnsTargetHeight: totalFillLevel:%.2f; totalArea:%.2f Height%.2f",totalFillLevel,totalArea,newHeight) return newHeight end -function LevelCompactAIDriver:debugRouting() - if self:isDebugActive() and self.bunkerSiloManager then - self.bunkerSiloManager:debugRouting(self.bestTarget) +---Debug of AIVehicleUtil.driveInDirection() pathFinding +---and also target silo unit and targetHeight of the shield +---@param float (optional) gx/gz temporary goal node +function LevelCompactAIDriver:debugRouting(gx,gz) + if self:isDebugActive() then + if self.bunkerSiloManager then + self.bunkerSiloManager:debugRouting(self.bestTarget,nil,self.targetHeight) + end + if gx and gz then + local x,y,z = getWorldTranslation(self.vehicle.cp.directionNode) + cpDebug:drawLine(x,y+3,z,0,0,0,gx,y+3,gz) + end end end @@ -933,3 +831,105 @@ function LevelCompactAIDriver:renderText(y,text,xOffset) return y-0.02 end +---If a leveler is attached, then we don't need the normal joint control +function LevelCompactAIDriver:isShieldJointControlDisabled() + return self.leveler ~= nil +end + +---Is the driver actively pushing into the silo in mode: leveling,fillUp ? +---@return boolean is pushing into silo, so allow lowering of shield +function LevelCompactAIDriver:isShieldLoweringAllowed() + return self.fillUpState == self.states.PUSH and (self.levelState == self.states.DRIVE_SILOFILLUP or self.levelState == self.states.DRIVE_SILOLEVEL) +end + +---Update shield height and rotation +function LevelCompactAIDriver:updateShieldHeight() + if not self:isShieldJointControlDisabled() then + return + end + local shield = self.leveler + local spec = shield.spec_attacherJointControl + if shield and spec then + local jointDesc = spec.jointDesc + if self:isShieldLoweringAllowed() then + local levelerNode = self:getLevelerNode(shield) + local x,y,z = getWorldTranslation(levelerNode) + --small offset to make sure, shield never hits the ground + local safetyGroundOffset = 0.05 + local terrainHeight = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, x,y,z)+safetyGroundOffset + local targetHeight = self:getTargetShieldHeight() + + --safety check to make sure shieldHeightOffset ~= nil + if self.shieldHeightOffset == nil then + self.shieldHeightOffset = 0 + end + + self:updateShieldHeightOffset() + --get the height difference that needs to be adjusted + local heightDiff = terrainHeight+self.shieldHeightOffset+targetHeight-y + --lastHeightAlpha: is the shield height from 0-1 in between jointDesc.lowerDistanceToGround to jointDesc.upperDistanceToGround + --this is how the alpha gets calculated: + -- local upperAlpha = MathUtil.clamp((upperDistanceToGround - upperDistanceToGround) / (lowerDistanceToGround - upperDistanceToGround), 0, 1) == 0 + -- local lowerAlpha = MathUtil.clamp((lowerDistanceToGround - upperDistanceToGround) / (lowerDistanceToGround - upperDistanceToGround), 0, 1) == 1 + + local _,ny,_ = localToLocal(jointDesc.jointTransform,jointDesc.rootNode,0,0,0) + local alpha = MathUtil.clamp((ny+heightDiff - jointDesc.upperDistanceToGround) / (jointDesc.lowerDistanceToGround - jointDesc.upperDistanceToGround), 0, 1) + self:debug("lastCurAlpha: %.2f, nextAlpha: %.2f, heightDiff: %.2f",spec.lastHeightAlpha,alpha,heightDiff) + self:debug("terrainHeight: %.2f,shieldHeight: %.2f, shieldHeightOffset: %.2f, targetHeight: %.2f",terrainHeight,y,self.shieldHeightOffset,targetHeight) + if math.abs(heightDiff)>0.05 then + -- if math.abs(curAlpha - alpha) >0.1 then + spec.heightTargetAlpha = alpha + -- else -- shield height is okay, so don't move the shield anymore + -- shield:controlAttacherJoint(spec.heightController, alpha) + -- spec.heightTargetAlpha =-1 + -- end + else + spec.heightTargetAlpha =-1 + end + --rotate shield to standing on ground position, should roughly be 90 degree to ground + -- local angle = spec.maxTiltAngle + jointDesc.upperRotationOffset = jointDesc.upperRotationOffsetBackup + jointDesc.lowerRotationOffset = jointDesc.lowerRotationOffsetBackup + + else + self.shieldHeightOffset = 0 + --move shield to upperPosition and rotate it up + spec.heightTargetAlpha = jointDesc.upperAlpha + jointDesc.upperRotationOffset = jointDesc.upperRotationOffsetBackup - spec.maxTiltAngle + jointDesc.lowerRotationOffset = jointDesc.lowerRotationOffsetBackup - spec.maxTiltAngle + end + end +end + +---This one needs fine tuning +---If the driver is slower than 2 km/h, then move the shield slowly up (increase self.shieldHeightOffset) +function LevelCompactAIDriver:updateShieldHeightOffset() + local lastSpeed = self.vehicle.lastSpeedReal + local minSpeed = 2 + local maxSpeed = math.max(4,math.floor(self.vehicle.cp.settings.bunkerSpeed:get()*0.7)) + if lastSpeed < minSpeed then + if self:isShieldFull() then + self.shieldHeightOffset = self.shieldHeightOffset + 0.05 + end + elseif lastSpeed > maxSpeed then + self.shieldHeightOffset = math.max(self.shieldHeightOffset - 0.05,0) + end +end + +---Get the target height for the shield +---@return float targetHeight +function LevelCompactAIDriver:getTargetShieldHeight() + return self.targetHeight or 0 +end + +---Disables player control of shield +function LevelCompactAIDriver.actionEventAttacherJointControl(self,superFunc, actionName, inputValue, callbackState, isAnalog) + local rootVehicle = self:getRootVehicle() + if courseplay:isAIDriverActive(rootVehicle) then + if rootVehicle.cp.driver.isShieldJointControlDisabled and rootVehicle.cp.driver:isShieldJointControlDisabled() then + return + end + end + superFunc(self,actionName, inputValue, callbackState, isAnalog) +end +AttacherJointControl.actionEventAttacherJointControl = Utils.overwrittenFunction(AttacherJointControl.actionEventAttacherJointControl,LevelCompactAIDriver.actionEventAttacherJointControl) \ No newline at end of file diff --git a/ShovelModeAIDriver.lua b/ShovelModeAIDriver.lua index 4686b038c..4651bd83c 100644 --- a/ShovelModeAIDriver.lua +++ b/ShovelModeAIDriver.lua @@ -178,7 +178,7 @@ function ShovelModeAIDriver:drive(dt) if self:setShovelToPositionFinshed(2,dt) then --initialize first target point if self.bunkerSiloManager == nil then - local silo,isHeap = BunkerSiloManagerUtil.getTargetBunkerSilo(self.vehicle,nil,true) + local silo,isHeap = self:getTargetBunkerSilo() if silo then self.bunkerSiloManager = BunkerSiloManager(self.vehicle, silo, self:getWorkWidth(),self.shovel.rootNode,isHeap) end @@ -453,9 +453,9 @@ function ShovelModeAIDriver:getDriveStartUnloadRefSpeed() return self.vehicle.cp.speeds.turn end ---check for enough free space to start unloading +---Check if the current dischargeObject has enough free space ---@param dischargeNode dischargeNode of the shovel ----@return boolean has enough free space +---@return boolean object has enough free space, freeSpace > self:getMinNeededFreeCapacity() function ShovelModeAIDriver:hasEnoughSpaceInObject(dischargeNode) local fillType = self.shovel:getDischargeFillType(dischargeNode) local object = dischargeNode.dischargeObject @@ -469,9 +469,10 @@ function ShovelModeAIDriver:hasEnoughSpaceInObject(dischargeNode) end end ---check if no more free space here +---Check if the current dischargeObject is almost full ? ---@param dischargeNode dischargeNode of the shovel ----@return boolean not enough free space left +---@return boolean object is almost full, free capacity < 300*1/self.shovel:getDischargeNodeEmptyFactor() +--- the factor is used to compensate for shovels that unload to fast function ShovelModeAIDriver:almostFullObject(dischargeNode) local fillType = self.shovel:getDischargeFillType(dischargeNode) local object = dischargeNode.dischargeObject @@ -540,8 +541,9 @@ function ShovelModeAIDriver:getIsShovelEmpty() end --- get the minimum required free space ----@return boolean min needed free space +---Get the minimum required free space to start unloading, +---so we are not constantly starting to unload and the stop again directly +---@return float minimum required free space, shovel min(capacity/5,fillLevel) function ShovelModeAIDriver:getMinNeededFreeCapacity() return math.min(self.shovel:getFillUnitCapacity(1)/5,self.shovel:getFillUnitFillLevel(1)) end @@ -662,8 +664,8 @@ function ShovelModeAIDriver:onWaypointPassed(ix) end end --- are all the shovel positions correctly set ? ----@return boolean are shovel positions okay +---Check if all shovel positions are set correctly +---@return boolean are all shovel positions valid ? function ShovelModeAIDriver:checkShovelPositionsValid() local validToolPositions = self.vehicle.cp.settings.frontloaderToolPositions:hasValidToolPositions() if not validToolPositions then @@ -672,8 +674,8 @@ function ShovelModeAIDriver:checkShovelPositionsValid() return validToolPositions end --- are all 3 needed waitpoint correctly setup ? ----@return boolean has necessary waitpoints +---Check if all wait points are set correctly +---@return boolean are all wait points valid ? function ShovelModeAIDriver:checkWaypointsValid() if self.shovelFillStartPoint == nil or self.shovelFillEndPoint == nil or self.shovelEmptyPoint == nil then courseplay:setInfoText(self.vehicle, 'COURSEPLAY_NO_VALID_COURSE'); @@ -814,3 +816,10 @@ end function ShovelModeAIDriver:isProximitySpeedControlEnabled() return self.shovelState.properties.enableProximitySpeedControl end + +---Checks for bunker silo or heaps in between shovelFillStartPoint and shovelFillEndPoint +---@return table bunker silo or simulated heap silo +---@return boolean is the found silo a heap ? +function ShovelModeAIDriver:getTargetBunkerSilo() + return BunkerSiloManagerUtil.getTargetBunkerSiloBetweenWaypoints(self.vehicle,self.course,self.shovelFillStartPoint,self.shovelFillEndPoint,true) +end \ No newline at end of file diff --git a/UnloadableFieldworkAIDriver.lua b/UnloadableFieldworkAIDriver.lua index e070a3228..0437058db 100644 --- a/UnloadableFieldworkAIDriver.lua +++ b/UnloadableFieldworkAIDriver.lua @@ -100,43 +100,20 @@ end ---@return boolean true if unload took over the driving function UnloadableFieldworkAIDriver:driveUnloadOrRefill(dt) self:updateOffset() - local allowedToDrive = true - local isNearUnloadPoint, unloadPointIx = self.course:hasUnloadPointWithinDistance(self.ppc:getCurrentWaypointIx(),20) - -- by default, drive street/recorded speed. - self:setSpeed(self:getRecordedSpeed()) - if not self.ppc:isReversing() then - -- 'cause reverse does the raycasting for us - self:searchForTipTriggers() - end - local takeOverSteering = FieldworkAIDriver.driveUnloadOrRefill(self) - if self.vehicle.cp.totalFillLevel > 0 then - if self:hasTipTrigger() then - -- unload at tip trigger - self:setSpeed(self.vehicle.cp.speeds.approach) - allowedToDrive, takeOverSteering = self:dischargeAtTipTrigger(dt) - courseplay:setInfoText(self.vehicle,"COURSEPLAY_TIPTRIGGER_REACHED"); - self:setSpeed(self.vehicle.cp.speeds.turn) - end - end self.triggerHandler:enableFillTypeUnloading() self.triggerHandler:enableFillTypeUnloadingBunkerSilo() - -- tractor reaches unloadPoint - if isNearUnloadPoint then - self:setSpeed(self.vehicle.cp.speeds.approach) - courseplay:setInfoText(self.vehicle, "COURSEPLAY_TIPTRIGGER_REACHED"); - allowedToDrive, takeOverSteering = self:dischargeAtUnloadPoint(dt,unloadPointIx) - end - - if not allowedToDrive then - self:setSpeed(0) - end - -- done tipping? - if self:hasTipTrigger() and self.vehicle.cp.totalFillLevel <= 10 then - courseplay:resetTipTrigger(self.vehicle, true); + -- TODO: refactor that whole unload process, it was just copied from the legacy CP code + self:searchForTipTriggers() + local allowedToDrive, giveUpControl = self:onUnLoadCourse(true, dt) + if not allowedToDrive then + self:hold() + end + if giveUpControl then + return true end - - return takeOverSteering + FieldworkAIDriver.driveUnloadOrRefill(self,dt) + return false end --- Interface for AutoDrive