一、系统架构概览
┌─────────────────────────────────────────────────────────────┐
│ 4轴运动控制系统架构 │
├─────────────────────────────────────────────────────────────┤
│ 上位机 (PC/Qt) │ USB/串口 │ STM32F103 │ 驱动电路 │
│ (Candle/自制) │ 通信协议 │ (GRBL核心) │ (LV8728) │
│ │ │ │ │
│ • G-code解析 │ • 115200 │ • 轨迹规划 │ • 4路步进 │
│ • 可视化预览 │ • 实时状态 │ • 插补算法 │ • 1/128细分 │
│ • 参数配置 │ • 急停控制 │ • S型加减速 │ • 过流保护 │
└─────────────────────────────────────────────────────────────┘二、核心源代码实现
2.1 系统配置文件 (system.h)
c
/**
* @file system.h
* @brief 4轴运动控制系统配置
*/
#ifndef __SYSTEM_H
#define __SYSTEM_H
#include "stm32f10x.h"
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
// ==================== 硬件配置 ====================
#define MCU_STM32F103RET6 1
#define AXIS_COUNT 4 // X, Y, Z, A四轴
#define STEPPER_COUNT AXIS_COUNT
// 引脚定义 (根据实际PCB连接修改)
#define X_STEP_PIN GPIO_Pin_0
#define X_DIR_PIN GPIO_Pin_1
#define X_ENABLE_PIN GPIO_Pin_2
#define X_LIMIT_PIN GPIO_Pin_3
#define Y_STEP_PIN GPIO_Pin_4
#define Y_DIR_PIN GPIO_Pin_5
#define Y_ENABLE_PIN GPIO_Pin_6
#define Y_LIMIT_PIN GPIO_Pin_7
#define Z_STEP_PIN GPIO_Pin_8
#define Z_DIR_PIN GPIO_Pin_9
#define Z_ENABLE_PIN GPIO_Pin_10
#define Z_LIMIT_PIN GPIO_Pin_11
#define A_STEP_PIN GPIO_Pin_12
#define A_DIR_PIN GPIO_Pin_13
#define A_ENABLE_PIN GPIO_Pin_14
#define A_LIMIT_PIN GPIO_Pin_15
// ==================== 运动参数 ====================
#define DEFAULT_STEPS_PER_MM_X 6400.0f // 1.8°电机，16细分
#define DEFAULT_STEPS_PER_MM_Y 6400.0f
#define DEFAULT_STEPS_PER_MM_Z 6400.0f
#define DEFAULT_STEPS_PER_MM_A 6400.0f
#define DEFAULT_MAX_FEED_RATE 3000.0f // mm/min
#define DEFAULT_ACCELERATION 500.0f // mm/s²
#define DEFAULT_JERK 10.0f // mm/s³ (S型曲线)
// ==================== 系统状态 ====================
typedef enum {
STATE_IDLE = 0,
STATE_ALARM,
STATE_CHECK_MODE,
STATE_HOMING,
STATE_CYCLE,
STATE_HOLD,
STATE_JOG,
STATE_SAFETY_DOOR
} SystemState;
typedef enum {
AXIS_X = 0,
AXIS_Y = 1,
AXIS_Z = 2,
AXIS_A = 3
} AxisIndex;
// ==================== 运动数据结构 ====================
typedef struct {
float steps_per_mm[AXIS_COUNT];
float max_rate[AXIS_COUNT];
float acceleration[AXIS_COUNT];
float junction_deviation;
float arc_tolerance;
float jerk[AXIS_COUNT];
} Settings_t;
typedef struct {
int32_t position[AXIS_COUNT]; // 当前位置（步数）
int32_t target_position[AXIS_COUNT]; // 目标位置（步数）
float current_rate[AXIS_COUNT]; // 当前速度
float target_rate[AXIS_COUNT]; // 目标速度
SystemState state;
bool homing_complete;
} System_t;
extern Settings_t settings;
extern System_t system;
#endif /* __SYSTEM_H */2.2 步进电机驱动核心 (stepper.c)
c
/**
* @file stepper.c
* @brief 4轴步进电机核心驱动（移植自GRBL 1.1）
*/
#include "system.h"
#include "stepper.h"
#include "motion_control.h"
#include "planner.h"
// 步进电机状态机
typedef struct {
uint8_t step_outbits; // 步进输出位
uint8_t dir_outbits; // 方向输出位
uint8_t step_port_invert_mask;
uint8_t dir_port_invert_mask;
uint8_t enable_port_invert_mask;
uint32_t step_pulse_time; // 脉冲持续时间(us)
uint32_t step_delay; // 步进延迟(us)
int32_t steps_remaining[AXIS_COUNT];
int32_t step_counter[AXIS_COUNT];
bool direction[AXIS_COUNT];
} Stepper_t;
static Stepper_t stepper;
// 定时器1中断服务函数（步进脉冲生成）
void TIM1_UP_IRQHandler(void) {
if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET) {
TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
// 执行步进脉冲
if (stepper.steps_remaining[AXIS_X] > 0) {
GPIO_SetBits(GPIOA, X_STEP_PIN);
stepper.steps_remaining[AXIS_X]--;
stepper.step_counter[AXIS_X]++;
}
if (stepper.steps_remaining[AXIS_Y] > 0) {
GPIO_SetBits(GPIOA, Y_STEP_PIN);
stepper.steps_remaining[AXIS_Y]--;
stepper.step_counter[AXIS_Y]++;
}
if (stepper.steps_remaining[AXIS_Z] > 0) {
GPIO_SetBits(GPIOA, Z_STEP_PIN);
stepper.steps_remaining[AXIS_Z]--;
stepper.step_counter[AXIS_Z]++;
}
if (stepper.steps_remaining[AXIS_A] > 0) {
GPIO_SetBits(GPIOA, A_STEP_PIN);
stepper.steps_remaining[AXIS_A]--;
stepper.step_counter[AXIS_A]++;
}
// 清除脉冲（脉冲宽度控制）
delay_us(stepper.step_pulse_time);
GPIO_ResetBits(GPIOA, X_STEP_PIN | Y_STEP_PIN | Z_STEP_PIN | A_STEP_PIN);
}
}
// 初始化步进电机
void st_init(void) {
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
// 1. 初始化GPIO
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
GPIO_InitStructure.GPIO_Pin = X_STEP_PIN | X_DIR_PIN | X_ENABLE_PIN |
Y_STEP_PIN | Y_DIR_PIN | Y_ENABLE_PIN |
Z_STEP_PIN | Z_DIR_PIN | Z_ENABLE_PIN |
A_STEP_PIN | A_DIR_PIN | A_ENABLE_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// 2. 初始化定时器1（步进脉冲生成）
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
TIM_TimeBaseStructure.TIM_Period = 1000 - 1; // 1kHz基础频率
TIM_TimeBaseStructure.TIM_Prescaler = 72 - 1; // 72MHz/72 = 1MHz
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
// 3. 配置中断
NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// 4. 启动定时器
TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
TIM_Cmd(TIM1, ENABLE);
// 5. 初始化步进电机状态
memset(&stepper, 0, sizeof(Stepper_t));
stepper.step_pulse_time = 5; // 5us脉冲宽度
stepper.step_delay = 100; // 100us步进间隔
// 6. 使能所有轴
GPIO_ResetBits(GPIOA, X_ENABLE_PIN | Y_ENABLE_PIN | Z_ENABLE_PIN | A_ENABLE_PIN);
}
// 设置步进脉冲
void st_set_step(uint8_t axis, bool state) {
switch(axis) {
case AXIS_X:
if(state) GPIO_SetBits(GPIOA, X_STEP_PIN);
else GPIO_ResetBits(GPIOA, X_STEP_PIN);
break;
case AXIS_Y:
if(state) GPIO_SetBits(GPIOA, Y_STEP_PIN);
else GPIO_ResetBits(GPIOA, Y_STEP_PIN);
break;
case AXIS_Z:
if(state) GPIO_SetBits(GPIOA, Z_STEP_PIN);
else GPIO_ResetBits(GPIOA, Z_STEP_PIN);
break;
case AXIS_A:
if(state) GPIO_SetBits(GPIOA, A_STEP_PIN);
else GPIO_ResetBits(GPIOA, A_STEP_PIN);
break;
}
}
// 设置方向
void st_set_direction(uint8_t axis, bool direction) {
switch(axis) {
case AXIS_X:
if(direction) GPIO_SetBits(GPIOA, X_DIR_PIN);
else GPIO_ResetBits(GPIOA, X_DIR_PIN);
break;
case AXIS_Y:
if(direction) GPIO_SetBits(GPIOA, Y_DIR_PIN);
else GPIO_ResetBits(GPIOA, Y_DIR_PIN);
break;
case AXIS_Z:
if(direction) GPIO_SetBits(GPIOA, Z_DIR_PIN);
else GPIO_ResetBits(GPIOA, Z_DIR_PIN);
break;
case AXIS_A:
if(direction) GPIO_SetBits(GPIOA, A_DIR_PIN);
else GPIO_ResetBits(GPIOA, A_DIR_PIN);
break;
}
}
// 执行直线运动
void st_linear_move(int32_t *target_steps, float feed_rate) {
uint8_t axis;
// 计算各轴需要移动的步数
for(axis = 0; axis < AXIS_COUNT; axis++) {
stepper.steps_remaining[axis] = abs(target_steps[axis] - system.position[axis]);
stepper.direction[axis] = (target_steps[axis] > system.position[axis]);
// 设置方向引脚
st_set_direction(axis, stepper.direction[axis]);
// 更新系统位置
system.position[axis] = target_steps[axis];
}
// 计算主轴（移动步数最多的轴）
uint32_t max_steps = 0;
uint8_t master_axis = 0;
for(axis = 0; axis < AXIS_COUNT; axis++) {
if(stepper.steps_remaining[axis] > max_steps) {
max_steps = stepper.steps_remaining[axis];
master_axis = axis;
}
}
// 计算步进间隔
if(max_steps > 0) {
float step_frequency = (feed_rate * settings.steps_per_mm[master_axis]) / 60.0f;
uint32_t step_period_us = (uint32_t)(1000000.0f / step_frequency);
// 设置定时器周期
TIM_SetAutoreload(TIM1, step_period_us - 1);
}
}2.3 运动控制核心 (motion_control.c)
c
/**
* @file motion_control.c
* @brief 运动控制核心（直线/圆弧插补、S型加减速）
*/
#include "system.h"
#include "motion_control.h"
#include "stepper.h"
#include "planner.h"
// 直线插补
void mc_line(float *target, plan_line_data_t *pl_data) {
int32_t target_steps[AXIS_COUNT];
uint8_t axis;
// 1. 将毫米坐标转换为步数
for(axis = 0; axis < AXIS_COUNT; axis++) {
target_steps[axis] = (int32_t)(target[axis] * settings.steps_per_mm[axis]);
}
// 2. 添加到规划器缓冲区
plan_buffer_line(target_steps, pl_data);
// 3. 执行运动
st_linear_move(target_steps, pl_data->feed_rate);
}
// 圆弧插补（G02/G03）
void mc_arc(float *target, float *offset, float radius, uint8_t axis_0, uint8_t axis_1, uint8_t is_clockwise) {
// 圆弧插补算法（DDA数字微分分析法）
float center_x = system.position[axis_0] / settings.steps_per_mm[axis_0] + offset[0];
float center_y = system.position[axis_1] / settings.steps_per_mm[axis_1] + offset[1];
float start_angle = atan2(system.position[axis_1] / settings.steps_per_mm[axis_1] - center_y,
system.position[axis_0] / settings.steps_per_mm[axis_0] - center_x);
float end_angle = atan2(target[axis_1] - center_y, target[axis_0] - center_x);
float angle_diff = end_angle - start_angle;
if(is_clockwise) {
if(angle_diff > 0) angle_diff -= 2 * M_PI;
} else {
if(angle_diff < 0) angle_diff += 2 * M_PI;
}
// 离散化为小线段
uint16_t segments = (uint16_t)(fabs(angle_diff) * radius * 10); // 每段0.1mm
float angle_increment = angle_diff / segments;
float current_angle = start_angle;
float interpolated_target[AXIS_COUNT];
for(uint16_t i = 0; i < segments; i++) {
current_angle += angle_increment;
interpolated_target[axis_0] = center_x + radius * cos(current_angle);
interpolated_target[axis_1] = center_y + radius * sin(current_angle);
interpolated_target[2] = target[2]; // Z轴保持不变
interpolated_target[3] = target[3]; // A轴保持不变
mc_line(interpolated_target, NULL);
}
}
// S型加减速规划
void mc_s_curve_acceleration(plan_block_t *block) {
float t = 0.0f;
float dt = 0.001f; // 1ms时间步长
float current_velocity = 0.0f;
float current_acceleration = 0.0f;
while(t < block->acceleration_time) {
// 计算加加速度（Jerk）
float jerk = settings.jerk[0];
// 计算加速度
current_acceleration += jerk * dt;
if(current_acceleration > settings.acceleration[0]) {
current_acceleration = settings.acceleration[0];
}
// 计算速度
current_velocity += current_acceleration * dt;
if(current_velocity > block->max_entry_speed) {
current_velocity = block->max_entry_speed;
}
// 更新规划器
block->entry_speed = current_velocity;
t += dt;
}
}2.4 路径规划器 (planner.c)
c
/**
* @file planner.c
* @brief 路径规划器（前瞻控制、速度规划）
*/
#include "system.h"
#include "planner.h"
#define BLOCK_BUFFER_SIZE 16
typedef struct {
int32_t steps[AXIS_COUNT];
float entry_speed;
float exit_speed;
float max_entry_speed;
float acceleration;
float nominal_speed;
float millimeters;
bool recalculate_flag;
} plan_block_t;
typedef struct {
plan_block_t block_buffer[BLOCK_BUFFER_SIZE];
uint8_t write_index;
uint8_t read_index;
uint8_t available_blocks;
} plan_buffer_t;
static plan_buffer_t plan_buffer;
// 初始化规划器
void plan_init(void) {
memset(&plan_buffer, 0, sizeof(plan_buffer_t));
}
// 前瞻速度规划（Look-ahead）
void plan_lookahead_speed_planning(void) {
uint8_t current_index = plan_buffer.read_index;
uint8_t previous_index = (current_index == 0) ? BLOCK_BUFFER_SIZE - 1 : current_index - 1;
// 计算拐角速度限制
float junction_speed = settings.junction_deviation;
// 限制当前块的退出速度
if(plan_buffer.block_buffer[current_index].available_blocks > 0) {
plan_block_t *current_block = &plan_buffer.block_buffer[current_index];
plan_block_t *previous_block = &plan_buffer.block_buffer[previous_index];
// 计算拐角角度
float cos_theta = 0.0f;
for(uint8_t axis = 0; axis < AXIS_COUNT; axis++) {
cos_theta += (current_block->steps[axis] - previous_block->steps[axis]) *
(current_block->steps[axis] - previous_block->steps[axis]);
}
// 应用拐角速度限制
float limited_speed = junction_speed * fabs(cos_theta);
if(current_block->exit_speed > limited_speed) {
current_block->exit_speed = limited_speed;
}
}
}
// 添加直线到规划器
void plan_buffer_line(int32_t *steps, plan_line_data_t *pl_data) {
// 检查缓冲区是否满
if(plan_buffer.available_blocks >= BLOCK_BUFFER_SIZE) {
return; // 缓冲区满，等待
}
// 获取当前写入位置
plan_block_t *block = &plan_buffer.block_buffer[plan_buffer.write_index];
// 填充块数据
memcpy(block->steps, steps, sizeof(int32_t) * AXIS_COUNT);
block->entry_speed = pl_data->feed_rate;
block->max_entry_speed = pl_data->feed_rate;
block->acceleration = settings.acceleration[0];
block->recalculate_flag = true;
// 更新写入索引
plan_buffer.write_index = (plan_buffer.write_index + 1) % BLOCK_BUFFER_SIZE;
plan_buffer.available_blocks++;
// 执行前瞻规划
plan_lookahead_speed_planning();
}2.5 G代码解析器 (gcode.c)
c
/**
* @file gcode.c
* @brief G代码解析器
*/
#include "system.h"
#include "gcode.h"
#include "motion_control.h"
typedef struct {
float position[AXIS_COUNT];
float feed_rate;
uint8_t modal_group;
bool absolute_mode;
} gc_state_t;
static gc_state_t gc_state;
// 初始化G代码解析器
void gc_init(void) {
memset(&gc_state, 0, sizeof(gc_state_t));
gc_state.absolute_mode = true;
gc_state.feed_rate = settings.max_rate[0];
}
// 解析G代码行
uint8_t gc_execute_line(char *line) {
char *letter;
float value;
uint8_t group = 0;
// 移除空格和注释
gc_remove_spaces_and_comments(line);
letter = line;
while(*letter) {
if(*letter >= 'A' && *letter <= 'Z') {
char command = *letter;
letter++;
// 解析数值
value = gc_parse_float(&letter);
switch(command) {
case 'G': // G代码
gc_execute_g_command((uint16_t)value);
break;
case 'M': // M代码
gc_execute_m_command((uint16_t)value);
break;
case 'X': // X坐标
gc_state.position[AXIS_X] = value;
break;
case 'Y': // Y坐标
gc_state.position[AXIS_Y] = value;
break;
case 'Z': // Z坐标
gc_state.position[AXIS_Z] = value;
break;
case 'A': // A坐标
gc_state.position[AXIS_A] = value;
break;
case 'F': // 进给速度
gc_state.feed_rate = value;
break;
}
} else {
letter++;
}
}
return GC_OK;
}
// 执行G代码命令
void gc_execute_g_command(uint16_t code) {
plan_line_data_t pl_data;
pl_data.feed_rate = gc_state.feed_rate;
switch(code) {
case 0: // 快速定位
pl_data.feed_rate = settings.max_rate[0];
mc_line(gc_state.position, &pl_data);
break;
case 1: // 直线插补
mc_line(gc_state.position, &pl_data);
break;
case 2: // 顺时针圆弧
// 需要解析圆心偏移量I,J,K
break;
case 3: // 逆时针圆弧
break;
case 28: // 回原点
mc_homing_cycle();
break;
case 90: // 绝对坐标
gc_state.absolute_mode = true;
break;
case 91: // 相对坐标
gc_state.absolute_mode = false;
break;
}
}2.6 主程序 (main.c)
c
/**
* @file main.c
* @brief 4轴运动控制系统主程序
*/
#include "system.h"
#include "stepper.h"
#include "motion_control.h"
#include "planner.h"
#include "gcode.h"
#include "serial.h"
int main(void) {
// 1. 系统初始化
SystemInit();
serial_init(115200);
st_init();
plan_init();
gc_init();
printf("4-Axis Motion Control System Ready!\r\n");
printf("GRBL 1.1 Compatible\r\n");
// 2. 回原点
mc_homing_cycle();
// 3. 主循环
char gcode_line[128];
uint8_t line_index = 0;
while(1) {
// 从串口接收G代码
if(serial_available()) {
char c = serial_read();
if(c == '\n' || c == '\r') {
gcode_line[line_index] = '\0';
if(line_index > 0) {
// 执行G代码
uint8_t result = gc_execute_line(gcode_line);
if(result == GC_OK) {
printf("ok\r\n");
} else {
printf("error:%d\r\n", result);
}
}
line_index = 0;
} else if(line_index < 127) {
gcode_line[line_index++] = c;
}
}
// 执行运动控制
mc_run();
// 系统状态监控
if(system.state == STATE_IDLE) {
// 空闲状态，可以执行其他任务
}
delay_ms(1);
}
}参考代码 4轴运动控制 原理图 PCB图 源代码 www.youwenfan.com/contentcsu/60460.html
三、编译与部署
3.1 编译环境
- IDE: Keil MDK-ARM 或 STM32CubeIDE
- 编译器: ARM GCC
- 调试器: ST-Link V2
3.2 编译指令
bash
# 使用Makefile编译
make clean
make all
# 或使用STM32CubeIDE直接编译3.3 烧录到STM32
- 使用ST-Link Utility或STM32CubeProgrammer
- 选择生成的
.hex或.bin文件 - 烧录到STM32F103RET6
四、上位机软件
4.1 Qt Candle上位机（推荐）
cpp
// 简单的Qt串口通信示例
void MainWindow::sendGCode(QString gcode) {
QByteArray data = gcode.toUtf8() + "\n";
serialPort->write(data);
}
void MainWindow::on_pushButton_MoveX_clicked() {
sendGCode("G0 X10 F3000");
}4.2 测试G代码
G21 ; 设置单位为毫米
G90 ; 绝对坐标模式
G0 X0 Y0 Z0 A0 ; 快速移动到原点
G1 X100 Y50 F3000 ; 直线插补到(100,50)
G2 X150 Y100 I25 J0 F2000 ; 顺时针圆弧
M03 S1000 ; 主轴启动
G0 Z10 ; Z轴上升
M05 ; 主轴停止