refactor: plow rotation logic unified

Author: Peter Vaiko

scripts/Course.lua

@@ -635,6 +635,29 @@ function Course:isNextTurnLeft(ix)
     end
 end
 
+--- Should the plow be rotated to the left at the waypoint ix?
+--- On the headland just check which side was worked last, on the center, check the direction of the next turn
+--- as at the first pass both sides are unworked and need some other indication on which side the plow should be
+function Course:shouldPlowBeOnTheLeft(ix)
+    local plowShouldBeOnTheLeft
+    if self:isOnHeadland(ix) then
+        local clockwise = self:isOnClockwiseHeadland(ix)
+        plowShouldBeOnTheLeft = not clockwise
+        CpUtil.debugVehicle(CpDebug.DBG_TURN, self.vehicle, 'On a headland (clockwise %s), plow should be on the left %s', tostring(clockwise), tostring(plowShouldBeOnTheLeft))
+    else
+        local isNextTurnLeft = self:isNextTurnLeft(ix)
+        if isNextTurnLeft == nil then
+            -- don't know if left or right, so just use the last worked side
+            plowShouldBeOnTheLeft = self:isLeftSideWorked(ix)
+            CpUtil.debugVehicle(CpDebug.DBG_TURN, self.vehicle, 'On the center, next turn direction unknown, plow should be on the left %s', tostring(plowShouldBeOnTheLeft))
+        else
+            plowShouldBeOnTheLeft = not isNextTurnLeft
+            CpUtil.debugVehicle(CpDebug.DBG_TURN, self.vehicle, 'On the center, plow should be on the left %s', tostring(plowShouldBeOnTheLeft))
+        end
+    end
+    return plowShouldBeOnTheLeft
+end
+
 function Course:getIxRollover(ix)
     if ix > #self.waypoints then
         return ix - #self.waypoints
@@ -1455,7 +1478,7 @@ end
 local function calculateYRot(waypoints, i)
     local x1, z1 = waypoints[i].x, waypoints[i].z
     local x2, z2 = waypoints[i + 1].x, waypoints[i + 1].z
-    if (x1 - x2) == 0 and (z1 - z2) == 0 then 
+    if (x1 - x2) == 0 and (z1 - z2) == 0 then
         -- Divide by zero fix ..
         return 0
     end
@@ -1810,13 +1833,13 @@ function Course.MultiVehicleData.createFromStream(stream, nVehicles)
 end
 
 function Course.MultiVehicleData.getAllowedPositions(nMultiToolVehicles)
-    if nMultiToolVehicles == 2 then 
+    if nMultiToolVehicles == 2 then
         return {-1,1}
-    elseif nMultiToolVehicles == 3 then 
+    elseif nMultiToolVehicles == 3 then
         return {-1,0,1}
-    elseif nMultiToolVehicles == 4 then 
+    elseif nMultiToolVehicles == 4 then
         return {-2,-1,1,2}
-    elseif nMultiToolVehicles == 5 then 
+    elseif nMultiToolVehicles == 5 then
         return {-2,-1,0,1,2}
     end
     return {0}

scripts/ai/controllers/PlowController.lua

@@ -12,7 +12,7 @@ function PlowController:init(vehicle, implement)
     -- the plow was not rotated into the working position before lowering. If that's the case, onLowering will
     -- rotate it but it needs to know which direction to lower, which, however is not known at that point
     -- anymore
-    self.lastTurnIsLeftTurn = CpTemporaryObject()
+    self.lastPlowSide = CpTemporaryObject()
 end
 
 
@@ -115,38 +115,38 @@ end
 -- TODO: this whole magic hack would not be necessary if we moved the actual lowering into onTurnEndProgress()
 function PlowController:onLowering()
     -- if we just turned (that is, not starting to work)
-    local lastTurnIsLeftTurn = self.lastTurnIsLeftTurn:get()
-    if lastTurnIsLeftTurn ~= nil and
+    local lastPlowSide = self.lastPlowSide:get()
+    if lastPlowSide ~= nil and
             self:isRotatablePlow() and not self:isFullyRotated() and not self:isRotationActive() then
-        self:debug('Lowering, rotating plow to working position (last turn is left %s).', lastTurnIsLeftTurn)
+        self:debug('Lowering, rotating plow to working position (plow in last turn was left %s).', lastPlowSide)
         -- rotation direction depends on the direction of the last turn
-        self.implement:setRotationMax(lastTurnIsLeftTurn)
+        self.implement:setRotationMax(lastPlowSide)
     end
 end
 
 --- This is called in every loop when we approach the start of the row, the location where
