256 lines
12 KiB
C#
256 lines
12 KiB
C#
using System.Drawing;
|
|
using System.Linq;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
|
|
using SimpleLite.RCS;
|
|
|
|
using SimpleLite.RCS.CarTypes;
|
|
using SimpleCore;
|
|
using SimpleCore.Compiler;
|
|
using SimpleCore.Library;
|
|
using SimpleCore.PropType;
|
|
using StandardScene.Coders;
|
|
|
|
namespace StandardScene.CarTypes
|
|
{
|
|
//钻车
|
|
[TemplateTrackCoderSettings(
|
|
priority = 18,
|
|
useVerb = "(dst.name=='FetchSite1'||dst.name=='FetchSite2')&&(plan.action=='fetch')",
|
|
templateString = "agv.Wait();agv.TireFollowing(${plan.TireNum},${plan.FrontLidarDetect},${src.id},${dst.id},${plan.FirstTire});agv.Wait();",
|
|
blockVerb = "true",
|
|
siteFields = typeof(MultiWheelLifterSiteFields),
|
|
trackFields = typeof(MultiWheelLifterTrackFields),
|
|
planFields = typeof(MultiWheelLifterPlanFields))]
|
|
|
|
[TemplateTrackCoderSettings(
|
|
priority = 18,
|
|
useVerb = "(dst.name=='PutSite1'||dst.name=='PutSite2')&&(plan.action=='leave')",
|
|
templateString = "agv.Wait();agv.TireFollowing(${plan.TireNum},${plan.FrontLidarDetect},${src.id},${dst.id},${plan.FirstTire});agv.Wait();",
|
|
blockVerb = "true",
|
|
siteFields = typeof(MultiWheelLifterSiteFields),
|
|
trackFields = typeof(MultiWheelLifterTrackFields),
|
|
planFields = typeof(MultiWheelLifterPlanFields))]
|
|
|
|
//离开时钻车(后出的那台车)
|
|
[TemplateTrackCoderSettings(
|
|
priority = 18,
|
|
useVerb = "dst.name=='ReadyFetchSite'&&plan.action=='leave'&&plan.Reverse==true",
|
|
templateString = "agv.Wait();agv.TireFollowing(${plan.TireNum},${plan.FrontLidarDetect},${src.id},${dst.id},${plan.FirstTire});agv.Wait();" +
|
|
"agv.LineTracking(${src.id},${dst.id},${plan.LineDistance});agv.Wait()",
|
|
blockVerb = "true",
|
|
siteFields = typeof(MultiWheelLifterSiteFields),
|
|
trackFields = typeof(MultiWheelLifterTrackFields),
|
|
planFields = typeof(MultiWheelLifterPlanFields))]
|
|
|
|
//离开时直线行走(先出的那台车)
|
|
[TemplateTrackCoderSettings(
|
|
priority = 18,
|
|
useVerb = "(dst.name=='LeaveSite'||dst.name=='ReadyFetchSite')&&plan.action=='leave'&&plan.Reverse==false",
|
|
templateString = "agv.Wait();agv.LineTracking(${src.id},${dst.id},${plan.LineDistance});agv.Wait();",
|
|
blockVerb = "true",
|
|
siteFields = typeof(MultiWheelLifterSiteFields),
|
|
trackFields = typeof(MultiWheelLifterTrackFields),
|
|
planFields = typeof(MultiWheelLifterPlanFields))]
|
|
|
|
//下使能+夹抱+上使能
|
|
[TemplateSiteCoderSettings(
|
|
priority = 18,
|
|
useVerb = "((dst.name=='FetchSite1'||dst.name=='FetchSite2')&&plan.action=='clamp')",
|
|
templateString = "agv.Wait();agv.DriverDisable();agv.Wait();agv.ClamptoTarget(${dst.ClampClose});agv.Wait();agv.DriverAble();",
|
|
blockVerb = "true",
|
|
siteFields = typeof(MultiWheelLifterSiteFields),
|
|
trackFields = typeof(MultiWheelLifterTrackFields),
|
|
planFields = typeof(MultiWheelLifterPlanFields))]
|
|
|
|
|
|
//抱夹打开
|
|
[TemplateSiteCoderSettings(
|
|
priority = 18,
|
|
useVerb = "(dst.name=='PutSite1'||dst.name=='PutSite2')&&plan.action=='put'",
|
|
templateString = "agv.Wait();agv.DriverDisable();agv.Wait();agv.ClamptoTarget(${dst.ClampClose});agv.Wait();agv.DriverAble();",
|
|
blockVerb = "true",
|
|
siteFields = typeof(MultiWheelLifterSiteFields),
|
|
trackFields = typeof(MultiWheelLifterTrackFields),
|
|
planFields = typeof(MultiWheelLifterPlanFields))]
|
|
|
|
|
|
//临时夹抱动作
|
|
[TemplateSiteCoderSettings(
|
|
priority = 18,
|
|
useVerb = "plan.action=='templateclamp'||plan.action=='templateput'",
|
|
templateString = "agv.Wait();agv.DriverDisable();agv.Wait();agv.ClamptoTarget(${plan.ClampClose});agv.Wait();agv.DriverAble();",
|
|
blockVerb = "true",
|
|
siteFields = typeof(MultiWheelLifterSiteFields),
|
|
trackFields = typeof(MultiWheelLifterTrackFields),
|
|
planFields = typeof(MultiWheelLifterPlanFields))]
|
|
|
|
[TemplateTrackCoderSettings(
|
|
priority = 2,
|
|
useVerb = "plan.action=='fetch'",
|
|
templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
|
"${track.id},${track.Speed},${track.Reverse||plan.Reverse}," +
|
|
"${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection},${track.MagnetChoose},${track.MultiVehicleSync},${track.typeInfo});",
|
|
blockVerb = "true",
|
|
siteFields = typeof(MultiWheelLifterSiteFields),
|
|
trackFields = typeof(MultiWheelLifterTrackFields),
|
|
planFields = typeof(MultiWheelLifterPlanFields))]
|
|
|
|
//[TemplateTrackCoderSettings(
|
|
// priority = 3,
|
|
// useVerb = "plan.action=='leave'&&(dst.name=='KeepDirectionSite1'||dst.name=='KeepDirectionSite2')",
|
|
// templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
|
// "${track.id},${track.Speed},${track.Reverse||plan.Reverse}," +
|
|
// "${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection},${track.MagnetChoose},${track.MultiVehicleSync},${track.typeInfo});",
|
|
// blockVerb = "true",
|
|
// siteFields = typeof(MultiWheelLifterSiteFields),
|
|
// trackFields = typeof(MultiWheelLifterTrackFields),
|
|
// planFields = typeof(MultiWheelLifterPlanFields))]
|
|
|
|
[TemplateTrackCoderSettings(
|
|
priority = 3,
|
|
useVerb = "plan.action=='leave'",
|
|
templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
|
"${track.id},${track.Speed},${track.Reverse||plan.Reverse}," +
|
|
"${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection},${track.MagnetChoose},${track.MultiVehicleSync},${track.typeInfo});",
|
|
blockVerb = "true",
|
|
siteFields = typeof(MultiWheelLifterSiteFields),
|
|
trackFields = typeof(MultiWheelLifterTrackFields),
|
|
planFields = typeof(MultiWheelLifterPlanFields))]
|
|
|
|
[TemplateTrackCoderSettings(
|
|
priority = 19,
|
|
useVerb = "dst.tag>0 && src.tag>0",
|
|
templateString = "agv.QrGo(${src.x},${src.y},${src.id},${src.tag},${dst.x},${dst.y},${dst.id},${dst.tag},${track.id}," +
|
|
"${track.Speed},${track.Reverse || (track.ReverseDst == dst.id)}," +
|
|
"${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection}," +
|
|
"${track.typeInfo});",
|
|
blockVerb = "true",
|
|
siteFields = typeof(MultiWheelLifterSiteFields),
|
|
trackFields = typeof(MultiWheelLifterTrackFields),
|
|
planFields = typeof(MultiWheelLifterPlanFields))]
|
|
|
|
// 通用避障/IO coder 已抽离为 StandardScene.Coders.*（见类前 [ProgramTrackCoderSettings] 引用）
|
|
|
|
// 避障区尺寸(4参,含中心点)切换 coder 已抽离为 StandardScene.Coders.AvoidanceParamCoder（见类前 [ProgramTrackCoderSettings] 引用）
|
|
|
|
// 纠偏阈值 coder 已抽离为 StandardScene.Coders.TrackingErrThreshCoder
|
|
|
|
[ProgramTrackCoderSettings(priority = 27, program = typeof(LidarAreaSwitchCoder))]
|
|
[ProgramTrackCoderSettings(priority = 20, program = typeof(AvoidanceDistanceCoder))]
|
|
[ProgramTrackCoderSettings(priority = 20, program = typeof(IoAreaSwitchCoder))]
|
|
[ProgramTrackCoderSettings(priority = 20, program = typeof(TrackingErrThreshCoder))]
|
|
[ProgramTrackCoderSettings(priority = 20, program = typeof(AvoidanceParamCoder))]
|
|
[EnvelopConfig(lengthX = 2000, lengthY = 2000, centerX = 0, centerY = 0)]
|
|
|
|
[CarType(Name = "多车联动AGV")]
|
|
public class MultiVehicleCar : GhostCar
|
|
{
|
|
[FieldMember] public float CarLength = 2000;
|
|
[FieldMember] public float CarWidth = 1400;
|
|
public static async Task<MultiVehicleCar> Create()
|
|
{
|
|
var car = new MultiVehicleCar()
|
|
{
|
|
lstatus = "连接中",
|
|
address = "127.0.0.1",
|
|
name = "联动AGV"
|
|
};
|
|
return car;
|
|
}
|
|
|
|
protected override void draw(Graphics eGraphics)
|
|
{
|
|
var HalfCarLength = CarLength / 2;
|
|
var HalfCarWidth = CarWidth / 2;
|
|
eGraphics.FillRectangle(Brushes.Gray, -HalfCarLength, -HalfCarWidth, HalfCarLength * 2, HalfCarWidth * 2);
|
|
eGraphics.DrawRectangle(Pens.White, -HalfCarLength, -HalfCarWidth, HalfCarLength * 2, HalfCarWidth * 2);
|
|
eGraphics.DrawLine(Pens.White, HalfCarLength - HalfCarWidth, -HalfCarWidth, HalfCarLength, 0);
|
|
eGraphics.DrawLine(Pens.White, HalfCarLength - HalfCarWidth, HalfCarWidth, HalfCarLength, 0);
|
|
}
|
|
|
|
[MethodMember(Name = "模拟车联动初始化")]
|
|
public void GhostMultiAgvSyncInit()
|
|
{
|
|
var car1 = (GhostCar)SimpleLib.GetAllCars().First(cc => cc.name == "sync-1");
|
|
while (true)
|
|
{
|
|
car1.ManualTeleport();
|
|
car1.x = 1770;
|
|
car1.y = 0;
|
|
car1.th = 0;
|
|
Thread.Sleep(100);
|
|
if (LessMath.dist(car1.x, car1.y, 1770, 0) < 1 && car1.th == 0) break;
|
|
}
|
|
car1.NoSchedule();
|
|
car1.siteID = -1;
|
|
car1.tags.Clear();
|
|
car1.status.usage.AddUsage("base", new CarUsage.CarUsageInfo() { scheduling = false, refreshing = false });
|
|
car1.fields["MultiVehicleSync"] = "True";
|
|
|
|
var car2 = (GhostCar)SimpleLib.GetAllCars().First(cc => cc.name == "sync-2");
|
|
while (true)
|
|
{
|
|
//car2.ManualTeleport(-750, 0, 180);
|
|
car2.ManualTeleport();
|
|
car2.x = -750;
|
|
car2.y = 0;
|
|
car2.th = 180;
|
|
Thread.Sleep(100);
|
|
if (LessMath.dist(car2.x, car2.y, -750, 0) < 1 && car2.th == 180) break;
|
|
}
|
|
var car2Site = SimpleLib.GetAllSites().OrderBy(ss => LessMath.dist(ss.x, ss.y, car2.x, car2.y)).First();
|
|
var syncInitSite = SimpleLib.GetSite(int.Parse(car2Site.fields["SyncInitId"]));
|
|
car2.TrafficReset(syncInitSite);
|
|
car2.siteID = syncInitSite.id;
|
|
car2.fields["MultiVehicleSync"] = "True";
|
|
}
|
|
|
|
[MethodMember(Name = "进入联动状态")]
|
|
public void StartMultiAgvSync()
|
|
{
|
|
var car1 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-1");
|
|
car1.NoSchedule();
|
|
car1.siteID = -1;
|
|
car1.tags.Clear();
|
|
car1.status.usage.AddUsage("base", new CarUsage.CarUsageInfo() { scheduling = false, refreshing = false });
|
|
car1.fields["MultiVehicleSync"] = "True";
|
|
|
|
var car2 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-2");
|
|
var car2Site = SimpleLib.GetAllSites().OrderBy(ss => LessMath.dist(ss.x, ss.y, car2.x, car2.y)).First();
|
|
var syncInitSite = SimpleLib.GetSite(int.Parse(car2Site.fields["SyncInitId"]));
|
|
car2.TrafficReset(syncInitSite);
|
|
car2.siteID = syncInitSite.id;
|
|
car2.fields["MultiVehicleSync"] = "True";
|
|
}
|
|
|
|
[MethodMember(Name = "退出联动状态")]
|
|
public void ExitMultiAgvSync()
|
|
{
|
|
var car1 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-1");
|
|
car1.fields["MultiVehicleSync"] = "False";
|
|
car1.Reset();
|
|
var car2 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-2");
|
|
car2.fields["MultiVehicleSync"] = "False";
|
|
car2.Reset();
|
|
}
|
|
|
|
//[MethodMember(Name = "返厂检修new", Description = "不再刷新小车状态，当然也不再被调度")]
|
|
//[I18N.DocumentTranslation(Name = "Blown new", Description = "Car no longer refresh status and scheduling", locale = "en")]
|
|
public new void Blown()
|
|
{
|
|
AppendDebug("ui-blown");
|
|
Diagnosis.Post($"Car {name}({id}) blown");
|
|
NoSchedule();
|
|
siteID = -1;
|
|
tags.Clear();
|
|
status.usage.AddUsage(
|
|
"base",
|
|
new CarUsage.CarUsageInfo { scheduling = false, refreshing = false }
|
|
);
|
|
lstatus = "返厂检修";
|
|
}
|
|
}
|
|
}
|