Alignment course fine tuning

Courseplay · Dec 29, 2021 · dcf843d · dcf843d
1 parent 5642add
commit dcf843d
Show file tree

Hide file tree

Showing 5 changed files with 6 additions and 4 deletions.
diff --git a/scripts/ai/AIDriveStrategyFieldWorkCourse.lua b/scripts/ai/AIDriveStrategyFieldWorkCourse.lua
@@ -71,7 +71,7 @@ function AIDriveStrategyFieldWorkCourse:start(course, startIx)
     if distance > 2 * self.turningRadius then
         self:debug('Start waypoint is far (%.1f m), use an alignment course to get there.', distance)
         alignmentCourse = AlignmentCourse(self.vehicle, self.vehicle:getAIDirectionNode(), self.turningRadius,
-                course, startIx, math.min(0, -self.frontMarkerDistance)):getCourse()
+                course, startIx, math.min(-2, -self.frontMarkerDistance)):getCourse()
     end
     if alignmentCourse then
         self.ppc:setShortLookaheadDistance()
@@ -384,7 +384,6 @@ function AIDriveStrategyFieldWorkCourse:onLastWaypointPassed()
         self:debug('Alignment to first waypoint ended, start work, first lowering implements.')
         self.state = self.states.WAITING_FOR_LOWER
         self:lowerImplements()
-        self.ppc:setNormalLookaheadDistance()
         self:startRememberedCourse()
     else
         self:debug('Last waypoint of the course reached.')

diff --git a/scripts/ai/CpMathUtil.lua b/scripts/ai/CpMathUtil.lua
@@ -42,7 +42,6 @@ end
 function CpMathUtil.getSeries(from, to, step)
 	local nValues = math.max(1, math.floor(math.abs((from - to) / step)))
 	local delta = (to - from) / nValues
-	CpUtil.info('%.1f %.1f %.1f', nValues, delta, step)
 	local value = from
 	local series = {}
 	for i = 0, nValues do

diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua
@@ -470,6 +470,8 @@ function AlignmentCourse:init(vehicle, vehicleDirectionNode, turningRadius, cour
 	print(course:getWaypointAngleDeg(ix))
 	local goal = State3D(x, -z, CourseGenerator.fromCpAngle(math.rad(course:getWaypointAngleDeg(ix))))
 
+	-- have a little reserve to make sure vehicles can always follow the course
+	turningRadius = turningRadius * 1.1
 	local solution = PathfinderUtil.dubinsSolver:solve(start, goal, turningRadius)
 
 	local alignmentWaypoints = solution:getWaypoints(start, turningRadius)

diff --git a/scripts/pathfinder/Dubins.lua b/scripts/pathfinder/Dubins.lua
@@ -46,7 +46,7 @@ function DubinsSolution:getLength(turnRadius)
 end
 
 function DubinsSolution:getWaypoints(start, turnRadius)
-    return dubins_path_sample_many(self.pathDescriptor, 2)
+    return dubins_path_sample_many(self.pathDescriptor, 1)
 end
 
 

diff --git a/scripts/test/CpMathUtilTest.lua b/scripts/test/CpMathUtilTest.lua
@@ -23,3 +23,5 @@ lu.assertItemsEquals(s, {1, 10})
 s = CpMathUtil.getSeries(10, 1, 100)
 lu.assertItemsEquals(s, {1, 10})
 
+s = CpMathUtil.getSeries(0, 10.5, 1)
+lu.assertItemsEquals(s, {0, 1.05, 2.1, 3.15, 4.2, 5.25, 6.3, 7.35, 8.4, 9.45, 10.5})
-Original file line number
+Diff line change
@@ Expand Up / @@ -46,7 +46,7 @@ function DubinsSolution:getLength(turnRadius) @@
     end
     function DubinsSolution:getWaypoints(start, turnRadius)
-        return dubins_path_sample_many(self.pathDescriptor, 2)
+        return dubins_path_sample_many(self.pathDescriptor, 1)
     end
@@ Expand Down @@