上一期我们把六自由度机械臂的结构和基础舵机控制跑通之后很多朋友留言问“能不能像工业机械臂那样先用手拖着机械臂摆几个姿势让它记住然后自己重复执行” 这个功能叫直接示教也叫拖拽示教是机械臂入门里最直观、也最容易做出成就感的玩法。这一期教程围绕 STM32F103 单片机展开重点解决三件事如何把关节角度实时读回单片机如何进入“示教记录”状态拖动机器人时按固定周期保存关节角度如何把记录下来的轨迹平滑回放让机械臂自行复现刚才的动作。如果你手里有第一期的六自由度机械臂结构件这一期不需要大改硬件主要是把传感器回馈和控制状态机补上。需要完整材料包清单的可以在评论区留言或私信交流我会把装配图和接线表一起发给你。下面我们直接从原理开始再进入 STM32F103 的代码实现。1. 项目背景让机械臂从“会动”到“能记住动作”1.1 这一期要解决什么问题传统 DIY 机械臂控制通常是在代码里写死一组角度// 写死一组动作 Servo_SetJointAngle(0, 90); Servo_SetJointAngle(1, 45); Servo_SetJointAngle(2, 30);这种方法的问题是每改变一次动作就要重新改代码、重新编译、重新烧录。如果只是让机械臂做固定演示问题不大但如果想让机械臂学会一段连续动作比如“从左边抓起来转到右边放下”写死角度的工作量会非常大而且很难调出流畅轨迹。所以这一期的目标就是做一个“示教器”功能用手拖动机器臂机械臂实时记录各个关节的角度变化 拖完松开按一下回放键机械臂自己把刚才的动作重复出来。这个功能在工业上称为示教再现在小型六自由度机械臂上同样可以实现。1.2 直接示教和关节空间轨迹工业机械臂的示教通常有两类示教方式说明难度离线示教在软件里通过运动学计算轨迹再上传给机器人需要 DH 参数、正逆解直接示教人工拖动机器臂系统记录关节角度不需要运动学计算实现简单本文采用直接示教。它的核心思路是机械臂末端走什么路最终都体现在六个关节角度的变化上。如果我们不关心末端在世界坐标系里的绝对位置只记录每个关节在每一时刻的角度值那么回放时只需要按照同样的时间线去驱动六个舵机就可以了。这就是“关节空间轨迹复现”。它和“笛卡尔空间轨迹规划”有本质区别关节空间只记录关节角度适合复现一段动作笛卡尔空间需要解算末端位置适合抓取、写字、画圆等任务。对于本项目的直接示教需求关节空间记录已经足够不需要引入逆运动学这也是它能跑在 STM32F103 上的原因。2. 整体架构与信号流程2.1 系统连接图先看机械臂控制系统的信号链路建议在脑子里建立一份数据流图复位/串口指令 - STM32F103 状态机 - PWM 信号 - 六路舵机 ^ | | | 关节角度采集 ------ 电位器/编码器传感器简单来说STM32F103 用定时器输出 6 路 PWM 控制舵机角度每个关节旁边装有角度传感器通过 ADC 把电位器电压读回单片机示教时舵机允许被外力拖动同时角度传感器持续输出角度值回放时单片机根据已经记录的角度数组反推出舵机 PWM让机械臂复现动作。这里需要特别注意示教时记录的是“传感器测到的实际关节角度”而不是“代码里发送给舵机的目标角度”。否则一旦舵机堵转或负载导致实际角度没到位记录和回放就会产生误差。2.2 工作状态机设计为了让代码清晰机械臂整体运行状态用一个状态机来管理状态说明进入条件IDLE 空闲机械臂保持当前位置上电复位、任意功能结束RECORD 示教记录舵机释放扭矩允许拖动同时保存轨迹按下“记录键”或收到 R 指令PLAY 轨迹回放按记录数据驱动舵机运动按下“回放键”或收到 P 指令STOP 停止停止当前动作回到空闲按下“停止键”或收到 S 指令如果还需要掉电保存可以在回放前增加一个“读取存储”步骤如果不需要直接放在 RAM 中就行。2.3 为什么需要定时器中断来管理采样机械臂轨迹回放最怕的就是“时间不均匀”。例如我们计划每 20ms 采样一个点如果不用定时器中断而只是在while循环里用HAL_Delay采样那么串口接收、按键判断、舵机刷新都会干扰采样周期导致记录下来的轨迹时间轴忽快忽慢回放时动作就会卡顿。因此本项目必须把“固定周期采样”和“固定周期回放”放到定时器中断里处理主循环只负责接收指令和刷新状态。3. 核心原理拆解3.1 如何拖动机器臂并读取关节角度要让机械臂可以“用手拖着动”首先得让舵机不再锁死关节。这里有一个常见的误区和坑点普通 PWM 舵机并没有标准的“脱离扭矩”指令。很多舵机只要不断电、不撤 PWM就算我们发送中位信号它也会持续抵抗外力而如果线断了有些舵机反而会因为内部减速齿轮自锁依然很难手动转动。所以在直接示教方案中推荐两种硬件做法方案 A使用带扭矩关闭功能的总线舵机总线舵机如 ST3215、LX-16A 等通常支持读取当前角度设置舵机扭矩关闭扭矩关闭后可以用手转动关节转动过程中持续读回当前角度。这种方式最省事硬件接线也简单。缺点是总线舵机价格更高需要额外的串口协议驱动。方案 B普通 PWM 舵机 关节角度传感器在示教阶段给全部或单个舵机断电让关节可以自由活动此时关节处的电位器或编码器通过 ADC 读取角度。回放阶段再给舵机通电由 PWM 指令复现角度。这种方案更适合低成本材料包。本文后面的代码围绕“PWM 舵机 ADC 角度传感器”的思路展开同时把舵机断电和角度读取都封装成独立接口。如果你使用总线舵机只需要替换这两个接口函数即可。很多朋友在第一次做拖拽示教时直接把舵机线从舵机端拔掉然后发现还是转不动这是因为舵机减速齿轮箱的自锁阻力太大。此时建议在关节轴的另一侧并联一个角度电位器并确保机械结构上可以手动带动关节转动而不要强行去滚舵机输出轴。3.2 PWM 舵机控制原理普通模拟/数字舵机的控制信号是 50Hz 的周期脉冲周期 20ms脉宽对应角度。脉宽典型角度0.5ms0°1.0ms45°1.5ms90°2.0ms135°2.5ms180°因此我们只需要让 STM32F103 的定时器产生 20ms 周期、脉宽可变的 PWM就能控制舵机角度。假设定时器时钟为 72MHz预分频为 72得到 1MHz 的计数频率也就是每个计数单位为 1us。要让 PWM 周期为 20ms重装载值设为20000us - 1 19999脉宽 PWM 占空比对应比较寄存器的值CCR 500 angle / 180.0f * 2000;其中500 对应 0.5ms 脉宽2000 对应 2.0ms 脉宽范围500 2000 2500对应 2.5ms即 180°。3.3 轨迹采样与插值回放机械臂示教记录实际上是记录一条“角度-时间”曲线。例如时间 0ms 关节角度 [ 10°, 20°, 30°, 40°, 50°, 60°] 时间 20ms 关节角度 [ 15°, 22°, 29°, 43°, 52°, 61°] 时间 40ms 关节角度 [ 23°, 25°, 28°, 47°, 55°, 63°] ...回放时最简单的思路是每隔 20ms 把记录的采样点依次发给舵机。但是如果采样间隔较长比如 50ms回放时舵机会一顿一顿地走。为了保证流畅通常需要对相邻两点做线性插值。线性插值公式为当前角度 前一点角度 (后一点角度 - 前一点角度) * (当前时间 - 前一点时间) / (后一点时间 - 前一点时间)当采样周期固定为 20ms 时插值计算可以进一步简化为表格索引和比例运算。4. 硬件与环境准备4.1 材料清单下面以低成本材料包为例模块数量说明STM32F103C8T6 最小系统板1主控六自由度机械臂结构件1 套铝合金或 3D 打印PWM 舵机6关节执行器旋转电位器/角度传感器6并联在关节轴上舵机电源 5V~7.4V1电流建议 3A 以上单独 3.3V 稳压1给 MCU 和传感器供电USB 转串口模块1调试可选按键若干2~3记录、回放、停止很多材料包里六个舵机并不是完全同型号但只要 PPM/PWM 信号标准一致代码层面可以不区分。真正的区别在于扭矩和耗电所以电源需要按最大堵转电流预留。4.2 软件环境与版本建议本文代码基于 HAL 库STM32CubeMX 6.xKeil MDK 5.x 或 STM32CubeIDESTM32F1 HAL 库。不同版本生成的代码结构略有差异但下面的核心逻辑都可以直接移植。版本不建议刻意追求最新能正常生成工程即可。4.3 接线与电源注意事项六个舵机同时运动时瞬时电流可能达到 2A 以上。如果使用 AMS1117 这类小电流 LDO 给 MCU 供电舵机会把电源电压拉低导致 STM32 复位。建议采用以下供电结构电池/电源适配器 - 舵机电源模块 - 舵机 - 5V/3.3V LDO - STM32F103并且注意MCU 的 GND 和舵机的 GND 必须共地舵机 PWM 信号线不建议直接从舵机驱动板绕太长如果信号线超过 20cm可以加 1kΩ 串联电阻电位器中间抽头接 ADC另外两端分别接 3.3V 和 GND不要接 5V否则会超过 STM32F103 ADC 输入范围。5. STM32F103 工程初始化5.1 用 CubeMX 创建基础工程在 STM32CubeMX 中需要开启以下资源RCC外部晶振或内部晶振SYSDebug Serial Wire定时器一个用于产生 6 路 PWM一个用于产生 10ms 系统节拍ADC至少 6 个通道用于角度传感器USART1用于调试和串口指令GPIO三个按键分别是记录、回放、停止。如果 MCU 的定时器通道不够可以使用两组定时器例如舵机定时器/通道说明舵机 0TIM2_CH1示例映射按实际接线调整舵机 1TIM2_CH2示例映射舵机 2TIM2_CH3示例映射舵机 3TIM2_CH4示例映射舵机 4TIM3_CH1示例映射舵机 5TIM3_CH2示例映射角度传感器供电为 3.3V 时ADC 精度按 12 位处理分辨率约 0.08° 左右已经足够用于舵机示教回放。5.2 定时器参数建议PWM 定时器配置如下时钟源内部时钟 72MHz 预分频 PSC71 计数周期 ARR19999 输出比较模式PWM Mode 1 脉冲初始值1500对应 1.5ms 中位系统节拍定时器可以配置为节拍周期10ms 中断优先级高于主循环10ms 节拍用于轨迹记录和回放刷新。如果希望轨迹更细腻也可以改成 5ms但要注意记录点数会增加RAM 消耗也随之翻倍。6. 核心代码编写下面开始编写机械臂示教代码。为了便于阅读代码按功能拆成几个文件你可以直接新建到自己的 Keil/CubeIDE 工程中。6.1 舵机 PWM 模块新建servo_ctrl.h#ifndef __SERVO_CTRL_H #define __SERVO_CTRL_H #include main.h #include stdint.h #define SERVO_NUM 6U void Servo_InitAll(void); void Servo_SetJointAngle(uint8_t index, float angleDeg); void Servo_TorqueOffAll(void); uint32_t Servo_AngleToPulse(float angleDeg); #endif新建servo_ctrl.c#include servo_ctrl.h // 如果使用 CubeMX 生成htim2、htim3 是全局变量 extern TIM_HandleTypeDef htim2; extern TIM_HandleTypeDef htim3; // 每个舵机使用的定时器句柄按实际接线修改 static TIM_HandleTypeDef* servoTimer[SERVO_NUM] { htim2, htim2, htim2, htim2, htim3, htim3 }; // 每个舵机使用的定时器通道 static uint32_t servoChannel[SERVO_NUM] { TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3, TIM_CHANNEL_4, TIM_CHANNEL_1, TIM_CHANNEL_2 }; void Servo_InitAll(void) { // PWM 定时器已经在 MX_TIM2_Init / MX_TIM3_Init 中初始化 // 这里开始输出中位脉冲 for (uint8_t i 0; i SERVO_NUM; i) { __HAL_TIM_SET_COMPARE(servoTimer[i], servoChannel[i], 1500); HAL_TIM_PWM_Start(servoTimer[i], servoChannel[i]); } } uint32_t Servo_AngleToPulse(float angleDeg) { // 控制角度限制在 0~180 if (angleDeg 0.0f) { angleDeg 0.0f; } if (angleDeg 180.0f) { angleDeg 180.0f; } // 0.5ms5002.0ms范围2000定时器计数频率1MHz return (uint32_t)(500.0f (angleDeg / 180.0f) * 2000.0f); } void Servo_SetJointAngle(uint8_t index, float angleDeg) { if (index SERVO_NUM) { return; } uint32_t pulse Servo_AngleToPulse(angleDeg); __HAL_TIM_SET_COMPARE(servoTimer[index], servoChannel[index], pulse); } void Servo_TorqueOffAll(void) { // 注意普通 PWM 舵机停止 PWM 不一定等于“脱力” // 如果实测无法拖动需要在舵机电源回路加 MOSFET 开关 for (uint8_t i 0; i SERVO_NUM; i) { HAL_TIM_PWM_Stop(servoTimer[i], servoChannel[i]); } }这里把Servo_TorqueOffAll()留作对外接口具体如何断电需要根据硬件调整。若你的舵机在PWM_Stop后依然锁死请在示教前断开舵机电源这也是很多低成本机械臂最常采用的硬件实现。6.2 关节角度采集模块假设使用 ADC 连续采样DMA 把六个通道的结果放到数组adcRaw[6]中。新建joint_sensor.h#ifndef __JOINT_SENSOR_H #define __JOINT_SENSOR_H #include main.h #include stdint.h #define JOINT_NUM 6U void JointSensor_Init(void); float JointSensor_GetAngleDeg(uint8_t index); #endif新建joint_sensor.c#include joint_sensor.h extern ADC_HandleTypeDef hadc1; static volatile uint16_t adcRaw[JOINT_NUM]; void JointSensor_Init(void) { // 启动 ADC DMA连续转换 6 个通道 HAL_ADC_Start_DMA(hadc1, (uint32_t *)adcRaw, JOINT_NUM); } float JointSensor_GetAngleDeg(uint8_t index) { if (index JOINT_NUM) { return 0.0f; } // 传感器输出电压 0~3.3V对应 0~4095 // 机械量程按 0~180° 近似实际装配后需要校准 float angle ((float)adcRaw[index]) / 4095.0f * 180.0f; return angle; }以上代码只适用于“电位器整圈对应 0~180°”的理想情况。真实安装中电位器起始角度不一定和机械零点重合而且每个关节机械限位不同。因此实际工程中还要加一个校准偏移量和缩放系数typedef struct { float offsetDeg; // 零点偏移 float scale; // 单位 ADC 值对应角度 } JointCalib_t; static const JointCalib_t jointCalib[JOINT_NUM] { {0.0f, 180.0f / 4095.0f}, {0.0f, 180.0f / 4095.0f}, {0.0f, 180.0f / 4095.0f}, {0.0f, 180.0f / 4095.0f}, {0.0f, 180.0f / 4095.0f}, {0.0f, 180.0f / 4095.0f}, }; float JointSensor_GetAngleDeg(uint8_t index) { if (index JOINT_NUM) { return 0.0f; } float raw (float)adcRaw[index]; return raw * jointCalib[index].scale jointCalib[index].offsetDeg; }每个材料包机械结构不同强烈建议在正式示教前做一次“绝对零点对齐”把机械臂摆到一个容易测量的基准姿态用上位机或串口打印此刻六个 ADC 读数将读数换算成角度记录到上面的校准结构体里。6.3 示教记录与回放模块这是整个功能的核心包含状态机、记录缓冲区和插值回放算法。新建drag_teach.h#ifndef __DRAG_TEACH_H #define __DRAG_TEACH_H #include stdint.h #define JOINT_NUM 6U #define MAX_POINTS 600U // 最大采样点数 #define SAMPLE_DT_MS 20U // 采样周期 20ms typedef enum { ARM_STATE_IDLE 0, ARM_STATE_RECORD, ARM_STATE_PLAY } ArmState_t; typedef struct { uint16_t pointCount; int16_t jointAngle[MAX_POINTS][JOINT_NUM]; // 单位0.1° } Trajectory_t; void DragTeach_StartRecord(void); void DragTeach_StartPlay(float speed); void DragTeach_Stop(void); void DragTeach_10msTick(void); ArmState_t DragTeach_GetState(void); #endif新建drag_teach.c#include drag_teach.h #include servo_ctrl.h #include joint_sensor.h static Trajectory_t gTraj; static ArmState_t gState ARM_STATE_IDLE; static float gPlaySpeed 1.0f; static uint32_t gPlayTimeMs 0; static uint16_t gRecordTick 0; void DragTeach_StartRecord(void) { if (gState ! ARM_STATE_IDLE) { return; } gTraj.pointCount 0; gRecordTick 0; // 示教前舵机释放扭矩 Servo_TorqueOffAll(); gState ARM_STATE_RECORD; } void DragTeach_StartPlay(float speed) { if (gState ! ARM_STATE_IDLE) { return; } if (gTraj.pointCount 2) { // 没有有效轨迹 gState ARM_STATE_IDLE; return; } gPlaySpeed speed; gPlayTimeMs 0; // 重新使能舵机 Servo_InitAll(); gState ARM_STATE_PLAY; } void DragTeach_Stop(void) { gState ARM_STATE_IDLE; } ArmState_t DragTeach_GetState(void) { return gState; } static void Record_Tick(void) { if (gTraj.pointCount MAX_POINTS) { // 记录缓冲区已满自动结束 gState ARM_STATE_IDLE; return; } uint16_t idx gTraj.pointCount; for (uint8_t j 0; j JOINT_NUM; j) { float deg JointSensor_GetAngleDeg(j); gTraj.jointAngle[idx][j] (int16_t)(deg * 10.0f); } gTraj.pointCount; } // 根据当前时间对某关节做线性插值 static float InterpAngle(uint8_t joint, uint32_t timeMs) { if (gTraj.pointCount 2) { return JointSensor_GetAngleDeg(joint); } float t (float)timeMs / (float)SAMPLE_DT_MS; uint16_t i1 (uint 基于STM32的六自由度机械臂拖拽示教与轨迹复现详解
上一期我们把六自由度机械臂的结构和基础舵机控制跑通之后很多朋友留言问“能不能像工业机械臂那样先用手拖着机械臂摆几个姿势让它记住然后自己重复执行” 这个功能叫直接示教也叫拖拽示教是机械臂入门里最直观、也最容易做出成就感的玩法。这一期教程围绕 STM32F103 单片机展开重点解决三件事如何把关节角度实时读回单片机如何进入“示教记录”状态拖动机器人时按固定周期保存关节角度如何把记录下来的轨迹平滑回放让机械臂自行复现刚才的动作。如果你手里有第一期的六自由度机械臂结构件这一期不需要大改硬件主要是把传感器回馈和控制状态机补上。需要完整材料包清单的可以在评论区留言或私信交流我会把装配图和接线表一起发给你。下面我们直接从原理开始再进入 STM32F103 的代码实现。1. 项目背景让机械臂从“会动”到“能记住动作”1.1 这一期要解决什么问题传统 DIY 机械臂控制通常是在代码里写死一组角度// 写死一组动作 Servo_SetJointAngle(0, 90); Servo_SetJointAngle(1, 45); Servo_SetJointAngle(2, 30);这种方法的问题是每改变一次动作就要重新改代码、重新编译、重新烧录。如果只是让机械臂做固定演示问题不大但如果想让机械臂学会一段连续动作比如“从左边抓起来转到右边放下”写死角度的工作量会非常大而且很难调出流畅轨迹。所以这一期的目标就是做一个“示教器”功能用手拖动机器臂机械臂实时记录各个关节的角度变化 拖完松开按一下回放键机械臂自己把刚才的动作重复出来。这个功能在工业上称为示教再现在小型六自由度机械臂上同样可以实现。1.2 直接示教和关节空间轨迹工业机械臂的示教通常有两类示教方式说明难度离线示教在软件里通过运动学计算轨迹再上传给机器人需要 DH 参数、正逆解直接示教人工拖动机器臂系统记录关节角度不需要运动学计算实现简单本文采用直接示教。它的核心思路是机械臂末端走什么路最终都体现在六个关节角度的变化上。如果我们不关心末端在世界坐标系里的绝对位置只记录每个关节在每一时刻的角度值那么回放时只需要按照同样的时间线去驱动六个舵机就可以了。这就是“关节空间轨迹复现”。它和“笛卡尔空间轨迹规划”有本质区别关节空间只记录关节角度适合复现一段动作笛卡尔空间需要解算末端位置适合抓取、写字、画圆等任务。对于本项目的直接示教需求关节空间记录已经足够不需要引入逆运动学这也是它能跑在 STM32F103 上的原因。2. 整体架构与信号流程2.1 系统连接图先看机械臂控制系统的信号链路建议在脑子里建立一份数据流图复位/串口指令 - STM32F103 状态机 - PWM 信号 - 六路舵机 ^ | | | 关节角度采集 ------ 电位器/编码器传感器简单来说STM32F103 用定时器输出 6 路 PWM 控制舵机角度每个关节旁边装有角度传感器通过 ADC 把电位器电压读回单片机示教时舵机允许被外力拖动同时角度传感器持续输出角度值回放时单片机根据已经记录的角度数组反推出舵机 PWM让机械臂复现动作。这里需要特别注意示教时记录的是“传感器测到的实际关节角度”而不是“代码里发送给舵机的目标角度”。否则一旦舵机堵转或负载导致实际角度没到位记录和回放就会产生误差。2.2 工作状态机设计为了让代码清晰机械臂整体运行状态用一个状态机来管理状态说明进入条件IDLE 空闲机械臂保持当前位置上电复位、任意功能结束RECORD 示教记录舵机释放扭矩允许拖动同时保存轨迹按下“记录键”或收到 R 指令PLAY 轨迹回放按记录数据驱动舵机运动按下“回放键”或收到 P 指令STOP 停止停止当前动作回到空闲按下“停止键”或收到 S 指令如果还需要掉电保存可以在回放前增加一个“读取存储”步骤如果不需要直接放在 RAM 中就行。2.3 为什么需要定时器中断来管理采样机械臂轨迹回放最怕的就是“时间不均匀”。例如我们计划每 20ms 采样一个点如果不用定时器中断而只是在while循环里用HAL_Delay采样那么串口接收、按键判断、舵机刷新都会干扰采样周期导致记录下来的轨迹时间轴忽快忽慢回放时动作就会卡顿。因此本项目必须把“固定周期采样”和“固定周期回放”放到定时器中断里处理主循环只负责接收指令和刷新状态。3. 核心原理拆解3.1 如何拖动机器臂并读取关节角度要让机械臂可以“用手拖着动”首先得让舵机不再锁死关节。这里有一个常见的误区和坑点普通 PWM 舵机并没有标准的“脱离扭矩”指令。很多舵机只要不断电、不撤 PWM就算我们发送中位信号它也会持续抵抗外力而如果线断了有些舵机反而会因为内部减速齿轮自锁依然很难手动转动。所以在直接示教方案中推荐两种硬件做法方案 A使用带扭矩关闭功能的总线舵机总线舵机如 ST3215、LX-16A 等通常支持读取当前角度设置舵机扭矩关闭扭矩关闭后可以用手转动关节转动过程中持续读回当前角度。这种方式最省事硬件接线也简单。缺点是总线舵机价格更高需要额外的串口协议驱动。方案 B普通 PWM 舵机 关节角度传感器在示教阶段给全部或单个舵机断电让关节可以自由活动此时关节处的电位器或编码器通过 ADC 读取角度。回放阶段再给舵机通电由 PWM 指令复现角度。这种方案更适合低成本材料包。本文后面的代码围绕“PWM 舵机 ADC 角度传感器”的思路展开同时把舵机断电和角度读取都封装成独立接口。如果你使用总线舵机只需要替换这两个接口函数即可。很多朋友在第一次做拖拽示教时直接把舵机线从舵机端拔掉然后发现还是转不动这是因为舵机减速齿轮箱的自锁阻力太大。此时建议在关节轴的另一侧并联一个角度电位器并确保机械结构上可以手动带动关节转动而不要强行去滚舵机输出轴。3.2 PWM 舵机控制原理普通模拟/数字舵机的控制信号是 50Hz 的周期脉冲周期 20ms脉宽对应角度。脉宽典型角度0.5ms0°1.0ms45°1.5ms90°2.0ms135°2.5ms180°因此我们只需要让 STM32F103 的定时器产生 20ms 周期、脉宽可变的 PWM就能控制舵机角度。假设定时器时钟为 72MHz预分频为 72得到 1MHz 的计数频率也就是每个计数单位为 1us。要让 PWM 周期为 20ms重装载值设为20000us - 1 19999脉宽 PWM 占空比对应比较寄存器的值CCR 500 angle / 180.0f * 2000;其中500 对应 0.5ms 脉宽2000 对应 2.0ms 脉宽范围500 2000 2500对应 2.5ms即 180°。3.3 轨迹采样与插值回放机械臂示教记录实际上是记录一条“角度-时间”曲线。例如时间 0ms 关节角度 [ 10°, 20°, 30°, 40°, 50°, 60°] 时间 20ms 关节角度 [ 15°, 22°, 29°, 43°, 52°, 61°] 时间 40ms 关节角度 [ 23°, 25°, 28°, 47°, 55°, 63°] ...回放时最简单的思路是每隔 20ms 把记录的采样点依次发给舵机。但是如果采样间隔较长比如 50ms回放时舵机会一顿一顿地走。为了保证流畅通常需要对相邻两点做线性插值。线性插值公式为当前角度 前一点角度 (后一点角度 - 前一点角度) * (当前时间 - 前一点时间) / (后一点时间 - 前一点时间)当采样周期固定为 20ms 时插值计算可以进一步简化为表格索引和比例运算。4. 硬件与环境准备4.1 材料清单下面以低成本材料包为例模块数量说明STM32F103C8T6 最小系统板1主控六自由度机械臂结构件1 套铝合金或 3D 打印PWM 舵机6关节执行器旋转电位器/角度传感器6并联在关节轴上舵机电源 5V~7.4V1电流建议 3A 以上单独 3.3V 稳压1给 MCU 和传感器供电USB 转串口模块1调试可选按键若干2~3记录、回放、停止很多材料包里六个舵机并不是完全同型号但只要 PPM/PWM 信号标准一致代码层面可以不区分。真正的区别在于扭矩和耗电所以电源需要按最大堵转电流预留。4.2 软件环境与版本建议本文代码基于 HAL 库STM32CubeMX 6.xKeil MDK 5.x 或 STM32CubeIDESTM32F1 HAL 库。不同版本生成的代码结构略有差异但下面的核心逻辑都可以直接移植。版本不建议刻意追求最新能正常生成工程即可。4.3 接线与电源注意事项六个舵机同时运动时瞬时电流可能达到 2A 以上。如果使用 AMS1117 这类小电流 LDO 给 MCU 供电舵机会把电源电压拉低导致 STM32 复位。建议采用以下供电结构电池/电源适配器 - 舵机电源模块 - 舵机 - 5V/3.3V LDO - STM32F103并且注意MCU 的 GND 和舵机的 GND 必须共地舵机 PWM 信号线不建议直接从舵机驱动板绕太长如果信号线超过 20cm可以加 1kΩ 串联电阻电位器中间抽头接 ADC另外两端分别接 3.3V 和 GND不要接 5V否则会超过 STM32F103 ADC 输入范围。5. STM32F103 工程初始化5.1 用 CubeMX 创建基础工程在 STM32CubeMX 中需要开启以下资源RCC外部晶振或内部晶振SYSDebug Serial Wire定时器一个用于产生 6 路 PWM一个用于产生 10ms 系统节拍ADC至少 6 个通道用于角度传感器USART1用于调试和串口指令GPIO三个按键分别是记录、回放、停止。如果 MCU 的定时器通道不够可以使用两组定时器例如舵机定时器/通道说明舵机 0TIM2_CH1示例映射按实际接线调整舵机 1TIM2_CH2示例映射舵机 2TIM2_CH3示例映射舵机 3TIM2_CH4示例映射舵机 4TIM3_CH1示例映射舵机 5TIM3_CH2示例映射角度传感器供电为 3.3V 时ADC 精度按 12 位处理分辨率约 0.08° 左右已经足够用于舵机示教回放。5.2 定时器参数建议PWM 定时器配置如下时钟源内部时钟 72MHz 预分频 PSC71 计数周期 ARR19999 输出比较模式PWM Mode 1 脉冲初始值1500对应 1.5ms 中位系统节拍定时器可以配置为节拍周期10ms 中断优先级高于主循环10ms 节拍用于轨迹记录和回放刷新。如果希望轨迹更细腻也可以改成 5ms但要注意记录点数会增加RAM 消耗也随之翻倍。6. 核心代码编写下面开始编写机械臂示教代码。为了便于阅读代码按功能拆成几个文件你可以直接新建到自己的 Keil/CubeIDE 工程中。6.1 舵机 PWM 模块新建servo_ctrl.h#ifndef __SERVO_CTRL_H #define __SERVO_CTRL_H #include main.h #include stdint.h #define SERVO_NUM 6U void Servo_InitAll(void); void Servo_SetJointAngle(uint8_t index, float angleDeg); void Servo_TorqueOffAll(void); uint32_t Servo_AngleToPulse(float angleDeg); #endif新建servo_ctrl.c#include servo_ctrl.h // 如果使用 CubeMX 生成htim2、htim3 是全局变量 extern TIM_HandleTypeDef htim2; extern TIM_HandleTypeDef htim3; // 每个舵机使用的定时器句柄按实际接线修改 static TIM_HandleTypeDef* servoTimer[SERVO_NUM] { htim2, htim2, htim2, htim2, htim3, htim3 }; // 每个舵机使用的定时器通道 static uint32_t servoChannel[SERVO_NUM] { TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3, TIM_CHANNEL_4, TIM_CHANNEL_1, TIM_CHANNEL_2 }; void Servo_InitAll(void) { // PWM 定时器已经在 MX_TIM2_Init / MX_TIM3_Init 中初始化 // 这里开始输出中位脉冲 for (uint8_t i 0; i SERVO_NUM; i) { __HAL_TIM_SET_COMPARE(servoTimer[i], servoChannel[i], 1500); HAL_TIM_PWM_Start(servoTimer[i], servoChannel[i]); } } uint32_t Servo_AngleToPulse(float angleDeg) { // 控制角度限制在 0~180 if (angleDeg 0.0f) { angleDeg 0.0f; } if (angleDeg 180.0f) { angleDeg 180.0f; } // 0.5ms5002.0ms范围2000定时器计数频率1MHz return (uint32_t)(500.0f (angleDeg / 180.0f) * 2000.0f); } void Servo_SetJointAngle(uint8_t index, float angleDeg) { if (index SERVO_NUM) { return; } uint32_t pulse Servo_AngleToPulse(angleDeg); __HAL_TIM_SET_COMPARE(servoTimer[index], servoChannel[index], pulse); } void Servo_TorqueOffAll(void) { // 注意普通 PWM 舵机停止 PWM 不一定等于“脱力” // 如果实测无法拖动需要在舵机电源回路加 MOSFET 开关 for (uint8_t i 0; i SERVO_NUM; i) { HAL_TIM_PWM_Stop(servoTimer[i], servoChannel[i]); } }这里把Servo_TorqueOffAll()留作对外接口具体如何断电需要根据硬件调整。若你的舵机在PWM_Stop后依然锁死请在示教前断开舵机电源这也是很多低成本机械臂最常采用的硬件实现。6.2 关节角度采集模块假设使用 ADC 连续采样DMA 把六个通道的结果放到数组adcRaw[6]中。新建joint_sensor.h#ifndef __JOINT_SENSOR_H #define __JOINT_SENSOR_H #include main.h #include stdint.h #define JOINT_NUM 6U void JointSensor_Init(void); float JointSensor_GetAngleDeg(uint8_t index); #endif新建joint_sensor.c#include joint_sensor.h extern ADC_HandleTypeDef hadc1; static volatile uint16_t adcRaw[JOINT_NUM]; void JointSensor_Init(void) { // 启动 ADC DMA连续转换 6 个通道 HAL_ADC_Start_DMA(hadc1, (uint32_t *)adcRaw, JOINT_NUM); } float JointSensor_GetAngleDeg(uint8_t index) { if (index JOINT_NUM) { return 0.0f; } // 传感器输出电压 0~3.3V对应 0~4095 // 机械量程按 0~180° 近似实际装配后需要校准 float angle ((float)adcRaw[index]) / 4095.0f * 180.0f; return angle; }以上代码只适用于“电位器整圈对应 0~180°”的理想情况。真实安装中电位器起始角度不一定和机械零点重合而且每个关节机械限位不同。因此实际工程中还要加一个校准偏移量和缩放系数typedef struct { float offsetDeg; // 零点偏移 float scale; // 单位 ADC 值对应角度 } JointCalib_t; static const JointCalib_t jointCalib[JOINT_NUM] { {0.0f, 180.0f / 4095.0f}, {0.0f, 180.0f / 4095.0f}, {0.0f, 180.0f / 4095.0f}, {0.0f, 180.0f / 4095.0f}, {0.0f, 180.0f / 4095.0f}, {0.0f, 180.0f / 4095.0f}, }; float JointSensor_GetAngleDeg(uint8_t index) { if (index JOINT_NUM) { return 0.0f; } float raw (float)adcRaw[index]; return raw * jointCalib[index].scale jointCalib[index].offsetDeg; }每个材料包机械结构不同强烈建议在正式示教前做一次“绝对零点对齐”把机械臂摆到一个容易测量的基准姿态用上位机或串口打印此刻六个 ADC 读数将读数换算成角度记录到上面的校准结构体里。6.3 示教记录与回放模块这是整个功能的核心包含状态机、记录缓冲区和插值回放算法。新建drag_teach.h#ifndef __DRAG_TEACH_H #define __DRAG_TEACH_H #include stdint.h #define JOINT_NUM 6U #define MAX_POINTS 600U // 最大采样点数 #define SAMPLE_DT_MS 20U // 采样周期 20ms typedef enum { ARM_STATE_IDLE 0, ARM_STATE_RECORD, ARM_STATE_PLAY } ArmState_t; typedef struct { uint16_t pointCount; int16_t jointAngle[MAX_POINTS][JOINT_NUM]; // 单位0.1° } Trajectory_t; void DragTeach_StartRecord(void); void DragTeach_StartPlay(float speed); void DragTeach_Stop(void); void DragTeach_10msTick(void); ArmState_t DragTeach_GetState(void); #endif新建drag_teach.c#include drag_teach.h #include servo_ctrl.h #include joint_sensor.h static Trajectory_t gTraj; static ArmState_t gState ARM_STATE_IDLE; static float gPlaySpeed 1.0f; static uint32_t gPlayTimeMs 0; static uint16_t gRecordTick 0; void DragTeach_StartRecord(void) { if (gState ! ARM_STATE_IDLE) { return; } gTraj.pointCount 0; gRecordTick 0; // 示教前舵机释放扭矩 Servo_TorqueOffAll(); gState ARM_STATE_RECORD; } void DragTeach_StartPlay(float speed) { if (gState ! ARM_STATE_IDLE) { return; } if (gTraj.pointCount 2) { // 没有有效轨迹 gState ARM_STATE_IDLE; return; } gPlaySpeed speed; gPlayTimeMs 0; // 重新使能舵机 Servo_InitAll(); gState ARM_STATE_PLAY; } void DragTeach_Stop(void) { gState ARM_STATE_IDLE; } ArmState_t DragTeach_GetState(void) { return gState; } static void Record_Tick(void) { if (gTraj.pointCount MAX_POINTS) { // 记录缓冲区已满自动结束 gState ARM_STATE_IDLE; return; } uint16_t idx gTraj.pointCount; for (uint8_t j 0; j JOINT_NUM; j) { float deg JointSensor_GetAngleDeg(j); gTraj.jointAngle[idx][j] (int16_t)(deg * 10.0f); } gTraj.pointCount; } // 根据当前时间对某关节做线性插值 static float InterpAngle(uint8_t joint, uint32_t timeMs) { if (gTraj.pointCount 2) { return JointSensor_GetAngleDeg(joint); } float t (float)timeMs / (float)SAMPLE_DT_MS; uint16_t i1 (uint