---- the plow must be lowered. Currently AIDriveStrategyFieldworkCourse takes care of the lowering,
+--- the plow must be lowered. Currently the WorkStarter takes care of the lowering,
 --- here we only make sure that the plow is rotated to the work position (from the center position)
 --- in time.
 ---@param workStartNode number node where the work starts as calculated by TurnContext
 ---@param reversing boolean driving in reverse now
 ---@param shouldLower boolean the implement should be lowered as we are close to the work start (this
 --- should most likely be calculated here in the controller, but for now, we get it from an argument
----@param isLeftTurn boolean is this a left turn?
-function PlowController:onTurnEndProgress(workStartNode, reversing, shouldLower, isLeftTurn)
-    self.lastTurnIsLeftTurn:set(isLeftTurn or false, 2000)
+---@param shouldBeOnTheLeft boolean should the plow be turned to the left to be in the good position after the turn?
+function PlowController:onTurnEndProgress(workStartNode, reversing, shouldLower, shouldBeOnTheLeft)
+    self.lastPlowSide:set(shouldBeOnTheLeft or false, 2000)
     if self:isRotatablePlow() and not self:isFullyRotated() and not self:isRotationActive() then
         -- more or less aligned with the first waypoint of the row, start rotating to working position
         if CpMathUtil.isSameDirection(self.implement.rootNode, workStartNode, 30) or shouldLower then
             if self.towed then
                 -- let towed plows remain in the center position while reversing to the start of the row
                 if not reversing then
                     self:debug('Rotating towed plow to working position.')
-                    self.implement:setRotationMax(isLeftTurn)
+                    self.implement:setRotationMax(shouldBeOnTheLeft)
                 end
             else
                 self:debug('Rotating hitch-mounted plow to working position.')
-                self.implement:setRotationMax(isLeftTurn)
+                self.implement:setRotationMax(shouldBeOnTheLeft)
             end
         end
     end

scripts/ai/strategies/AIDriveStrategyPlowCourse.lua

@@ -150,25 +150,8 @@ end
 --- Initial plow rotation based on the ridge marker side selection by the course generator.
 function AIDriveStrategyPlowCourse:rotatePlows()
     self:debug('Starting work: check if plow needs to be turned.')
-    -- on the headland just check which side was worked last, on the center, check the direction of the next turn
-    -- as at the first pass both sides are unworked and need some other indication on which side the plow should be
     local ix = self.ppc:getCurrentWaypointIx()
-    local plowShouldBeOnTheLeft
-    if self.course:isOnHeadland(ix) then
-        local clockwise = self.course:isOnClockwiseHeadland(ix)
-        plowShouldBeOnTheLeft = not clockwise
-        self:debug('On a headland (clockwise %s), plow should be on the left %s', tostring(clockwise), tostring(plowShouldBeOnTheLeft))
-    else
-        local isNextTurnLeft = self.course:isNextTurnLeft(ix)
-        if isNextTurnLeft == nil then
-            -- don't know if left or right, so just use the last worked side
-            plowShouldBeOnTheLeft = self.course:isLeftSideWorked(ix)
-            self:debug('On the center, next turn direction unknown, plow should be on the left %s', tostring(plowShouldBeOnTheLeft))
-        else
-            plowShouldBeOnTheLeft = not isNextTurnLeft
-            self:debug('On the center, plow should be on the left %s', tostring(plowShouldBeOnTheLeft))
-        end
-    end
+    local plowShouldBeOnTheLeft = self.course:shouldPlowBeOnTheLeft(ix)
     for _, controller in pairs(self.controllers) do
         if controller.rotate then 
             controller:rotate(plowShouldBeOnTheLeft)

scripts/ai/turns/AITurn.lua

@@ -1000,8 +1000,6 @@ function StartRowOnly:getDriveData()
                 TurnManeuver.LOWER_IMPLEMENT_AT_TURN_END) then
             self.state = self.states.APPROACHING_ROW
             self:debug('Approaching row')
-            self.driveStrategy:raiseControllerEvent(AIDriveStrategyCourse.onTurnEndProgressEvent,
-                    self:getLowerImplementNode(), self.ppc:isReversing(), true, not self.turnContext:isNextTurnLeft())
         end
     elseif self.state == self.states.APPROACHING_ROW then
         self.workStartHandler:lowerImplementsAsNeeded(self:getLowerImplementNode(), self.ppc:isReversing())

scripts/ai/turns/TurnContext.lua

@@ -89,7 +89,7 @@ function TurnContext:init(vehicle, course, turnStartIx, turnEndIx, turnNodes, wo
 
     self.dx, _, self.dz = localToLocal(self.turnEndWpNode.node, self.workEndNode, 0, 0, 0)
     self.leftTurn = self.dx > 0
-    self.nextTurnLeft = course:isNextTurnLeft(turnEndIx)
+    self.plowShouldBeOnTheLeft = course:shouldPlowBeOnTheLeft(self.turnEndWpIx)
     self:debug('start ix = %d, back marker = %.1f, front marker = %.1f',
             turnStartIx, self.backMarkerDistance, self.frontMarkerDistance)
 end
@@ -238,10 +238,6 @@ function TurnContext:getHeadlandAngle()
     return math.abs(CpMathUtil.getDeltaAngle(math.rad(self.turnEndWp.angle), math.rad(self.turnStartWp.angle)))
 end
 
-function TurnContext:isNextTurnLeft()
-    return self.nextTurnLeft
-end
-
 function TurnContext:isHeadlandCorner()
 	-- in headland turns there's no significant direction change at the turn start waypoint, as the turn end waypoint
 	-- marks the actual corner. In a non-headland turn (usually 180) there is about 90 degrees direction change at
@@ -282,6 +278,11 @@ function TurnContext:isLeftTurn()
     end
 end
 
+--- Should the plow be rotated to the left after the turn?
+function TurnContext:shouldPlowBeOnTheLeft()
+    return self.plowShouldBeOnTheLeft
+end
+
 ---@return number offset of the turn end in meters forward (>0) or back (<0)
 function TurnContext:getTurnEndForwardOffset()
     return self.dz

scripts/ai/turns/WorkStartHandler.lua

@@ -58,7 +58,7 @@ function WorkStartHandler:lowerImplementsAsNeeded(workStartNode, reversing)
         -- this must be called before the implement is lowered, for instance to rotate the plow before lowering it.
         -- ideally, this should all be in the PlowController since it is internal to the plow.
         self.driveStrategy:raiseControllerEvent(AIDriveStrategyCourse.onTurnEndProgressEvent,
-                workStartNode, reversing, shouldLowerThis, self.turnContext:isLeftTurn())
+                workStartNode, reversing, shouldLowerThis, self.turnContext:shouldPlowBeOnTheLeft())
         if reversing then
             dz = math.max(dz, thisDz)
             allShouldBeLowered = allShouldBeLowered and shouldLowerThis