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

Turns again #4

Merged
merged 12 commits into from
Dec 15, 2024
Merged
Changes from 1 commit
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
Prev Previous commit
Next Next commit
wip
Peter Vaiko committed Dec 8, 2024
commit 5394a6bf6bafc837286449bcc3ef146478768c6c
16 changes: 14 additions & 2 deletions scripts/ai/AIUtil.lua
Original file line number Diff line number Diff line change
@@ -179,13 +179,25 @@ function AIUtil.getOffsetForTowBarLength(r, towBarLength)
return AIUtil.getTractorRadiusFromImplementRadius(r, towBarLength) - r
end

--- When a tractor is towing an implement in a turn, on what radius will the implement be if
--- the radius the tractor is driving is known?
---@param r number the radius the tractor is on
---@param towBarLength number the length of the tow bar
---@return number the radius the implement will be on. Can be negative, meaning the implement will be
--- moving backwards in the turn
function AIUtil.getImplementRadiusFromTractorRadius(r, towBarLength)
local rImplement = math.sqrt( r * r - towBarLength * towBarLength ) -- the radius the tractor should be on
local rSquared = r * r - towBarLength * towBarLength
local rImplement = rSquared > 0 and math.sqrt(rSquared) or -math.sqrt(-rSquared)
return rImplement
end

--- When a tractor is towing an implement in a turn, on what radius will the tractor be if
--- the radius the implement is known?
---@param r number the radius the implement is following
---@param towBarLength number the length of the tow bar
---@return number the radius the tractor will be on
function AIUtil.getTractorRadiusFromImplementRadius(r, towBarLength)
local rTractor = math.sqrt( r * r + towBarLength * towBarLength ) -- the radius the tractor should be on
local rTractor = math.sqrt( r * r + towBarLength * towBarLength )
return rTractor
end

1 change: 1 addition & 0 deletions scripts/ai/turns/TurnContext.lua
Original file line number Diff line number Diff line change
@@ -230,6 +230,7 @@ end

---@return number angle (radian) between the row and the headland, 90 degrees means the headland is perpendicular to the row
function TurnContext:getHeadlandAngle()
print(self.turnEndWp.angle, self.turnStartWp.angle)
return math.abs(CpMathUtil.getDeltaAngle(math.rad(self.turnEndWp.angle), math.rad(self.turnStartWp.angle)))
end

24 changes: 16 additions & 8 deletions scripts/ai/turns/TurnManeuver.lua
Original file line number Diff line number Diff line change
@@ -339,8 +339,13 @@ function AnalyticTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, t
self:debug('Start generating')

local turnEndNode, endZOffset = self.turnContext:getTurnEndNodeAndOffsets(self.steeringLength)
self:debug('r=%.1f, w=%.1f, steeringLength=%.1f, distanceToFieldEdge=%.1f, goalOffset=%.1f',
turningRadius, workWidth, steeringLength, distanceToFieldEdge, endZOffset)
local _, _, dz = localToLocal(vehicleDirectionNode, turnEndNode, 0, 0, 0)
-- zOffset from the turn end (work start). If there is a zOffset in the turn, that is, the turn end is behind the
-- turn start due to an angled headland, we still want to make the complete 180 turn es close to the field edge
-- as we can, so a towed implement, with an offset arc is turned 180 as soon as possible and has time to align
endZOffset = math.min(dz, endZOffset)
self:debug('r=%.1f, w=%.1f, steeringLength=%.1f, distanceToFieldEdge=%.1f, goalOffset=%.1f, dz=%.1f',
turningRadius, workWidth, steeringLength, distanceToFieldEdge, endZOffset, dz)
self.course = self:findAnalyticPath(vehicleDirectionNode, 0, 0, turnEndNode, self.turnEndXOffset, endZOffset, self.turningRadius)
local endingTurnLength
local dBack = self:getDistanceToMoveBack(self.course, workWidth, distanceToFieldEdge)
@@ -372,9 +377,11 @@ function AnalyticTurnManeuver:getDistanceToMoveBack(course, workWidth, distanceT
distanceToFieldEdge = distanceToFieldEdge + turnEndForwardOffset / 2
-- with a headland at angle, we have to move further back, so the left/right edge of the swath also stays on
-- the field, not only the center
distanceToFieldEdge = distanceToFieldEdge - (workWidth / 2 / math.abs(math.tan(self.turnContext:getHeadlandAngle())))
self:debug('dzMax=%.1f, workWidth=%.1f, spaceNeeded=%.1f, turnEndForwardOffset=%.1f, distanceToFieldEdge=%.1f', dzMax, workWidth,
spaceNeededOnFieldForTurn, turnEndForwardOffset, distanceToFieldEdge)
local headlandAngle = self.turnContext:getHeadlandAngle()
distanceToFieldEdge = distanceToFieldEdge -
(headlandAngle > 0.0001 and (workWidth / 2 / math.abs(math.tan(headlandAngle))) or 0)
self:debug('dzMax=%.1f, workWidth=%.1f, spaceNeeded=%.1f, turnEndForwardOffset=%.1f, headlandAngle=%.1f, distanceToFieldEdge=%.1f', dzMax, workWidth,
spaceNeededOnFieldForTurn, turnEndForwardOffset, math.deg(headlandAngle), distanceToFieldEdge)
return spaceNeededOnFieldForTurn - distanceToFieldEdge
end

@@ -400,12 +407,13 @@ TowedDubinsTurnManeuver = CpObject(DubinsTurnManeuver)
function TowedDubinsTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius,
workWidth, steeringLength, distanceToFieldEdge)
self.debugPrefix = '(TowedDubinsTurn): '
local xOffset = turningRadius - AIUtil.getImplementRadiusFromTractorRadius(turningRadius, steeringLength)
self.vehicle = vehicle
local implementRadius = AIUtil.getImplementRadiusFromTractorRadius(turningRadius, steeringLength)
local xOffset = turningRadius - implementRadius
self.turnEndXOffset = turnContext:isLeftTurn() and -xOffset or xOffset
self:debug('Towed implement, offsetting turn end %.1f to accommodate tight turn ', xOffset)
self:debug('Towed implement, offsetting turn end %.1f to accommodate tight turn, implement radius %.1f ', xOffset, implementRadius)
AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius,
workWidth, steeringLength, distanceToFieldEdge)
--self:calculateTractorCourse(self.course)
end

function TowedDubinsTurnManeuver:calculateTractorCourse(course)