使用手册 - MDCS适配详细说明

一、介绍

MDCS（Medulla,Detour,Clumsy ,Simple ）是一套完整的 AGV（Automated Guided Vehicle）小车自动驾驶模型与控制架构，它由以下 4 个软件组成：  Medulla：硬件适配软件，针对不同的硬件进行通讯以及驱动适配；  Detour：AGV 导航定位软件，可以进行 SLAM、二维码或 UWB 定位等；  Clumsy：自动驾驶软件，定义了 AGV 的基本动作单元，例如转向/差动驱动控制，以 及取放料等动作；  Simple：AGV 调度软件，进行多车的任务调度和路径规划。 请按照以下步骤进行适配：

==== 第一步，适配底盘--Medulla ====

一，根据通讯协议，我们先了解该车具备哪些硬件? 本适配工程硬件如下：IO 模块(modbus Tcp),电机驱动(can 总线),电池（串口 485）

二、工程的组成

工程的组成主要有：设备定义类为{proName}Cart.cs 还有其他 3 种主要的外设类，采用轮询 方式，分别控制 IO、电机、音乐和电池。命名为 IORoutine,MotorRoutine 和 MusicRoutine 和 BatteryRoutine，周期可以根据设备要求进行配置。 //梯形图逻辑，scanInterval 为扫描周期 [UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 20)] [UseLadderLogic(logic = typeof(IORoutine), scanInterval = 100)] [UseLadderLogic(logic = typeof(BatteryRoutine), scanInterval = 1000)] [UseLadderLogic(logic = typeof(MusicLightRoutine), scanInterval = 100)]

三、基本概念

AsUpperIO:指 Clumsy 或遥控器等指定的属性，如下发速度，角度，避障区域等 AsLowerIO:指从硬件读取的信号属性，如按钮，灯光，音乐，编码器值等 IOObjectMonitor:用于界面显示供监控的变量

四 、进入适配

了解以上概念，我们开始进入适配：

1,定义基础变量

分为与 Clumsy 交互的变量，IO 硬件控制的输入输出变量，编码器变量，通讯硬件变量，报 警变量，需要显示或中转使用的变量，具体可以根据项目协议表进行编写，如下： [AsUpperIO(desc = "下发速度(0-1000)", timeOutReset= true)] public float velocity; [AsUpperIO(desc = "下发方向盘角度", timeOutReset = true)] public float theta; [AsLowerIO(desc = "实际方向盘角度")] public float actualTh; [AsLowerIO(desc = "实际速度")] public float actualV; [AsLowerIO(desc = "举升控制")] public int lift=0; [AsLowerIO(desc = "货叉高度控制")] public int liftHeight; [AsLowerIO(desc = "启动")] public int start; [AsLowerIO(desc = "停止")] public int stop; [AsLowerIO(desc = "复位")] public int reset; [AsLowerIO(desc = "急停")] public int emergencyStop;

===== 2，适配 IO 类 =====

2.1 根据协议，此处我们使用 Modbus TCP 协议，驱动已经写好 ，直接 使用即可，首先初始化 ModbusTCP 服务，在 IORoutine 中进行编写,如下图 ModbusTCP mtcp = new ModbusTCP(); mtcp.start("192.168.127.1",502);//初始化 ModbusTCP 服务 2.2 初始化服务后，我们开始具体 IO 变量的读取和赋值。 var read = mtcp.registerBuffer(1, 0x0000, 1);//站号 起始地址 读取长度 var readData = read[0]; cart.start= readData >> 0 == 1 ? 1 : 0; cart.stop= readData >> 1 == 1 ? 1 : 0; ........ 2.3 将所有 IO 适配后进行测试看读取和写入是否正确，然后我们进入下一步

3，适配电机驱动--重要

3.1 根据 CAN 协议，确认电机控制方法，此项目协议如下示范协议\CAN 协议.xlsxx，在 MotorRoutine 中进行编写。

