% 用二维数组代替地图和场地信息
% 可用场地：0
% 小车本身：1
% 货物点及入库点：2
% 地图边界: 100
% AGV出发区：11
% 监测区：12
% 充电区：13
% 生产区A1、A2：14
% 生产区B3、B4、B5：15
% 仓库1、4: 16
% 仓库2、3：17
% 注：以下坐标均在matlab的map矩阵中表示，下标是从[1,1]开始
% 需要注意一个问题 平常的x-y轴坐标表示方法与matlab的矩阵表示方法相反
% 四个入库点 入库A [4,16] [27,16] 入库B [16,4] [16,27]
% 基本假设：
% （1）AGV 的行驶速度为匀速；
% （2）AGV 在转弯时的用时固定；
% （3）AGV 从货架上叉取货物的用时固定；
% （4）AGV 从自身储位上放货物的用时固定；
% （5）不考虑 AGV 自身启动和制动对速度的影响；
% 定义：小车移动一个格子的时间为1s，转向花费的时间为1s
% 小车到达取货点时取货时间1s 卸货时间1s
global UnitTime TurningTime
global PickUpTime
global DrapTime
UnitTime = 1; TurningTime = 1;
PickUpTime = 1; DrapTime = 1;
%% 地图初始化
[AGVMapSize, AGVMap] = mapInit();
%% 向地图中添加场地信息 -- 出发区、充电区、生产地、仓库、监测区
AGVMap = addMapVenue(AGVMap);
%% 任务分配采用随机任务分配策略
x = rand(1,12) * (4-1) + 1; % 12个任务随机分配给3辆小车
%% 向地图中添加小车及货物信息 -- 3辆小车、12个货物、4个入库点
% 注：更改小车点和货物点后需要在函数addMapGoods、函数judgeCargoShelves、函数mapPlot里面进行对应修改
[AGVInitCoorPoint, AGVStartPoint, storageA, storageB, AGVMission, AGVMap] = addMapGoods(AGVMap, x);
AGVTime = [0;0;0]; % 记录三辆小车的总任务时间
numAGV = 3;
AGVPath = cell(1,numAGV); % AGV的初始化路径矩阵
maxRow = max([size(AGVMission{1},1), size(AGVMission{2},1), size(AGVMission{3},1)]); % 获取小车中的最大任务数
% AGVPath的第一列和第二列表示规划路径的横纵坐标
% 第三列表示是否发生转弯 0代表未发生转弯 1代表在此节点上发生了转弯
% 第四列表示花费总时间
% 第五列表示是空载还是负载 0代表空载 1代表负载低优先级任务 2代表负载高优先级任务
% 第六列表示是发生冲突的类型 1代表相交节点冲突 2代表相向节点冲突 3代表相向冲突
% 另注：如果小车不去对应的货物点取货，那么那个取货点对他来说一直是一个障碍物，卸货点也是一样的设置方式
for i = 1:maxRow
for j = 1:numAGV
% 执行顺序：小车1的任务1，小车2的任务1，小车3的任务1，小车1的任务2，小车2的任务2...
if i > size(AGVMission{j},1)
continue
else
fprintf('开始第%d个小车的第%d个货物的轨迹规划\n', j, i)
% 对应的小车去对应的货物取货
isTake = 1; % 是否取货
isSave = 0; % 是否卸货
start = AGVStartPoint(j,:);
goal = AGVMission{j}(i,:);
travelTime = AGVTime(j);
% 取货最优路径
AGVMap(goal(1), goal(2)) = 0; % 将目标货物点设置为可用场地
[AGVpickUpPath, ~, travelTime] = Astar(start, goal, AGVMapSize, AGVMap, 0, isTake, isSave, travelTime);
AGVPath{j} = [AGVPath{j};AGVpickUpPath]; % 将路径存储到路径矩阵中
AGVMap(goal(1), goal(2)) = 1; % 将目标货物点设置为不可用场地
% 卸货路径
start = goal;
travelTime = travelTime + PickUpTime; % 取货时间1s
% 判断货物对应的货架
[goal1, goal2] = judgeCargoShelves(start, storageA, storageB);
AGVMap(goal1(1), goal1(2)) = 0; % 将目标卸货点设置为可用场地
AGVMap(goal2(1), goal2(2)) = 0; % 将目标卸货点设置为可用场地
% 判断此时任务点离哪一个入库点更近 进行卸货
isTake = 0;
isSave = 1;
tmpTime = travelTime;
[AGVUnLoadPath1, ~, travelTime1] = Astar(start, goal1, AGVMapSize, AGVMap, 0, isTake, isSave, tmpTime);
[AGVUnLoadPath2, ~, travelTime2] = Astar(start, goal2, AGVMapSize, AGVMap, 0, isTake, isSave, tmpTime);
if travelTime1 < travelTime2
AGVPath{j} = [AGVPath{j};AGVUnLoadPath1]; % 将路径存储到路径矩阵中
travelTime = travelTime1;
start = goal1;
else
AGVPath{j} = [AGVPath{j};AGVUnLoadPath2]; % 将路径存储到路径矩阵中
travelTime = travelTime2;
start = goal2;
end
travelTime = travelTime + DrapTime; % 卸货时间1s
AGVMap(goal1(1), goal1(2)) = 1; % 将目标卸货点设置为不可用场地
AGVMap(goal2(1), goal2(2)) = 1; % 将目标卸货点设置为不可用场地
AGVTime(j) = travelTime;
% 如果已经执行完最后一个任务，则回到原始出发点
if i == size(AGVMission{j},1)
isTake = 1;
isSave = 0;
travelTime = AGVTime(j);
goal = AGVInitCoorPoint(j,:);
[AGVpickUpPath, ~, ~] = Astar(start, goal, AGVMapSize, AGVMap, 0, isTake, isSave, travelTime);
AGVPath{j} = [AGVPath{j};AGVpickUpPath]; % 将路径存储到路径矩阵中
AGVMap(goal(1), goal(2)) = 1; % 将静止的小车视为一个障碍物
else
% 此时代表一次任务完成，开始执行下一个小车的任务
AGVStartPoint(j,:) = start; % 更新出发点
end
end
end
end
% 对路径进行预处理 -- 对回到初始位置的小车置为高优先级,避免发生碰撞
AGVPath = preProcessing(AGVPath);
% 时间窗算法 规划出多AGV无冲突的路径
[AGVPath, sumTime] = timeWindow(AGVPath, AGVMap);
% 对时间窗算法规划后的路径进行处理 -- 细分路径和补齐长度
AGVPath = finalProcessing(AGVPath);
% 动态展示绘图效果
% 开始绘图
mapPlot(AGVPath, 1);
fprintf('总花费时间为%d秒\n', sumTime)
