1079
一、系统总体部分
1.1 概述
这是一个参赛的项目，当时申请板卡理由是希望使用LPC55S69来进行串口数据管理，后来发现这个实在没什么挑战性（更主要的是可视性太差，儿子完全不感兴趣），于是任性的换了一个课题：基于视觉系统的机械手。
说是基于视觉系统，实际上就是简单的颜色分类，我们使用机械手拾取不同颜色的积木分类存放。
1.2 系统结构
系统构造复杂，将会导致系统使用复杂、维护复杂，失效率升高，因此尽可能借（chao）鉴（xi）已经过验证使用的代码是一个好习惯；另外，从节约的角度来说，使用过往的硬件也是一个好习惯。因此我们尽可能的简化了设计和实现。
本项目除了挑战赛主办方提供的MCU板卡外，主要硬件包括：机械手一套、摄像头、距离传感器、若干彩色积木块（儿子友情赞助）。完成图如下：
1.3 制作
购买一套4自由度机械手（木制或亚克力）的大约30RMB（唔，主要是穷，其实高精度机械手也不便宜），不过好丑，配上若干mg90s金属齿舵机（用模拟舵机快坑死了，下次改进可以用数字舵机，后面会细讲），大概70元左右，配上利旧的云台（云台是以前资金宽裕的时候买的，一个云台就比这个机械手贵多了）；视觉系统只需要能够进行画面和颜色识别的ESP32-CAM即可（我这里是利旧采用了“小MU”，是不是显得更高大上了呢？）为了精确测距，选用了激光测距模块VL53L0X。
在本项目中，总新增硬件成本大约200元（实际上即使不包括参赛指定的LPC55S69，总硬件成本也应该是超过了1000元），人力投入大约为80小时。
二、软硬件模块设计与实现
2.1 硬件板间接口
2.1.1 系统简介
本设计和核心思想是采用了IIC通信，通过LPC55S69读取和写入数据，来实现机械手对各种积木块的认识和抓取。
IIC只用两根线，一条Serial Clock (SCL)，提供信号触发时间边缘，一条Serial Data Line (SDA) 为双向信号数据传输。IIC无需片选，主控器件通过地址码建立多机通信的机制，根据7位“从器件专用地址码”确定MCU需要操作的设备，“从器件专用地址码”高4位为器件类型，由生产厂家制定，低3位为器件引脚定义地址，由使用者定义。
在本设计中，LPC55S69为主用MCU，提供时钟信号和进行设备信息读取和写入。带有三个从设备，分别为“小MU视觉传感器”、“VL53L0X测距模块”和“PCA9865舵机驱动模块”。IIC相关的代码可以从例程“lpcxpresso55s69_se_hostlib_se05x_ex_i2cMaster”中大量套用现有代码，各从设备模块的库是从这些模块厂商提供的arduino应用中直接取用的，仅需要少量修改。
2.1.2 舵机和PCA9865模块
舵机是用来控制机械手位置和抓取动作的，本机械手用的是模拟舵机，给出特定时间的高电平，舵机能转动在一定的角度，理论上这时如果不给出信号，只保持上电状态，舵机能保持不动，但我在LPC55S69安装一块arduino扩展板，尝试用GPIO驱动舵机，也许是功率不足，也许是代码有问题，也可能是不能用arduino的扩展板，舵机倒是可以转动，就是抖动不停，而且根本抓不牢，推测如果是数字电机应该就没这个问题了。
因此加了一块PCA9865模块，把舵机驱动这块，完全交给其控制，只需要对PCA9865写数据就行。我用的是网购的PCA9865驱动模块，支持16路输出（其实我只用了4路），采用的默认IIC从地址是0x40（可以通过板上硬件跳线修改），使用“Adafruit_PWMServoDriver.h”头之后，可以通过setPWM(n, 0, pulse)指定0~15路输出的高电平持续周期，而对于9g舵机来说，高电平宽度在20ms内通过控制脉冲宽度范围0.5ms~2.5ms为有效信号，直接写入就行。
2.1.3 小MU视觉传感器
小MU视觉传感器是我之前一个项目中利旧留下来的，是一块ESP32加上摄像头的工具，支持读取特定区域内的颜色、形状，再通过WLAN、蓝牙、TTL或者IIC传输。我这里采用的是其IIC接口。它的驱动为“MuVisionSensor.h”，同样是从arduino应用中套用出来的，其IIC的从地址是0x60。
2.1.4 激光测距模块VL53L0X
激光测距模块是本次新购，也是首次使用这种高精度模块（之前超声波测试模块干扰大，精度差）。激光测距的原理其实很简单，就是通过读取反射信号的往返时间，再根据空气中光速计算距离，从资料上可以找到VL53L0X最远测距的距离是2m，精度是2mm，就本系统来说，精度是足够的。VL53L0X通过I2C读取，缺省地址是0x29，其采用的头文件是“VL53L0.h”，因为是采用近距离模式，因此在程序中调用vl53l0x_set_mode函数，将其模式设置高精度。
2.2 硬件结构
采用外购的机械手，抛弃原配的底座，换上高精度云台，同时云台上还安装了摄像头和距离传感器，确保云台转动时，机械臂、摄像头、距离传感器的相对位置不变。机械臂本身采用了两个舵机来决定机械爪所在位置，因平行四边形原理，可以保证机械爪基本上是可以保证在水平方向上的，具体的硬件安装与本赛关系不大，暂略。
需要注意的是安装时确保VL53L0X的探测方向与机械爪的中心部分在一个竖直平面内。
2.3 软件设计
2.3.1 设计思想
分阶段实施：
（1）可行性验证
首先学习MCU资料，根据Demo写一点简单代码，做一些接口信号的测试，入手购买项目中必要的配套硬件（比如激光测距模块和舵机、舵机驱动板等）。
（2）开发阶段
搭建软硬件平台，测试完成单个功能模块正常工作。
（3）调测阶段
根据实际需求进行少量调整，将代码拼接起来，完成功能模块的功能验证。
（4）项目总结
打包代码，形成文档记录。
2.3.2 软件模块描述
该系统的软件核心为机械手的控制，包括决定方向的云台、实现抓取功能的开合机械爪、和调节具体位置的机械臂。其中云台为其中最为关键的部分，通过摄像头确认机械手能够对准积木的方向，至于机械臂，只要相差不太多，机械爪的开合可自动调整积木位置，直至将积木抓取住。因此设计了对云台的闭环自动控制技术。
机械臂部分，因为采用了两个关节的控制环，且不是完全独立，其相互存在的耦合使得机械爪所在位置在二维平面上的运动呈现出严重的非线性特征，要得到机械手控制的精确运动轨迹比较困难，采用的方式为预制定位，先建立好两个舵机的对应转角表格，根据激光测距得到的积木距离，打开机械爪，查表运动，使机械爪由云台直接向积木推前，直到使得积木正好在机械爪所能掌控的范围内，再闭合机械爪，实现积木的抓取，再抬高机械爪，根据之前读取的颜色，将积木放置在指定的区域内。稍微的误差，可以通过机械爪推动积木进行校准。
2.3.2.1 云台的PID算法
如前文所述，云台确定了机械爪可运行的二维平面，如何使这个二维平面能够穿过积木的中心位置呢？我们使用了PID算法，在云台转动时，读取积木在摄像头内的位置，并根据偏差进行调整。调整时，先估算一个大致需要转动的角度，驱动舵机时云台转动，因为舵机本身的误差和PCA9865时钟的误差，肯定不可能一次对准，根据误差，再进行下步调节。其中转动的系数为Kp，很容易理解，单独的P调节系统始终会处于存在误差的情况。因此我们引入了积分环节的输出系数Ki，在这个系统中，因为代码原因（主要是我也不知道调用数学函数），采用的一个较小的量δ乘以非线性系数（读取出来的误差），再进行相加，取代积分函数∫，事实证明是可行的。
说是PID算法，其实在这个项目里面，并没有用到D功能，在上述计算公式中，可以认为Kd=0。
2.3.2.2 机械臂的定位
机械臂的定位包括：
（1）原始位置，即抬高机械爪，由云台转动，探测积木所在方向。
（2）准备位置，即机械爪停留在最靠近云台的位置，这时机械爪张开。
（3）推进至抓取准备位置，即根据距离传感器读取到的距离，将机械爪向前推进（因为精度原因，推进并非完全水平方向，后续表格做得更细，会更加接近水平）。
（4）抓取积木块，当机械爪到达指定位置后，驱动舵机，使爪子闭合起来，这时机械爪就能抓取积木。
（5）抬高机械爪，恢复至机械臂原始位置。
（6）由云台带动机械臂，转动到某个指定位置，松开机械爪，使积木块自行掉落在指定区域内。
2.3.2.3 机械爪的开合
机械爪只有开合动作，相对简单，因为机械手比较简陋，没有压力传感器，因此当机械手抓握积木时，我使用最小的闭合角度，通过舵机本身力反馈抓取积木，因为舵机本身是防烧的金属舵机（9g尺寸，14g力），且驱动电压并不高（标称6V，实测5.3V），不会烧坏。
2.3.3 代码联调
这个系统的代码调试相对简单，全代码已化简为iic通信，在fsl_i2c.c中，初始化IIC，再进行调用。只需要根据IIC三个从设备的地址分别读取或者写入数据，就能实现期望的功能。
三、系统功能验证
3.1 软硬件配合
舵机虽然采用了0~180°，但事实上，需要转动的角度远小于180°，以机械爪为例，只需要大约90°左右，机械爪就能从张开到最大值变为紧紧闭合。考虑到舵机本身特点，我们装配机械手时，先在舵机上加电，使其归零，再进行装配。
3.2 图像识别
小MU内部集成了一块ESP32，ESP32作为MCU，可以协助计算出目标积木块的颜色。同时，
3.3 距离
通过距离传感器读取积木块到传感器之间的距离，当距离小于或大于机械爪正常行进位置，则认为该距离不适合，直接丢弃（后期可以考虑增加声光告警）
大致内容如上描述，后续再补充，另在论坛上直接重新编辑，不让新增图片了，也许是bug或网络问题导致的。
下载ECAD模型
