Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix: reversing with offset tools #2764

Merged
merged 9 commits into from
Sep 16, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 0 additions & 5 deletions config/VehicleConfigurations.xml
Original file line number Diff line number Diff line change
Expand Up @@ -185,7 +185,6 @@ You can define the following custom settings:
<!--\vehicles\ropa-->
<Vehicle name="keiler2.xml"
toolOffsetX = "-2.0"
noReverse = "true"
/>
<!--\vehicles\ropa-->
<Vehicle name="nawaRoMaus.xml"
Expand Down Expand Up @@ -228,12 +227,10 @@ You can define the following custom settings:
/>
<!--\vehicles\lemken-->
<Vehicle name="titan11.xml"
noReverse = "true"
turnRadius = "7.5"
implementWheelAlwaysOnGround = "true"
/>
<Vehicle name="titan18.xml"
noReverse="true"
turnRadius = "7.5"
implementWheelAlwaysOnGround = "true"
raiseLate = "true"
Expand All @@ -252,7 +249,6 @@ You can define the following custom settings:
noReverse = "false"
turnRadius = "9"
/>
<!--vehicles\seedHawk-->
<Vehicle name="980AirCart.xml"
noReverse = "true"
/>
Expand Down Expand Up @@ -406,7 +402,6 @@ You can define the following custom settings:
turnRadius = "7.5"
implementWheelAlwaysOnGround = "true"
workingWidth = "10.5"
noReverse = "true"
/>
<!--Mod: KOECKERLING Vector 460-->
<Vehicle name="vector460.xml"
Expand Down
22 changes: 13 additions & 9 deletions scripts/ai/AIDriveStrategyFieldWorkCourse.lua
Original file line number Diff line number Diff line change
Expand Up @@ -339,18 +339,18 @@ function AIDriveStrategyFieldWorkCourse:shouldLowerImplements(turnEndNode, rever
end

---@param object table is a vehicle or implement object with AI markers (marking the working area of the implement)
---@param turnEndNode number node at the first waypoint of the row, pointing in the direction of travel. This is where
---@param workStartNode number node at the first waypoint of the row, pointing in the direction of travel. This is where
--- the implement should be in the working position after a turn
---@param reversing boolean are we reversing? When reversing towards the turn end point, we must lower the implements
--- when we are _behind_ the turn end node (dz < 0), otherwise once we reach it (dz > 0)
---@return boolean, boolean, number the second one is true when the first is valid, and the distance to the work start
--- in meters (<0) when driving forward, nil when driving backwards.
function AIDriveStrategyFieldWorkCourse:shouldLowerThisImplement(object, turnEndNode, reversing)
function AIDriveStrategyFieldWorkCourse:shouldLowerThisImplement(object, workStartNode, reversing)
local aiLeftMarker, aiRightMarker, aiBackMarker = WorkWidthUtil.getAIMarkers(object, true)
if not aiLeftMarker then return false, false, nil end
local dxLeft, _, dzLeft = localToLocal(aiLeftMarker, turnEndNode, 0, 0, 0)
local dxRight, _, dzRight = localToLocal(aiRightMarker, turnEndNode, 0, 0, 0)
local dxBack, _, dzBack = localToLocal(aiBackMarker, turnEndNode, 0, 0, 0)
local dxLeft, _, dzLeft = localToLocal(aiLeftMarker, workStartNode, 0, 0, 0)
local dxRight, _, dzRight = localToLocal(aiRightMarker, workStartNode, 0, 0, 0)
local dxBack, _, dzBack = localToLocal(aiBackMarker, workStartNode, 0, 0, 0)
local loweringDistance
if AIUtil.hasAIImplementWithSpecialization(self.vehicle, SowingMachine) then
-- sowing machines are stopped while lowering, but leave a little reserve to allow for stopping
Expand All @@ -362,10 +362,14 @@ function AIDriveStrategyFieldWorkCourse:shouldLowerThisImplement(object, turnEnd
loweringDistance = math.min(self.vehicle.lastSpeed, self.settings.turnSpeed:getValue() / 3600) *
self.loweringDurationMs + 0.5 -- vehicle.lastSpeed is in meters per millisecond
end
local dzFront = (dzLeft + dzRight) / 2
local aligned = CpMathUtil.isSameDirection(object.rootNode, workStartNode, 15)
-- some implements, especially plows may have the left and right markers offset longitudinally
-- so if the implement is aligned with the row direction already, then just take the front one
-- if not aligned, work with an average
local dzFront = aligned and math.max(dzLeft, dzRight) or (dzLeft + dzRight) / 2
local dxFront = (dxLeft + dxRight) / 2
self:debug('%s: dzLeft = %.1f, dzRight = %.1f, dzFront = %.1f, dxFront = %.1f, dzBack = %.1f, loweringDistance = %.1f, reversing %s',
CpUtil.getName(object), dzLeft, dzRight, dzFront, dxFront, dzBack, loweringDistance, tostring(reversing))
self:debug('%s: dzLeft = %.1f, dzRight = %.1f, aligned = %s, dzFront = %.1f, dxFront = %.1f, dzBack = %.1f, loweringDistance = %.1f, reversing %s',
CpUtil.getName(object), dzLeft, dzRight, aligned, dzFront, dxFront, dzBack, loweringDistance, tostring(reversing))
local dz = self:getImplementLowerEarly() and dzFront or dzBack
if reversing then
return dz < 0, true, nil
Expand Down Expand Up @@ -397,7 +401,7 @@ end
function AIDriveStrategyFieldWorkCourse:isThisImplementAligned(object, node)
local aiFrontMarker, _, _ = WorkWidthUtil.getAIMarkers(object, true)
if not aiFrontMarker then return true end
return CpMathUtil.isSameDirection(aiFrontMarker, node, 2)
return CpMathUtil.isSameDirection(aiFrontMarker, node, 5)
end

-----------------------------------------------------------------------------------------------------------------------
Expand Down
28 changes: 20 additions & 8 deletions scripts/ai/AIDriveStrategyPlowCourse.lua
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,9 @@ function AIDriveStrategyPlowCourse.new(customMt)
local self = AIDriveStrategyFieldWorkCourse.new(customMt)
AIDriveStrategyFieldWorkCourse.initStates(self, AIDriveStrategyPlowCourse.myStates)
self.debugChannel = CpDebug.DBG_FIELDWORK
-- the plow offset is automatically calculated on each waypoint and if it wasn't calculated for a while
-- or when some event (like a turn) invalidated it
self.plowOffsetUnknown = CpTemporaryObject(true)
return self
end

Expand Down Expand Up @@ -86,6 +89,9 @@ function AIDriveStrategyPlowCourse:getDriveData(dt, vX, vY, vZ)
self:debug('Plow is unfolded and ready to start')
end
end
if self.plowOffsetUnknown:get() then
self:updatePlowOffset()
end
return AIDriveStrategyFieldWorkCourse.getDriveData(self, dt, vX, vY, vZ)
end

Expand All @@ -104,15 +110,23 @@ end
function AIDriveStrategyPlowCourse:updatePlowOffset()
local xOffset = 0
for _, controller in pairs(self.controllers) do
if controller.getAutomaticXOffset then
xOffset = xOffset + controller:getAutomaticXOffset()
if controller.getAutomaticXOffset then
local autoOffset = controller:getAutomaticXOffset()
if autoOffset == nil then
self:debugSparse('Plow offset can\'t be calculated now, leaving offset at %.2f', self.aiOffsetX)
return
end
xOffset = xOffset + autoOffset
end
end
local oldOffset = self.aiOffsetX
-- set to the average of old and new to smooth a little bit to avoid oscillations
self.aiOffsetX = (0.5 * self.aiOffsetX + 1.5 * xOffset) / 2
self:debug("Plow offset calculated was %.2f and it changed from %.2f to %.2f",
xOffset, oldOffset, self.aiOffsetX)
-- when we have a valid previous value
self.aiOffsetX = self.plowOffsetUnknown:get() and xOffset or ((0.5 * self.aiOffsetX + 1.5 * xOffset) / 2)
if math.abs(oldOffset - xOffset) > 0.05 then
self:debug("Plow offset calculated was %.2f and it changed from %.2f to %.2f", xOffset, oldOffset, self.aiOffsetX)
end
self.plowOffsetUnknown:set(false, 3000)
end

--- Is a plow currently rotating?
Expand Down Expand Up @@ -173,8 +187,6 @@ end
--- When we return from a turn, the offset is reverted and should immediately set, not waiting
--- for the first waypoint to pass as it is on the wrong side right after the turn
function AIDriveStrategyPlowCourse:resumeFieldworkAfterTurn(ix)
-- call twice to trick the smoothing and reach the desired value sooner.
self:updatePlowOffset()
self:updatePlowOffset()
self.plowOffsetUnknown:reset()
AIDriveStrategyPlowCourse.superClass().resumeFieldworkAfterTurn(self, ix)
end
Loading
Loading