3.2 根据协议，将速度以及角度通过报文下发给驱动器 public ZLGCAN can = new ZLGCAN(); public override void Operation() { var sendV = cart.velocity; var sendTh = cart.theta; can.sendMessage1(0x1a6, new byte[8] { (byte) (sendV > 0 ? 0x3 : 0x5), 0, (byte) sendV, (byte) (((int) sendTh * 10) & 0xff), (byte) (((int) -sendTh * 10) >> 8), 10, 10, 255 }); if (can.canRecv.TryGetValue(0x226, out var tup1)) { cart.actualV = BitConverter.ToInt16(tup1.Item1, 2); cart.actualTh = -BitConverter.ToInt16(tup1.Item1, 4) * 0.01f; } } 3.3 这一步完成了，我们就可以初步进行小车行走测试，通过 medulla 遥控器，在 SteerWheelCart 里面配置 //设置 MedullaAPI，使得 clumsy 等系统可以直接设置 IO 变量 [UseMedullaAPI(name = "move", UpperIOs = new[] {nameof(velocity), nameof(theta) })] 这时候我们要进行关键的一步，见文档示范协议\MDCS 部署程序配置.pdf 看 medulla 配置即 可 我们需要保证，下发-90° 驱动器实际反馈也是-90,并且实际舵轮转向也是左转 90°,下发线 速度，反馈的速度也应该和下发一致。 到这里 Medulla 的主要工作已经完成了，我们继续下面的其他硬件适配！

4，适配电池

4.1 此处我们使用了 485 通讯，因此我们要实例化 SerialPort，并且调用我们写的初始化串口 方法，传入需要开启的端口以及端口波特率 public static SerialPort serialPort; initSerialPort("COM1", 9600);//初始化 COM1 端口，波特率 9600 4.2，端口打开后，我们就根据具体的电池协议进行报文的发送和读取了-协议

