forked from Courseplay/courseplay
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmode7.lua
208 lines (203 loc) · 8.34 KB
/
mode7.lua
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
local abs, ceil = math.abs, math.ceil;
function courseplay:handleMode7(vehicle, cx, cy, cz, refSpeed, allowedToDrive)
-- backup protection
if vehicle.attachedCutters == nil then
--TODO local text "wrong worktool for this mode"
return false
end
local pipeState = courseplay:getTrailerInPipeRangeState(vehicle);
if not vehicle.cp.makeHeaps then
if pipeState > 0 then
vehicle:setPipeState(pipeState);
elseif not vehicle.aiIsStarted then
vehicle:setPipeState(1);
end;
end
if (vehicle.cp.waypointIndex == vehicle.cp.numWaypoints and vehicle.cp.modeState ~= 5) or (vehicle.cp.mode7GoBackBeforeUnloading and vehicle.cp.modeState ~= 5) then
if vehicle.cp.curTarget.x ~= nil then
courseplay:setModeState(vehicle, 5);
courseplay:debug(nameNum(vehicle) .. ": " .. tostring(debug.getinfo(1).currentline) .. ": modeState = 5", 11);
else
allowedToDrive = false;
--TODO local text "no aithreshing"
end
end
-- AutoCombine
local isAutoCombine = false;
if vehicle.acParameters ~= nil and vehicle.acParameters.enabled then
isAutoCombine = true;
end
-- wait untill fillLevel is reached
if vehicle.aiIsStarted then
if vehicle.cp.totalFillLevelPercent >= vehicle.cp.driveOnAtFillLevel then
local cx7, cz7 = vehicle.Waypoints[vehicle.cp.numWaypoints].cx, vehicle.Waypoints[vehicle.cp.numWaypoints].cz;
local cty7 = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, cx7,1, cz7)
local lx7, lz7 = AIVehicleUtil.getDriveDirection(vehicle.cp.DirectionNode, cx7, cty7, cz7);
local x7,y7,z7 = localToWorld(vehicle.cp.DirectionNode, 0, 0, -30);
local x7d,y7d,z7d = localToWorld(vehicle.cp.DirectionNode, 0, 0, -15);
vehicle.cp.mode7t = {};
vehicle.cp.mode7t.x = x7;
vehicle.cp.mode7t.y = y7;
vehicle.cp.mode7t.z = z7;
vehicle.cp.mode7d = {};
vehicle.cp.mode7d.x = x7d;
vehicle.cp.mode7d.y = y7d;
vehicle.cp.mode7d.z = z7d;
local fx,fy,fz = 0,0,0;
local isField = true;
for i = 0.5, 3 do
fx,fy,fz = localToWorld(vehicle.cp.DirectionNode, 0, 0, -i*vehicle.cp.turnDiameter);
if not courseplay:isField(fx, fz) then
isField = false;
break;
end
end
if isField or vehicle.cp.totalFillLevelPercent > 99 then
local dx, _, dz = localDirectionToWorld(vehicle.cp.DirectionNode, 0, 0, 1)
local length = Utils.vector2Length(dx, dz)
vehicle.lastaiThreshingDirectionX = dx / length;
vehicle.lastaiThreshingDirectionZ = dz / length;
vehicle:stopAIVehicle(AIVehicle.STOP_REASON_USER);
if vehicle.cp.hasDriveControl and vehicle.cp.driveControl.hasManualMotorStart then
vehicle.driveControl.manMotorStart.wasHired = false
end
vehicle.cp.shortestDistToWp = nil;
vehicle.cp.nextTargets = {};
local sideOffset = math.max(0.34*3*vehicle.cp.turnDiameter,vehicle.cp.workWidth);
courseplay:debug(nameNum(vehicle) .. ": sideOffset = "..tostring(sideOffset), 11);
if lx7 < 0 then
courseplay:debug(nameNum(vehicle) .. ": approach from right", 11);
vehicle.cp.curTarget.x, vehicle.cp.curTarget.y, vehicle.cp.curTarget.z = localToWorld(vehicle.cp.DirectionNode, -sideOffset , 0, -3*vehicle.cp.turnDiameter);
courseplay:addNewTargetVector(vehicle, sideOffset , 0);
courseplay:addNewTargetVector(vehicle, 0 ,3.5);
else
courseplay:debug(nameNum(vehicle) .. ": approach from left", 11);
vehicle.cp.curTarget.x, vehicle.cp.curTarget.y, vehicle.cp.curTarget.z = localToWorld(vehicle.cp.DirectionNode, sideOffset , 0, -3*vehicle.cp.turnDiameter);
courseplay:addNewTargetVector(vehicle, -sideOffset , 0);
courseplay:addNewTargetVector(vehicle, 0 ,3.5);
end
vehicle.cp.mode7Unloading = true;
vehicle.cp.mode7GoBackBeforeUnloading = true;
vehicle.cp.mode7SpeedBackup = vehicle.cruiseControl.maxSpeed
courseplay:start(vehicle);
courseplay:setWaypointIndex(vehicle, 1);
else
if courseplay.debugChannels[11] then
local dbgctx7, dbgcty7, dbgctz7 = getWorldTranslation(vehicle.cp.DirectionNode);
local dbgcx, _, dbgcz = localToWorld(vehicle.cp.DirectionNode, 0 , 0, -3*vehicle.cp.turnDiameter);
drawDebugLine(dbgctx7, dbgcty7+3, dbgctz7, 1, 1, 1, dbgcx, dbgcty7+3, dbgcz, 1, 1, 1);
end
return false;
end
else
return false;
end
-- go back for 15m
elseif vehicle.cp.mode7Unloading then
refSpeed = vehicle.cp.speeds.field;
if vehicle.cp.mode7GoBackBeforeUnloading then
refSpeed = vehicle.cp.speeds.turn;
local dist = courseplay:distanceToPoint(vehicle, vehicle.cp.mode7d.x,vehicle.cp.mode7d.y,vehicle.cp.mode7d.z);
if dist < 1 then
vehicle.cp.mode7GoBackBeforeUnloading = false;
courseplay:setWaypointIndex(vehicle, 2);
courseplay:setModeState(vehicle, 0);
courseplay:debug(nameNum(vehicle) .. ": " .. tostring(debug.getinfo(1).currentline) .. ": modeState = 0", 11);
end
end
--finished work
else
allowedToDrive = false
CpManager:setGlobalInfoText(vehicle, 'WORK_END');
end
--go to course
local continue = true
if vehicle.cp.modeState == 0 then
if vehicle.cp.waypointIndex ==2 then
refSpeed = vehicle.cp.speeds.field;
else
refSpeed = vehicle.cp.speeds.street;
end
--follow waypoints
elseif vehicle.cp.modeState == 5 then
local targets = #(vehicle.cp.nextTargets);
local aligned = false;
local ctx7, cty7, ctz7 = getWorldTranslation(vehicle.cp.DirectionNode);
if vehicle.cp.mode7GoBackBeforeUnloading then
cx = vehicle.cp.mode7t.x;
cy = vehicle.cp.mode7t.y;
cz = vehicle.cp.mode7t.z;
else
cx = vehicle.cp.curTarget.x;
cy = vehicle.cp.curTarget.y;
cz = vehicle.cp.curTarget.z;
end
courseplay:setInfoText(vehicle, string.format("COURSEPLAY_DRIVE_TO_WAYPOINT;%d;%d",cx,cz));
if courseplay.debugChannels[11] then
drawDebugPoint(cx, cy+3, cz, 0, 1 , 1, 1);
drawDebugLine(cx, cty7+3, cz, 1, 0, 0, ctx7, cty7+3, ctz7, 1, 0, 0);
end;
if not vehicle.cp.mode7GoBackBeforeUnloading then
refSpeed = vehicle.cp.speeds.field;
local distance_to_wp = courseplay:distanceToPoint(vehicle, cx, cy, cz);
local distToChange = 4;
if vehicle.cp.shortestDistToWp == nil or vehicle.cp.shortestDistToWp > distance_to_wp then
vehicle.cp.shortestDistToWp = distance_to_wp;
end
if distance_to_wp > vehicle.cp.shortestDistToWp and distance_to_wp < 6 then
distToChange = distance_to_wp + 1;
end
if targets == 2 then
vehicle.cp.curTargetMode7 = vehicle.cp.nextTargets[2];
if distance_to_wp < 25 then
refSpeed = vehicle.cp.speeds.turn;
end
elseif targets == 1 then
if isAutoCombine then
aligned = true;
courseplay:debug(nameNum(vehicle) .. ": AC aligned", 11);
end
if abs(vehicle.lastaiThreshingDirectionZ) > 0.1 then
if abs(vehicle.cp.curTargetMode7.x-ctx7)< 3 then
aligned = true;
courseplay:debug(nameNum(vehicle) .. ": aligned", 11);
end
else
if abs(vehicle.cp.curTargetMode7.z-ctz7)< 3 then
aligned = true;
courseplay:debug(nameNum(vehicle) .. ": aligned", 11);
end
end
refSpeed = vehicle.cp.speeds.turn;
elseif targets == 0 then
refSpeed = vehicle.cp.speeds.turn;
if distance_to_wp < 10 and not isAutoCombine then
vehicle:setIsTurnedOn(true);
courseplay:debug(nameNum(vehicle) .. ": setIsTurnedOn()", 11);
end
end
if distance_to_wp < distToChange or aligned then
vehicle.cp.shortestDistToWp = nil;
if targets > 0 then
courseplay:setCurrentTargetFromList(vehicle, 1);
courseplay:debug(nameNum(vehicle) .. ": set next Target from List", 11);
else
courseplay:setModeState(vehicle, 0);
courseplay:debug(nameNum(vehicle) .. ": " .. tostring(debug.getinfo(1).currentline) .. ": modeState = 0", 11);
if vehicle.lastaiThreshingDirectionX ~= nil then
vehicle.aiThreshingDirectionX = vehicle.lastaiThreshingDirectionX;
vehicle.aiThreshingDirectionZ = vehicle.lastaiThreshingDirectionZ;
courseplay:debug(nameNum(vehicle) .. ": restored vehicle.aiThreshingDirection", 11);
end
vehicle:startAIVehicle();
vehicle:setCruiseControlMaxSpeed(vehicle.cp.mode7SpeedBackup)
continue = false
vehicle.cp.mode7Unloading = false;
courseplay:debug(nameNum(vehicle) .. ": start AITreshing", 11);
courseplay:debug(nameNum(vehicle) .. ": fault: "..tostring(ceil(abs(ctx7-vehicle.cp.curTargetMode7.x)*100)).." cm X "..tostring(ceil(abs(ctz7-vehicle.cp.curTargetMode7.z)*100)).." cm Z", 11);
end
end
end
end
return continue, cx, cy, cz, refSpeed, allowedToDrive;
end;