▌01 机械臂控制器
在 基于STM32F103双轴机械臂完整电路板设计1 给出了 双轴机械臂 的控制电路设计。
▲ 已经基本焊接完毕的设计电路板
在 对于STM32F103三轴机械臂控制器进行基本功能测试-上下运动功能 给出了下位机已经部分的STM32F103cb的程序。
▌02 初步调试
1.设计错误
⊙ 晶体频率标识错误:
由原来的16MHz 修改成8MHz。
▲ 下载程序后LED闪烁
⊙ 舵机接口信号错误:
在舵机接口设计中,+5V,GND的顺序相反了。需要通过舵机接口引线将其修改过来。
▲ 舵机的信号借口
⊙ PWM输出没有上拉电阻:
下面是直接测量PWM输出(U6:PIN16)的信号。该信号无法驱动舵机运动。
修改方式: 在PSERV PIN3与邻近的+5V引线焊接510Ω上拉电阻。
X
▲ 焊接511电阻上接+5V
修正之后,可以在PSERV PIN3(U6:ULN2003-16)测量到如下的信号:
⊙ 错误修正:针对前面的错误,重新修改了电路板工程文件设计。
2.安装测试
(1)安装接口插座
▲ 焊接外部接口插座
(2)测试MODBUS
在main.c中主循环,使用:
if(++nShowCount >= 50) {
nShowCount = 0;
ON(ST3806_DIR2_PIN);
ON(ST3806_DIR1_PIN);
UART2SendChar(0x55);
测量PRT2、PRT3的 PIN3,PIN4,可以看到输出互补的485信号。波特率为115200(脉冲的宽度大约10微秒)。
(3)测量PWM输出
测量U6的PIN1(SERV01)信号,如下图所示。由于该信号通过ULN2003输出反向,符合舵机的驱动信号极性。
根据前面输出接口的修改,可以经由U6-16 输出正极性舵机驱动脉冲。
▲ 机械手爪运动
在SerialTXT中设置有调试命令: claw,命令参数为舵机脉冲对应的时间,单位微秒。
经过测试:claw 1000 :对应手爪张开最大; claw 2000对应手爪闭合。
3.测试命令串口输出
(1)简介
命令串口是用于机械臂与系统其它部分进行控制的端口。通信的底层协议见: 机械臂底层通信协议说明 。在电路设计中,使用了STM32F103cb的UART3来输出和接收串口控制命令。
(2)接口测试
静态测试 PSEN端口PIN3(上边第一个管脚),静态电压:-5.98V.
(3)软件测试
配置UART3串口的基本参数,如下图所示:
▲ UART3的参数配置
在主循环发送数据0x55,可以在PSEN-PIN3测量到如下的波形:
将PSEN端口PIN2,PIN3短接。通过UART3发送和接收,验证信号可以正常板级循环。
UART3SendChar(0x55);
if(UART3_CANRECE) {
unsigned char c;
UART3ReceChar(&c);
printf("%02x ", c);
}
4.调试关节角度读取
1.简介
根据博文 对于STM32F103三轴机械臂控制器进行基本功能测试-关节角度读取 中对于角度传感器读取的调试结果来看:
- 需要将 ST-3806-15-RS的协议按照 角度编码器 ST-3806-15-RS 中的方法,将它设置成115200。该传感器缺省的波特率为9600。
- 设置成115200之后,读取两个角度传感器的时间大约为4ms(每读取一个角度值大约2ms)。
(2)读取数据
调试过程中,转接线的可靠性影响了读取数据的产生。
▲ 临时转接插头
肩部传感器:连接到 PRT2
肘部传感器:连接到 PRT1
int nAngle1 = 0;
int nAngle2 = 0;
nAngle1 = ShoulderAngle();
nAngle2 = ElbowAngle();
printf("%d, %d\r\n", nAngle1, nAngle2);
5.上下运动光电传感器
在控制肩部运动速递之前,请参照 对于STM32F103控制的三轴机械臂基本功能测试-关节转动控制 分别设置肩部和肘部步进电机的设置编码开关。
(1)连接端口
根据 基于STM32F103双轴机械臂完整电路板设计 调试结果:
- 请注意机械臂上的传感器的接口信号定义与控制电路板信号定义线序不同; 需要将GND,SIGNAL交叉;
- 机械臂上的传感器,从里到外分别是VCC,GND, SIGNAL;
- 将机械臂下面的位置传感器接入PP1, 将机械臂上面的传感器接入PP2;
(2)端口电位
在光电管没有被遮挡时:PSS0,PSS1的电位大约:2V。这应该是ULN2003的输入电压。
在光电管内部有遮挡时:0.055V。
▌03 运动控制
下面给出使用Bootloader串口进行程序调试的相关命令。
1.肩部运动
(1)相关命令
shgoto arg : 肩部运动到arg所给定的位置。
arg∈(8192,24576),中间值:16384
(2)确定肩部运动参数
在control.h中有定义的几个常量,确定了肩部运动的特性。
#define ARTHROSIS_DIVIDER_SHOULDER_PERIOD 100
它与下面的SHOULDER_INC一期确定了肩部运动的速度。
#define ARTHROSIS_DIVIDER_SHOULDER_INC 100
#define ARTHROSIS_DIVIDER_SHOULDER_INC_MIN 5
这两个值确定了肩部运动的最快值和最慢值。
#define SHOULDER_DAMP_RANGE 2000
设置了肩部运动减速(软停止)对应的角度范围。请注意,肩部运动一周的(360°)对应的范围是32768。所以根据上面的range,可以大体确定肩部运动减速范围是:
2000
×
360
32768
=
27.972
{{2000 \times 360} \over {32768}} = 27.972
327682000×360=27.972
#define SHOULDER_INC_COUNT 100
这个参数确定了肩部运动软启动的速度。数值越小,软启动的越快。
2.肘部运动
elgoto arg: 肘部运动对应的命令。
arg∈(4096,28672),中间值:16384
3.抓爪运动
claw arg: 控制抓爪张和角度。
arg属于(1000,2000):1000对应抓张开; 2000:对应抓爪闭合
4.上下运动
armup: 机械臂运动到上极限;
armdown:机械臂运动到下极限;
▌04 控制命令
根据 机械臂底层通信协议说明 编写底层串口控制命令。
具体调试可以参见 底层串口调试 相关博文。
▲ 基本上组装完毕的机械臂
▌结论
基于STM32F103的机械臂控制板,实现了控制的平顺以及底层控制协议。
▲ 控制电路板暂时安放位置
■ 相关文献链接:
- 基于STM32F103双轴机械臂完整电路板设计
- 两轴机械臂+机械爪整体控制板设计与机械爪控制调试
- 对于STM32F103三轴机械臂控制器进行基本功能测试-上下运动功能
- 机械臂底层通信协议说明
- 对于STM32F103三轴机械臂控制器进行基本功能测试-关节角度读取
- 角度编码器 ST-3806-15-RS
- 对于STM32F103控制的三轴机械臂基本功能测试-关节转动控制
电路板AD工程文件:AD\XQWF\2020\机械臂\F103C82AXES.SchDoc ↩︎