现如今,科技日新月异,协作机械臂开始广泛地应用在我们的日常生活中,但你知道他是如何控制的吗?今天给大家分享的恩智浦杯 MCU 挑战赛获奖作品 TARM 机械臂控制卡。作者将给大家全方位讲解作品,360 度!淋漓尽致!让我们一起来瞧瞧吧!

 


项目介绍


作者简介:谭天诺 | 学生  广东工业大学

这篇是三等奖作品《TARM 机械臂下位机控制卡》,该参赛作品为针对液体搬运环境优化的机械臂下位机运动控制卡。针对日常生活中一个常见的场景——搬运液体(俗称“端水”),设计一项高效平稳的控制算法,配合自主开发的控制器驱动板卡,驱动自制的机械臂到达目标位置,使该算法达到高效平稳搬运液体的目的。


专家点评:该作品完成度较好,同时利用双核和 PowerQuad 的优势优化整个设计,作品展示完整流畅。且电机和舵机控制应用较为广泛,值得推广。

 

项目软件框架

 


 
项目所使用到的资源

 


程序组件介绍


1.communication 组件
负责对上下位机通讯。
外设启动了两个串口:FL0 控制台串口、FL2 设备串口。控制台串口位于板卡上;设备串口通过 arduino 型接口与驱动板连接,驱动板上可以通过跳帽选择使用 RS232 或者无线蓝牙模块作为传输介质。支持打断控制。


指令为自定义命令集,格式如下:
_&<命令><参数>;<命令><参数>;<命令><参数>

 


 
2.Timer 组件
定时器定时实现。timebase 为第一套算法提供时基,该算法仅支持一般恒速运动,可以调节一次运动的速度;可以实现三轴联动;使用的时钟是 CTIMER0。timepv 使用时钟 CTIMER1 为第二套算法提供时基,该算法仅支持主轴、副轴联动;对运动过程有变速控制,为针对液体平稳搬运环境的优化算法。

 

3.SYSTICK 时钟提供延时
CTIMER2 提供速度仿真计时器时基,功能用于开发工程中评估算法消耗时间。

 

4.limit 组件
限位器组件,协助机械臂进行初始化定位。限制机械臂运动行程。通过外部中断实现。触碰到限位器时,返回设定的零点。

 

5.IO 组件
对脉冲输出端口、方向控制端口配置、初始化。控制主轴、副轴、绕 Z 旋转轴的三个步进电机。

 

6.Closeloop 组件
反馈控制。通过硬件 IIC 总线访问 MPU6050,校准运动误差,使运动误差不积累。在外部因素影响导致 IIC 硬接线断开时,能自动转化为开环模式。

 

7.Pwm 组件
通过输出 PWM 控制夹持末端回转轴、夹持轴的舵机。

 

单核工程使用资源介绍


1、Ctimer 定时器
程序的核心就是通过定时器控制脉冲周期,不同的轴、不同的运行方式需要不同的 ctimer,此外速度仿真也需要计时器提供时基。该项目中一共使用了三个 ctimer 定时器。


2、iic
iic 与角度传感器通讯,反馈位姿信息。


3、Uart
下位机通过 uart 与上位机通讯。通讯形式为自定义指令。


4、PowerQuad
运算对运动速度精度的提升尤为明显,该框架下的运动,运算时间都是额外算进去的,运算时间越长,运动失真越厉害,特别是高速运动时达不到指令预设速度。PowerQuad 加速令该情况大为改善。


相关程序在 middleware 的 tarm_math 源文件内。


5、PIN 中断
当角度传感器无效时,运动自动变为完全开环状态,这时候需要限位装置初始化机械臂的定位。触碰到限位时,需要触发 PIN 中断,保证行程不过限制。


双核工程使用资源介绍


除单核所使用的五个外设外,双核工程的从核心额外添加音频播放功能,单独使用一个核心进行音频处理可以有效避免资源抢夺,运动完成一次后,通过 MAILBOX 向从核心发送信号,从核心播放音乐。因为指令发送可能是频繁的,因此播音的过程可能再启动一个运动。采用另一个核心处理音频,即保证音频信号的连续传输,也不会影响可能正在进行中的运动计算。

 

双核工程的额外功能:


