Seems to work

Courseplay · Mar 27, 2022 · 3dcb46c · 3dcb46c
1 parent 5c7d8d8
commit 3dcb46c
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 5 deletions.
diff --git a/scripts/ai/AIDriveStrategyVineFieldWorkCourse.lua b/scripts/ai/AIDriveStrategyVineFieldWorkCourse.lua
@@ -28,7 +28,6 @@ function AIDriveStrategyVineFieldWorkCourse.new(customMt)
         customMt = AIDriveStrategyVineFieldWorkCourse_mt
     end
     local self = AIDriveStrategyFieldWorkCourse.new(customMt)
-    print('xxxxxxxxxxxxxxxxxx')
     return self
 end
 
@@ -50,12 +49,14 @@ function AIDriveStrategyVineFieldWorkCourse:getImplementLowerEarly()
 end
 
 function AIDriveStrategyVineFieldWorkCourse:startTurn(ix)
-    print('yyyyyyyyy')
 
     local _, frontMarkerDistance = AIUtil.getFirstAttachedImplement(self.vehicle)
     local _, backMarkerDistance = AIUtil.getLastAttachedImplement(self.vehicle)
+
     self:debug('Starting a turn at waypoint %d, front marker %.1f, back marker %.1f', ix, frontMarkerDistance, backMarkerDistance)
     self.ppc:setShortLookaheadDistance()
+
+
     self.turnContext = TurnContext(self.course, ix, ix + 1, self.turnNodes, self:getWorkWidth(),
             frontMarkerDistance, -backMarkerDistance, 0, 0)
     self.aiTurn = VineTurn(self.vehicle, self, self.ppc, self.turnContext, self.course, self.workWidth)

diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua
@@ -211,7 +211,6 @@ function AITurn:getDriveData(dt)
 	local gx, gz, moveForwards
 	if self.state == self.states.INITIALIZING then
 		local rowFinishingCourse = self.turnContext:createFinishingRowCourse(self.vehicle)
-		rowFinishingCourse:print()
 		self.ppc:setCourse(rowFinishingCourse)
 		self.ppc:initialize(1)
 		self.state = self.states.FINISHING_ROW

diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua
@@ -497,10 +497,20 @@ function VineTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turni
 	TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, 0)
 
 	self:debug('Start generating')
-	self:debug('r=%.1f, w=%.1f', turningRadius, workWidth)
 
 	local turnEndNode, startOffset, goalOffset = self.turnContext:getTurnEndNodeAndOffsets(0)
+	local _, _, dz = turnContext:getLocalPositionOfTurnEnd(vehicle:getAIDirectionNode())
+	if dz > 0 then
+		startOffset = startOffset + dz
+	else
+		goalOffset = goalOffset + dz
+	end
+	self:debug('r=%.1f, w=%.1f, dz=%.1f, startOffset=%.1f, goalOffset=%.1f',
+			turningRadius, workWidth, dz, startOffset, goalOffset)
 	local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver,
-			vehicleDirectionNode, startOffset, turnEndNode, 0, goalOffset, self.turningRadius)
+			-- always move the goal a bit backwards to let the vehicle align
+			vehicleDirectionNode, startOffset, turnEndNode, 0, goalOffset - turnContext.frontMarkerDistance, self.turningRadius)
 	self.course = Course.createFromAnalyticPath(self.vehicle, path, true)
+	local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, 0, false)
+	TurnManeuver.setLowerImplements(self.course, endingTurnLength, true)
 end