4.3 如下图 根据协议，我们获取电池电压和电量 byte[] ToReadBytes = new byte[8] { 0x01, 0x03, 0x00, 0x00, 0x00, 0x23, 0x04, 0x13 }; if (serialPort.IsOpen) { serialPort.BaseStream.Write(ToReadBytes, 0, ToReadBytes.Length);//写数据 byte[] respLine = stringToHex(serialPort.ReadLine()); if (respLine.Length < 20) throw new Exception($"wrong ER response: {respLine}"); cart.voltage = (respLine[1] & 0xFF) << 8 | respLine[0];//电压 cart.soc = respLine[13];//电量 } 至此，电池就适配好了，我们继续下一步，声光以及按钮的逻辑编写

5，声光按钮

5.1 我们将声光报警分成这几个，然后我们针对这些模式，赋予不同的灯光以及音乐即可 var mode = 0; if (cart.velocity != 0) mode = 1;//运动状态模式 1 if (cart.soc < 25) mode = 2;//低电压模式 2 if (cart.obstacleStop1==1 ) mode = 3;//近避障模式 3 if (cart.IsFault() || cart.com_can1 != 0 || cart.com_can2 != 0) mode = 4;//报警模式 4 if (cart.lightFlash) mode = 5;//声光报警模式 5 5.2 我们根据小车运动状态，将按钮逻辑写进程序里，保证按下启动按钮后，小车可以进行 移动，复位按钮可以复位报警或状态，停止或急停按钮可以停止小车，并且将报警状态显示 在监控画面，具体可参考代码。

6，雷达数据的测试

可参考示范协议以及说明\MDCS 部署程序配置.pdf，其他品牌雷达的驱动程序可以找我们 要，目前市面大部分雷达都已有适配驱动。 至此，我们 Medulla 部分完成，进入下一部分，Detour 的使用

第二步，使用 Detour 定位软件-单线雷达

一、Detour 基础知识

1、Detour-车体编辑器

概念 3-1-1 车体编辑器，是一个用于编辑车体模型，激光参数的配置工具。 概念 3-1-1 车体编辑器-单线雷达位置(x,y,th)，表示雷达相对驱动中心位置，mm 单位。 概念 3-1-2 车体编辑器-单线雷达角度方向(angleSgn：±1)，表示雷达角度方向，如雷达倒 装情况。 概念 3-1-1 车体编辑器-单线雷达起始角度(rangeStartAngle)，表示雷达开始扫描的角度值。 概念 3-1-2 车体编辑器-单线雷达结束角度(rangeEndAngle)，表示雷达结束扫描的角度值。 概念 3-1-3 车体编辑器-单线雷达扫描开始距离(ignoreDist)，用于屏蔽激光周围杂点或车体 部分垃圾数据，mm 单位。 概念 3-1-4 车体编辑器-单线雷达最大扫描距离(maxDist)，表示雷达扫描最大半径，mm 单 位。 1.1 如果我们有其他外设，均可在添加中添加并且配置

2，Detour-detour.json 常用参数配置

2.1 我们先打开 Detour 软件，然后界面左上角，导航配置，选择保存到 Detour 文件夹下， 命名 detour 即可，会保存一个 json 配置文件，里面存储了 Detour 初始的参数，有些我们需 要设置下会方便调试如下

2.2 具体常用的 配置参数如下，我们先打开 detour.json 文件（右击记事本打开） "recordLastPos": true, //代表自动记录上一个位置信息，可以用于断电后或 detour 关闭后， 再次打开 detour 会自动定位小车断电前的或关闭前记录的位置信息，可以省去重复的手动 定位操作 "autoStart": true,//表示打开软件是否自动启动激光，代替如下图按钮

二，Detour 基础设置

1、车体绘制

为什么要进行车体绘制？ 过滤车体部分的激光数据，以免影响后期建图 1.1 我们打开 Detour-概览-车体布局-选择默认车体边框，如果为红色说明选中，如下

2，我们选择重绘值轮廓，在车体编辑器右上角，双击，然后我们我们根据车体大小 绘制出 车体轮廓即可，下图由于离线，所以无法看到激光数据，正常是可以看到激光数据的，我们 尽量让激光车体周围没有杂点即可。

3，激光参数

3.1 完成车体绘制后，我们选中激光，右侧会刷新出属性，我们需要修改的主要是 xyth 参数， 以及 angleSgn 用于指示激光雷达扫描方向，1 为逆时针扫描，-1 为顺时针扫描。endAngle， 为激光雷达扫描的最后一个点的角度。这两个参数必须配置，常见的雷达配置如 下： 1. 倍加福、万集、科力、西克 Nano 系列雷达， angleSgn=1, endAngle=180 2. 星秒雷达：angleSgn=1，endAngle=0 3. 西克 LMS 系列雷达，angleSgn=1, endAngle=270 3.2 激光物体参数填写完成后，我们需要对激光再次进行二次标定，这个标定是用来修正机 械安装误差

三，Detour 建图方法

请参考示范协议以及说明\MDCS 建图和路线绘制.pdf

=

=====四，Detour 定位接口 =====

1，暂停 Detour 地图匹配功能 1.1 此功能主要用于，特殊位置由于变化比较大，防止带飞小车定位，我们在小车不运动的 时候关闭实时匹配功能。 http://{ip}:4321/switchPosMatch?disabled=true //true 是关闭匹配，false 是打开 2，重定位 2.1 此工主要用于现场加入丢失定位，现场人员不会操作 AGV，可以把小车开到复位点，通 过按钮或者触摸屏进行该点重定位。 http://{ip}:4321/setLocation?x=11&y=22&th=33

第三步，适配自动驾驶系统--Clumsy

Clumsy 是小车的运动控制组件。开发者需要做的是：底盘性能测试，定义运动控制的输入 输出变量、定义参数表、定义动作、定义动作测试例程，并提供调度接口。

一，部署 Clumsy 以及测试优化底盘性能

1,部署 Clumsy 请参考示范协议以及说明\MDCS 部署程序配置.pdf-Clumsy 配置

1 定义车型

1.1 首先我们在{proName}Def.cs 文件中，因为底盘是单舵轮，所以定义 SwSteering 类，并且 继承 SteeringWriterClass，然后重写 WriteSteering 方法获取算法速度 public override void WriteSteering(float angle, float velocity)//重写 WriteSteering 获取 算法速度 { SWDef.self.velocity = velocity; SWDef.self.th = angle; } 1.2 车型定义文件还需要重写 Init 方法，这个方法会在 Clumsy 启动时调用。 public override void Init() { self = this;设置 self=this，//使得其它类可以访问 SWDef 的实例 ClumsyLib.Capture();//调用 ClumsyLib.Capture();从而启动 Clumsy 的传感器采集功能 } 1.3 此外还需要重写 DriveStop 方法。 public override void DriveStop() { velocity = 0; th = 0; }

2 定义与 Medulla 交互的变量

2.1 MedullaAdapter 工程中设备定义类 SWDefCart.cs 中摘抄标记了 AsUpperIO 和 AsLowerIO 的字段。这些字段中，AsLowerIO 字段会自动更新，AsUpperIO 字段会自动下发,因此我们将 需要交互的变量复制过来即可。 public static SWDef self; public static MedullaInterface mInterface = new MedullaInterface(); [AsUpperIO(desc = "下发速度(0-1000)", timeOutReset = true)] public float velocity; [AsUpperIO(desc = "下发方向盘角度", timeOutReset = true)] public float th; [AsLowerIO(desc = "实际方向盘角度")] public float actualTh; [AsLowerIO(desc = "实际速度")] public float actualV; [AsUpperIO(desc = "货叉移动方向")] public int lift; [AsLowerIO(desc = "电池充电接触器")] public int chargeContactor; [AsLowerIO(desc = "货叉位置")] public int liftPos; [AsLowerIO(desc = "货叉位置")] public bool getItem; [AsLowerIO(desc = "举升目标位置")] public int liftTarget; [AsLowerIO(desc = "障碍物减速区，为 0 时减速")] public int obstacleRetard; [AsLowerIO(desc = "障碍物停止区 1，为 0 时停止")] public int obstacleStop1; [AsLowerIO(desc = "障碍物停止区 2，为 0 时停止")] public int obstacleStop2; 这部分完成后，我们就开始进行自动功能的测试

三 定义动作

1，定义一个货叉举升下降的动作 动作我们定义在 Movement.cs 文件里面，里面已有一些常用的动作，我们先从简单的开始： 1.1，货叉举升的动作使用 MovementDefinition 来定义。该类的 Get 方法应是一个 Generator 函数，通过 yield return true 来表示动 作计算完毕待下发，释放 CPU 资源并等到下一个周期 再继续计算动作。这种方法可以使得动作编排足够紧凑，不需要复杂的状态机设计即可完成 复杂的动作序列。通过 Get()方法返回一个 IEnumerable <bool> 对象后，可产生一个 DriveTask 类进行动作执行。例子如下： class LiftFork : MovementDefinition { public int liftTarget = 0; public int liftStatu = 0; public override IEnumerable<bool> Get() { Console.WriteLine($"Start lifting to {liftTarget}"); SWDef.self.liftTarget = liftTarget; //根据货叉目标位置判断提升还是下降，并且记忆其状态 if (SWDef.self.liftPos < SWDef.self.liftTarget) { SWDef.self.lift = 1; liftStatu = 1; } else if (SWDef.self.liftPos >= SWDef.self.liftTarget) { SWDef.self.lift = -1; liftStatu = -1; } //如果目标位置和当前位置在 10mm 以内，循环跳出 while (Math.Abs(SWDef.self.liftPos - liftTarget) > 10) yield return true; SWDef.self.lift = 0;//货叉动作使能关闭 SWDef.self.liftPos = liftStatu;//给定到位状态 Thread.Sleep(500); Console.WriteLine($"Done lifting to {liftTarget}"); } } 1.2 以上我们一个货叉的动作就写好了，下面我们进行这个动作的测试

四 定义动作测试例程

1，定义一个动作测试的流程 1.1 我们定义一个 ForkTest 类，继承 MovementTest，动作分为开始测试和停止，我们重写 TestStop 以及 Test 方法，Test 中我们先调出 UI 供输入货叉提升高度值，然后新建一个 DriveTask 任务，排列到任务中，进行执行，等到 LiftFork 中的 Get 方法完成返回 true ，任务结束。 class ForkTest : MovementTest { private DriveTask dt; public override void TestStop() { dt?.Stop(); } public override void Test() { //调出 UI 输入货叉提升高度值 if (InputBox.ShowDialog("input fork height") != DialogResult.OK) return; dt = new DriveTask(new LiftFork() { liftTarget=Convert.ToInt32(InputBox.ResultValue) }.Get()); } } 1.2，进行测试，我们重新编译 clumsy，在目录 build\clumsy 找到其生成的 dll，放到 clumsy.exe 目录下，我们打开 clsumy.exe，可以看到界面上有个“测试货叉起升”的按钮

我们点击即可进行动作测试，如果需要停止点击 即可。 1.3 一个简单的动作测试我们就做好了，如果小车有其他功能也可以按照这个方法进行编 写。 注意：如果 clumsy 打开出现界面空白，后台没有加载插件或者加载其他插件失败请，点击 车辆配置，选择驱动，保存配置，再重启 clumsy 即可。

五 调度（Simple）接口编写

1 编写一个充电的接口

1.1 这一部分我们在 AGV.c 文件中编写，这一块我们只用编写具体需要执行的方法，用来承 接 Clumsy 和 Medulla 之间逻辑，供调度使用 1.2 我们也从简单的开始，有个需求我们需要在特定位置进行小车的充电工作，我们定义一 个方法 Charge,其中参数 isCharge 由调度控制，如下 public void Charge(bool isCharge) { SWDef.self.chargeContactor = isCharge ? 1 : 0; Console.WriteLine($"chargeContactor update to-{SWDef.self.chargeContactor}"); } 1.3 是不是很简单，将参数值赋给和 Medulla 交互的变量 chargeContactor 即可完成对充电口打开的 功能，如果有其他方法，比如切换障碍物区域控制音乐曲目，设置货叉高度等都可以这么实现。 第四步，Simpe 的适配 关于 Simpe 软件如何使用，可以参考示范协议以及说明\MDCS 建图和路线绘制.pdf 中路线绘 制内容，本适配主要是在已经熟悉 Simpe 基础操作的基础上进行具体场景的适配以及业务开 发

一，{proName}Car.cs 类中小车的动作编写

1 实现小车类

1.1 我们在 DemoCar.cs 中创建一个 DemoCar 并且继承 ClumsyCar，并且在 DemoCar()方法中放置 我们需要执行的方法，用 Create()生成一个小车，给小车初始属性 public class DemoCar : ClumsyCar { static DemoCar() { //初始化小车需要实现的方法 } public static async Task<DemoCar> Create() // boilerplate { var fl = new DemoCar() { lstatus = "连接中", address = "127.0.0.1", name = $"叉车底盘", speed = 50, haveCoordination = true }; return fl; } }

2 编写小车动作

2.1 如果我想把所有线段都增加一个默认的 speed 标记，对于小场景可以手动添加，对于大 场景，我们可以动态的进行添加。 2.2 对于小车动作，我们带上 MethodMember 的标记，会显示在界面-车辆-选择小车-动作里面 [MethodMember(name = "增加 speed 标记", desc = "双击增加 speed 标记")] public void AddSpeed() { //获取场景中所有的路线进行遍历 foreach (var tracks in SimpleLib.GetAllTracks()) { //判断路线是否有 speed 字段，如果有不添加 if (!tracks.fields.ContainsKey("speed")) { //给所有的线段加上 100 的速度，当前也可以根据业务逻辑等灵活使用 tracks.fields.Add("speed","100"); } } } 2.3 如下图，我们写好后生成 dll，到\build\Simple 目录下，打开 simpeComposer，添加托盘 车，选中小车就可以看到增加 speed 标记的方法，双击就可以把所有线段增加速度 拓展 1：这个方法的好处是我们可以根据现场业务逻辑灵活使用，如果现场都是重复性工位， 每个工位都需要增加一个 shelf 字段，那么我们就可以动态的给全场景增加和删除字段了。 拓展 2：我们也可以给小车增加其他功能比如关闭障碍物，远程关机，屏蔽报警等功能

3 编写 Coder-解释性编程

3.1 编写 coder 可以根据业务场景灵活的调用 AGV.cs 方法，我们示范一个可以让叉车 A-B 倒 走，B-A 正走的案例 3.2 首先我们线段标记 reverseDst:10 代表需要倒走的目标点

3.3 然后程序中，我们需要在 TrackFields 类里面增加 reverseDst 属性 class TrackFields { public int speed = -1; public int reverseDst = -1; } 3.4 我 们 在 useVerb 里 面 判 断 "track.reverseDst == dst.id" 如 果 条 件 满 足 ， 就 执 行 templateString 内容，调用 AGV.cs 方法里面的 ReverseGo 方法。 [TemplateTrackCoderSettings( priority = 5, useVerb = "track.reverseDst == dst.id", blockVerb = "true", templateString = "agv.ReverseGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id},${track.id}," + "${track.speed});", siteFields = typeof(SiteFields), trackFields = typeof(TrackFields))] 这样当我们需要小车从 A-B 走的时候就会自动将 ReverseGo 方法打包到 code 里面了

二 生成一个路径任务

1 创建一个移动任务

1.1 我们首先要了解 Simple 的机制，Simple 中的路线编译器实现小车移动以及功能的脚本下 发，先创建一个计划 SegmentPlan，然后对这个计划进行寻路 FindRoute,会返回一个路线脚 本 code（包含 A-B 路线所有站点以及路线功能）,然后 SendScript(code)即可下发给 Clumsy， Clumsy 解析然后进行移动或功能的实现。 1.2 了解以上我们开始编写一个简单的 move 动作，这一块我们在 Common.cs 里面编写，这 个方法和选中小车右击去某个点类似，具体流程如下方法：

1.3 我们可以写一个动作或者通过其他接口来调用这个 move 方法 ，这边我们就先用选中右 击站点来看下，可以看到，右击站点的时候，小车 Clumsy 命令窗口收到了这一串脚本，这 个脚本就是 SendScript(code) 里面 code 的内容，调用了 AGV.cs 的 Go()方法，Go 里面的参 数见绿色注释。 var host="127.0.0.1"; var agv=new AGV(){id=12, baseSpeed=1}; //srcx:起始点 X 坐标(mm)，srcy:起始点 Y 坐标(mm)，srcid:起始站点 ID， //dstx:终点 X 坐标，dsty:终点 Y 坐标，dstid：终点 ID，trackid:路径 ID,speed：路线速度 agv.Go(0,0,5,11507.2998046875,-5134.02587890625,8,9,100);

agv.Wait();

agv.Wait();

1.4 Clumsy 收到后解析器会进行脚本的解析执行

三 创建链式进程任务

1 创建链式进程任务

1.1 一般链式任务用于取放料的业务场景中，小车会先去取料再去放料，因此，我们这边写 了一个通用的链式任务方法。 1.2 进程任务继承 Mission 类，我们可以在进程，新建自定义进程，选择链式进程即可，然 后我们双击下面动作的启动进程，就可以通过界面点击进行取放料连贯动作，具体可以参考 ChainedDeliveryMission.cs 文件

1.2 这个是通过界面去调小车，我们也可以通过接口去调用，因此我们需要再写一个可以供 调用的接口出来 AutoEnqueue(),也很简单只用把 ManualEnqueue()复制，把起点和终点的赋值方法 改变即可，如下 public void AutoEnqueue(int getId,int putId) { try { Delivery former = null; var missionField = StringDictConvert<ChainedDeliveryParams>.Convert(fields); Delivery[] ls; while (true) { var d = new Delivery(); if (former != null) d.former = former; Site srcSite, dstSite; //获取起点 ID srcSite =SimpleLib.GetSite(getId); d.src = DemoCar.getId; DemoCar.getId = 0; if (former == null) { //获取终点 ID dstSite = SimpleLib.GetSite(putId); d.dst = DemoCar.putId; DemoCar.putId = 0; } else { d.dst = d.former.src; } G.pushStatus($"排序了一个叉车搬运任务{d.id}: {srcSite.id} -> {d.dst}, 上序任 务:{former?.id}"); lock (sync) { queue.Enqueue(d); } former = d; } } catch (Exception ex) { G.pushStatus($"结束任务链"); Console.WriteLine("ex", ex.Message); } }

三，webApi 编写

假设某个现场第三方调度我们小车，我们需要提供任务接口，状态接口，而且对方需要我们 的路线地图来进行他们的地图显示我们从以下作为案例供大家参考（WebApi.cs 文件）

1，接收任务

1.1 任务分为 2 种，一种是单点任务，一种是链式任务，具体请看代码

2 上传状态

1,我们需要提供小车状态给第三方系统，根据对方查询小车的编号返回对于小车的状态即可

3 上传地图

1，由于第三方需要我们的地图，我们地图是 json 格式因此直接发送给对方即可

拓展 1：如果想打开 Simpe 自动加载路线项目，请打开项目后，点击保存配置到 Simpe.exe 目录下 simpe.json 文件即可

拓展 2：如果 Simpe 和 Clumsy 通讯不上，检查防火墙关闭还有端口是否开放，Simple 端口 也可以在 simpe.json 文件中配置