1、SDIO+FATFS
通过 SDIO 驱动 TF 卡,通过 FATFS 读取所需的音频文件


2、DMA+IIS+IIC,驱动 WM8904 进行音频播放
FL4 外设作为 IIC,FL7 外设作为 IIS,驱动 WM8904 板载 WAV 解码 IC 播放音乐,通过耳机或者外部功放播放,每当运动完成时进行音频提醒


3、MAILBOX
使用 MAILBOX 进行主从核心之间通讯当主核心运动完成后,告知从核心,从核心播放音乐提醒。

 

通过 PowerQuad 加速算法


对 LPC5500 来说,最亮眼的无疑是比传统 DSP 核更快的 PowerQuad 单元,能够实现三角函数运算和开放运算的加速。


在对代码优化加速后,比如:
1.    void angle_to_xy(double prime_angle,double vice_angle,double *x,double *y)
2.    {
3.            double pri_ang_c,vic_ang_c,pri_ang_s,vic_ang_s;
4.    
5.    //        pri_ang_c = cos(prime_angle/180*3.14)*prime_length;
6.    //        vic_ang_c = cos(vice_angle/180*3.14)*vice_length;
7.    //
8.    //        pri_ang_s = sin(prime_angle/180*3.14)*prime_length;
9.    //        vic_ang_s = sin(vice_angle/180*3.14)*vice_length;
10.    
11.            pri_ang_c = tarm_cos(prime_angle/180*3.14)*prime_length;
12.            vic_ang_c = tarm_cos(vice_angle/180*3.14)*vice_length;
13.    
14.            pri_ang_s = tarm_sin(prime_angle/180*3.14)*prime_length;
15.            vic_ang_s = tarm_sin(vice_angle/180*3.14)*vice_length;
16.    
17.            *x = pri_ang_c + vic_ang_c;
18.            *y = pri_ang_s - vic_ang_s;
19.    }


控制效果


步进电机的控制是通过脉冲和方向实现的,控制速度是脉冲的功能,因此这里重点分析脉冲。本设在控制程序中嵌入了一个 speed debug 单元,这个单元可以输出运行过程信息和定时时间等信息,用于观察算法耗时与准确度。


首先这里需要点明,该算法的控制脉冲从控制逻辑上是等占空的,但运算过程都在低电平时期,因此低电平时期会包含算法解算所需要的时间。


通过逻辑分析仪,可以发现,脉冲最终产生脉冲高低电平间时差在 1.2ms 左右。

 


 
同样的运动,通过逻辑分析仪可以看到,高低电平之间大概有 500ms 的时间差,缩小了超过一半。

 

实物展示

 


所使用到的 LPC55S69 开发板
 
 


水杯搬运展示

 

设计者结语:


TARM 机械臂控制卡旨在实现液体搬运场景下的平稳运动。核心的运动算法是机械臂的空间解析与脉冲生成的时间控制,通过 LPC55S69 丰富的定时器资源与强大的数学运算能力保证算法的实用性、实时性。由于算法受数学运算速度的影响,PowerQuad 比寻常 M4F 的 DSP 内核更加强大的运算能力能够保证所控制的运动速度、加速度的精确性。


另外从核心承担音频文件读取与功放驱动,配合 DMA 功能,保证声音信号保真的情况下不会影响主核心的运动算法。


MCUXpresso IDE 配合 SDK 在驱动开发带来非常便捷、流畅的体验,无论是小到一个串口、IIC 亦或者整个 fatfs+DMA+SDIO 驱动体系都能轻松移植,加上明了的代码体系,能够大大缩短开发难度与开发周期。


彩蛋:


同时恩智浦社区开发小课堂邀请作者为大家详细讲解该项目的全部内容,想要了解机械臂是如何控制的,请扫描下方二维码哦!此外,小课堂还有恩智浦半导体各型号产品的实战经验,手把手教学,让你尽快上手恩智浦半导体各系列产品,敬请关注!
 

 

扫码关注更多小课堂精彩内容:
6 月 29 日:基于 LPC55S69 平台的多媒体控制系统
7 月 2 日:基于 CNN 的智能垃圾分类及远程监控系统
7 月 7 日:TARM 机械臂下位机控制卡