STM32与TC78H653FTG直流有刷电机驱动方案详解

1. 项目概述直流有刷电机驱动方案在工业自动化和消费电子领域直流有刷电机因其结构简单、控制方便、成本低廉等优势仍然是许多应用场景的首选驱动方案。本项目采用东芝公司的TC78H653FTG H桥驱动芯片与ST公司的STM32F412RE微控制器组合构建了一套高性能的直流有刷电机控制系统。TC78H653FTG是一款集成MOSFET的H桥驱动器支持3.5A持续电流输出峰值可达6A工作电压范围覆盖7V至28V。其内置的保护功能包括过流保护、过热关断和欠压锁定为电机驱动提供了硬件级的安全保障。STM32F412RE则是一款基于ARM Cortex-M4内核的微控制器运行频率高达100MHz具备丰富的定时器资源和硬件PWM输出特别适合电机控制应用。这套组合方案的价值在于TC78H653FTG解决了功率驱动部分的效率和散热问题而STM32F412RE则提供了灵活的控制算法实现平台。两者结合既保证了驱动性能又能实现速度闭环、位置控制等高级功能显著提升了直流有刷电机的应用潜力。2. 硬件设计与关键元件选型2.1 TC78H653FTG驱动电路设计TC78H653FTG采用HSSOP36封装尺寸紧凑但散热性能良好。其典型应用电路包含以下几个关键部分电源设计电机电源VM与逻辑电源VCC需要分别供电建议在VM引脚就近放置100μF电解电容并联0.1μF陶瓷电容VCC引脚需加0.1μF去耦电容输入接口设计IN1/IN2为方向控制引脚建议通过100Ω电阻连接MCUPWM输入引脚建议串联22Ω电阻以减小振铃使能引脚(ENABLE)应配置上拉电阻保护电路设计在电机两端并联100nF电容和肖特基二极管组成消弧电路电流检测电阻0.1Ω/2W连接在SR引脚与地之间散热考虑PCB设计时应预留足够的铜箔面积帮助散热持续大电流工作时建议添加散热片2.2 STM32F412RE接口设计STM32F412RE与TC78H653FTG的典型连接方式// GPIO配置 GPIO_InitTypeDef GPIO_InitStruct {0}; GPIO_InitStruct.Pin GPIO_PIN_0|GPIO_PIN_1; // IN1, IN2 GPIO_InitStruct.Mode GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull GPIO_NOPULL; GPIO_InitStruct.Speed GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(GPIOA, GPIO_InitStruct); // 高级定时器PWM配置 TIM_HandleTypeDef htim1; TIM_OC_InitTypeDef sConfigOC {0}; htim1.Instance TIM1; htim1.Init.Prescaler 0; htim1.Init.CounterMode TIM_COUNTERMODE_UP; htim1.Init.Period 999; // 10kHz PWM 100MHz htim1.Init.ClockDivision TIM_CLOCKDIVISION_DIV1; HAL_TIM_PWM_Init(htim1); sConfigOC.OCMode TIM_OCMODE_PWM1; sConfigOC.Pulse 500; // 初始占空比50% sConfigOC.OCPolarity TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode TIM_OCFAST_DISABLE; HAL_TIM_PWM_ConfigChannel(htim1, sConfigOC, TIM_CHANNEL_1);2.3 关键元件选型考量电机选择注意事项额定电压应与驱动电路匹配堵转电流不应超过TC78H653FTG的峰值电流能力对于需要频繁启停的应用应选择惯性较小的电机电源选型建议使用开关电源以提高效率电源电流能力应为电机额定电流的1.5倍以上考虑添加π型滤波电路减少电源噪声传感器选择如需闭环控制编码器增量式或绝对式根据精度需求选择霍尔传感器成本较低适合简单测速电流传感器ACS712等霍尔效应传感器3. 软件架构与核心算法实现3.1 基础驱动层实现电机驱动基本函数// 电机停止 void Motor_Stop(void) { HAL_GPIO_WritePin(IN1_GPIO_Port, IN1_Pin, GPIO_PIN_RESET); HAL_GPIO_WritePin(IN2_GPIO_Port, IN2_Pin, GPIO_PIN_RESET); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, 0); } // 电机正转 void Motor_CW(uint16_t speed) { HAL_GPIO_WritePin(IN1_GPIO_Port, IN1_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(IN2_GPIO_Port, IN2_Pin, GPIO_PIN_RESET); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, speed); } // 电机反转 void Motor_CCW(uint16_t speed) { HAL_GPIO_WritePin(IN1_GPIO_Port, IN1_Pin, GPIO_PIN_RESET); HAL_GPIO_WritePin(IN2_GPIO_Port, IN2_Pin, GPIO_PIN_SET); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, speed); }3.2 速度闭环控制实现PID控制器实现typedef struct { float Kp; float Ki; float Kd; float integral; float prev_error; float output_limit; } PID_Controller; void PID_Init(PID_Controller *pid, float Kp, float Ki, float Kd, float limit) { pid-Kp Kp; pid-Ki Ki; pid-Kd Kd; pid-integral 0; pid-prev_error 0; pid-output_limit limit; } float PID_Update(PID_Controller *pid, float setpoint, float measurement, float dt) { float error setpoint - measurement; // 比例项 float proportional pid-Kp * error; // 积分项抗饱和处理 pid-integral error * dt; if(pid-integral pid-output_limit) pid-integral pid-output_limit; else if(pid-integral -pid-output_limit) pid-integral -pid-output_limit; float integral pid-Ki * pid-integral; // 微分项 float derivative pid-Kd * (error - pid-prev_error) / dt; pid-prev_error error; // 计算输出并限幅 float output proportional integral derivative; if(output pid-output_limit) output pid-output_limit; else if(output -pid-output_limit) output -pid-output_limit; return output; }3.3 位置控制实现基于编码器的位置控制// 编码器接口配置使用TIM2作为编码器接口 void Encoder_Init(void) { TIM_Encoder_InitTypeDef encoder_config {0}; TIM_MasterConfigTypeDef master_config {0}; htim2.Instance TIM2; htim2.Init.Prescaler 0; htim2.Init.CounterMode TIM_COUNTERMODE_UP; htim2.Init.Period 0xFFFF; htim2.Init.ClockDivision TIM_CLOCKDIVISION_DIV1; encoder_config.EncoderMode TIM_ENCODERMODE_TI12; encoder_config.IC1Polarity TIM_ICPOLARITY_RISING; encoder_config.IC1Selection TIM_ICSELECTION_DIRECTTI; encoder_config.IC1Prescaler TIM_ICPSC_DIV1; encoder_config.IC1Filter 0; encoder_config.IC2Polarity TIM_ICPOLARITY_RISING; encoder_config.IC2Selection TIM_ICSELECTION_DIRECTTI; encoder_config.IC2Prescaler TIM_ICPSC_DIV1; encoder_config.IC2Filter 0; HAL_TIM_Encoder_Init(htim2, encoder_config); master_config.MasterOutputTrigger TIM_TRGO_RESET; master_config.MasterSlaveMode TIM_MASTERSLAVEMODE_DISABLE; HAL_TIMEx_MasterConfigSynchronization(htim2, master_config); HAL_TIM_Encoder_Start(htim2, TIM_CHANNEL_ALL); } // 位置控制函数 void Position_Control(float target_angle) { static PID_Controller pos_pid; static PID_Controller speed_pid; static uint32_t last_tick 0; // 初始化PID控制器仅第一次调用时执行 static uint8_t initialized 0; if(!initialized) { PID_Init(pos_pid, 10.0f, 0.0f, 0.5f, 1000.0f); // 位置环PID参数 PID_Init(speed_pid, 0.5f, 0.1f, 0.01f, 1000.0f); // 速度环PID参数 initialized 1; } // 获取当前编码器值假设每转产生400个脉冲 int16_t encoder_count (int16_t)TIM2-CNT; float current_angle (encoder_count / 400.0f) * 360.0f; // 计算时间间隔毫秒 uint32_t current_tick HAL_GetTick(); float dt (current_tick - last_tick) / 1000.0f; last_tick current_tick; // 位置环PID计算输出为目标速度 float target_speed PID_Update(pos_pid, target_angle, current_angle, dt); // 计算当前速度脉冲/秒 static int16_t last_count 0; float current_speed (encoder_count - last_count) / dt; last_count encoder_count; // 速度环PID计算输出为PWM值 float pwm_output PID_Update(speed_pid, target_speed, current_speed, dt); // 执行控制 if(pwm_output 0) { Motor_CW((uint16_t)fabsf(pwm_output)); } else { Motor_CCW((uint16_t)fabsf(pwm_output)); } }4. 系统优化与性能提升技巧4.1 PWM频率优化选择PWM频率的选择需要权衡多个因素电机电感电感较大的电机可以使用较低的PWM频率可闻噪声通常选择高于20kHz以避免可闻噪声开关损耗频率越高MOSFET开关损耗越大电流纹波频率越低电流纹波越大对于大多数小型直流有刷电机10-20kHz的PWM频率是一个较好的折中选择。可以通过以下公式估算最佳频率f_PWM R / (2πL)其中R是电机电阻L是电机电感。实际频率应至少比这个值高5倍。4.2 死区时间配置为防止H桥上下管直通必须设置适当的死区时间。STM32的高级定时器支持硬件死区插入// 配置死区时间约1μs 100MHz TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig {0}; sBreakDeadTimeConfig.OffStateRunMode TIM_OSSR_DISABLE; sBreakDeadTimeConfig.OffStateIDLEMode TIM_OSSI_DISABLE; sBreakDeadTimeConfig.LockLevel TIM_LOCKLEVEL_OFF; sBreakDeadTimeConfig.DeadTime 100; // 100*10ns 1μs sBreakDeadTimeConfig.BreakState TIM_BREAK_DISABLE; sBreakDeadTimeConfig.BreakPolarity TIM_BREAKPOLARITY_HIGH; sBreakDeadTimeConfig.AutomaticOutput TIM_AUTOMATICOUTPUT_DISABLE; HAL_TIMEx_ConfigBreakDeadTime(htim1, sBreakDeadTimeConfig);4.3 电流采样与过流保护利用TC78H653FTG的SR引脚实现电流采样// 配置ADC采样电流 ADC_ChannelConfTypeDef sConfig {0}; sConfig.Channel ADC_CHANNEL_0; sConfig.Rank 1; sConfig.SamplingTime ADC_SAMPLETIME_480CYCLES; HAL_ADC_ConfigChannel(hadc1, sConfig); // 获取电流值mA uint16_t Read_Current(void) { HAL_ADC_Start(hadc1); HAL_ADC_PollForConversion(hadc1, 10); uint16_t adc_value HAL_ADC_GetValue(hadc1); float voltage adc_value * 3.3f / 4095.0f; float current voltage / 0.1f; // 0.1Ω采样电阻 return (uint16_t)(current * 1000); // 返回mA值 } // 过流保护处理 void Overcurrent_Check(void) { static uint32_t last_check 0; if(HAL_GetTick() - last_check 10) { // 每10ms检查一次 uint16_t current Read_Current(); if(current 3500) { // 3.5A过流保护 Motor_Stop(); // 触发错误处理... } last_check HAL_GetTick(); } }4.4 温度监控与过热保护利用STM32内置温度传感器或外接NTC// 读取MCU内部温度传感器 float Read_Internal_Temp(void) { ADC_ChannelConfTypeDef sConfig {0}; sConfig.Channel ADC_CHANNEL_TEMPSENSOR; sConfig.Rank 1; sConfig.SamplingTime ADC_SAMPLETIME_480CYCLES; HAL_ADC_ConfigChannel(hadc1, sConfig); HAL_ADC_Start(hadc1); HAL_ADC_PollForConversion(hadc1, 10); uint16_t adc_value HAL_ADC_GetValue(hadc1); float voltage adc_value * 3.3f / 4095.0f; float temp ((voltage - 0.76f) / 0.0025f) 25.0f; return temp; } // 过热保护处理 void Thermal_Protection(void) { static uint32_t last_check 0; if(HAL_GetTick() - last_check 1000) { // 每1s检查一次 float temp Read_Internal_Temp(); if(temp 85.0f) { // 超过85°C降额运行 // 降低PWM占空比... } if(temp 105.0f) { // 超过105°C完全关断 Motor_Stop(); // 触发错误处理... } last_check HAL_GetTick(); } }5. 常见问题排查与解决方案5.1 电机不转动问题排查检查步骤电源检查测量VM引脚电压是否正常检查VCC引脚是否有5V电压确认接地连接良好信号检查用示波器检查PWM信号是否到达驱动芯片确认方向控制信号电平正确检查使能引脚是否为高电平保护状态检查测量芯片温度是否过高检查电流是否超过限制查看故障标志寄存器如有负载检查断开电机检查是否机械卡死测量电机电阻是否正常5.2 PWM抖动问题解决可能原因及解决方案接地不良改进PCB布局使用星型接地增加电源去耦电容信号干扰缩短PWM信号走线使用双绞线或屏蔽线在信号线上添加RC滤波如100Ω100pF软件问题检查定时器配置是否正确确保没有其他任务占用过多CPU时间使用DMA传输PWM数据电源噪声增加电源滤波电容使用线性稳压器为控制部分供电5.3 电机异常噪声处理噪声类型及解决方案高频啸叫调整PWM频率通常提高频率检查电机轴承状态增加电机两端电容0.1μF陶瓷电容机械振动检查电机安装是否牢固添加橡胶减震垫优化PID参数减少超调换向噪声检查电刷状态适当增加死区时间考虑使用正弦波驱动替代方波电流断续噪声提高PWM频率增加电机电感可串联小电感使用电流连续模式控制5.4 过流保护频繁触发排查步骤检查电机测量电机电阻是否异常检查是否有机械过载测试空载电流是否正常检查驱动电路确认电流检测电阻值正确检查MOSFET是否损坏验证过流保护阈值设置控制策略检查检查启动加速是否过快验证PID参数是否合理考虑添加软启动功能电源问题检查电源电流能力是否足够测量电源电压是否稳定检查电源线是否过细导致压降6. 高级应用与功能扩展6.1 网络通信接口集成基于STM32F412RE的以太网功能实现// 以太网初始化 void ETH_Init(void) { // 引脚配置 __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); __HAL_RCC_GPIOC_CLK_ENABLE(); GPIO_InitTypeDef GPIO_InitStruct {0}; GPIO_InitStruct.Pin GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_7; GPIO_InitStruct.Mode GPIO_MODE_AF_PP; GPIO_InitStruct.Pull GPIO_NOPULL; GPIO_InitStruct.Speed GPIO_SPEED_FREQ_HIGH; GPIO_InitStruct.Alternate GPIO_AF11_ETH; HAL_GPIO_Init(GPIOA, GPIO_InitStruct); // 更多引脚配置... // 以太网外设配置 __HAL_RCC_ETH_CLK_ENABLE(); ETH_MACConfigTypeDef MACConf {0}; MACConf.DuplexMode ETH_FULLDUPLEX_MODE; MACConf.Speed ETH_SPEED_100M; MACConf.ChecksumMode ETH_CHECKSUM_BY_HARDWARE; HAL_ETH_SetMACConfig(heth, MACConf); // 启动以太网 HAL_ETH_Start(heth); } // 简单的电机控制协议处理 void Process_Network_Command(uint8_t *data, uint16_t len) { if(len 3) { uint8_t cmd data[0]; uint16_t value (data[1] 8) | data[2]; switch(cmd) { case 0x01: // 设置速度 if(value 1000) { __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, value); } break; case 0x02: // 设置方向 if(value 0) { Motor_CW(__HAL_TIM_GET_COMPARE(htim1, TIM_CHANNEL_1)); } else { Motor_CCW(__HAL_TIM_GET_COMPARE(htim1, TIM_CHANNEL_1)); } break; // 更多命令... } } }6.2 能量回馈制动实现利用STM32的刹车功能实现快速制动// 配置刹车功能 void Brake_Init(void) { TIM_HandleTypeDef htim3; TIM_OC_InitTypeDef sConfigOC {0}; TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig {0}; htim3.Instance TIM3; htim3.Init.Prescaler 0; htim3.Init.CounterMode TIM_COUNTERMODE_UP; htim3.Init.Period 999; htim3.Init.ClockDivision TIM_CLOCKDIVISION_DIV1; HAL_TIM_PWM_Init(htim3); sConfigOC.OCMode TIM_OCMODE_PWM1; sConfigOC.Pulse 0; sConfigOC.OCPolarity TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode TIM_OCFAST_DISABLE; HAL_TIM_PWM_ConfigChannel(htim3, sConfigOC, TIM_CHANNEL_1); sBreakDeadTimeConfig.OffStateRunMode TIM_OSSR_DISABLE; sBreakDeadTimeConfig.OffStateIDLEMode TIM_OSSI_DISABLE; sBreakDeadTimeConfig.LockLevel TIM_LOCKLEVEL_OFF; sBreakDeadTimeConfig.DeadTime 100; sBreakDeadTimeConfig.BreakState TIM_BREAK_ENABLE; sBreakDeadTimeConfig.BreakPolarity TIM_BREAKPOLARITY_HIGH; sBreakDeadTimeConfig.AutomaticOutput TIM_AUTOMATICOUTPUT_DISABLE; HAL_TIMEx_ConfigBreakDeadTime(htim3, sBreakDeadTimeConfig); HAL_TIM_PWM_Start(htim3, TIM_CHANNEL_1); } // 制动函数 void Motor_Brake(uint16_t intensity) { // 强度限制在0-1000范围内 if(intensity 1000) intensity 1000; // 设置制动PWM占空比 __HAL_TIM_SET_COMPARE(htim3, TIM_CHANNEL_1, intensity); // 同时短接电机两端通过H桥下管 HAL_GPIO_WritePin(IN1_GPIO_Port, IN1_Pin, GPIO_PIN_RESET); HAL_GPIO_WritePin(IN2_GPIO_Port, IN2_Pin, GPIO_PIN_RESET); // 启用刹车输入 HAL_GPIO_WritePin(BRAKE_GPIO_Port, BRAKE_Pin, GPIO_PIN_SET); }6.3 多电机同步控制使用STM32的多定时器实现同步控制// 初始化三个定时器用于多电机控制 void Multi_Motor_Init(void) { // 定时器1 - 电机1 htim1.Instance TIM1; htim1.Init.Prescaler 0; htim1.Init.CounterMode TIM_COUNTERMODE_UP; htim1.Init.Period 999; htim1.Init.ClockDivision TIM_CLOCKDIVISION_DIV1; HAL_TIM_PWM_Init(htim1); // 定时器2 - 电机2 htim2.Instance TIM2; htim2.Init.Prescaler 0; htim2.Init.CounterMode TIM_COUNTERMODE_UP; htim2.Init.Period 999; htim2.Init.ClockDivision TIM_CLOCKDIVISION_DIV1; HAL_TIM_PWM_Init(htim2); // 定时器3 - 电机3 htim3.Instance TIM3; htim3.Init.Prescaler 0; htim3.Init.CounterMode TIM_COUNTERMODE_UP; htim3.Init.Period 999; htim3.Init.ClockDivision TIM_CLOCKDIVISION_DIV1; HAL_TIM_PWM_Init(htim3); // 配置主从模式使三个定时器同步 TIM_MasterConfigTypeDef sMasterConfig {0}; sMasterConfig.MasterOutputTrigger TIM_TRGO_UPDATE; sMasterConfig.MasterSlaveMode TIM_MASTERSLAVEMODE_ENABLE; HAL_TIMEx_MasterConfigSynchronization(htim1, sMasterConfig); TIM_SlaveConfigTypeDef sSlaveConfig {0}; sSlaveConfig.SlaveMode TIM_SLAVEMODE_TRIGGER; sSlaveConfig.InputTrigger TIM_TS_ITR0; // TIM1作为主定时器 HAL_TIM_SlaveConfigSynchronization(htim2, sSlaveConfig); HAL_TIM_SlaveConfigSynchronization(htim3, sSlaveConfig); // 启动所有定时器 HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim3, TIM_CHANNEL_1); }6.4 状态监测与故障诊断实现全面的系统监测功能typedef struct { uint16_t current; uint16_t voltage; int16_t temperature; uint32_t run_time; uint16_t error_code; uint32_t rotation_count; } Motor_Status; Motor_Status motor_status {0}; // 状态监测任务 void Status_Monitor_Task(void) { static uint32_t last_update 0; uint32_t current_time HAL_GetTick(); // 每100ms更新一次状态 if(current_time - last_update 100) { // 读取各项参数 motor_status.current Read_Current(); motor_status.voltage Read_Voltage(); motor_status.temperature (int16_t)Read_Internal_Temp(); // 更新运行时间秒 motor_status.run_time current_time / 1000; // 更新旋转计数假设有编码器 static int16_t last_encoder 0; int16_t current_encoder (int16_t)TIM2-CNT; motor_status.rotation_count abs(current_encoder - last_encoder) / 400; last_encoder current_encoder; // 检查错误条件 motor_status.error_code 0; if(motor_status.current 3500) motor_status.error_code | 0x01; if(motor_status.voltage 5000) motor_status.error_code | 0x02; if(motor_status.temperature 85) motor_status.error_code | 0x04; last_update current_time; } } // 通过串口发送状态报告 void Send_Status_Report(UART_HandleTypeDef *huart) { uint8_t buffer[50]; int len snprintf((char*)buffer, sizeof(buffer), I:%umA V:%umV T:%dC R:%lur E:%02X\n, motor_status.current, motor_status.voltage, motor_status.temperature, motor_status.rotation_count, motor_status.error_code); HAL_UART_Transmit(huart, buffer, len, 100); }7. 实际应用案例与性能测试7.1 工业传送带控制应用案例背景传送带长度5米负载最大10kg速度范围0.1-1.0m/s定位精度±5mm实现方案硬件配置电机24V/100W直流有刷电机驱动器TC78H653FTG加装散热片编码器1000线增量式编码器限位开关两端各一个控制算法// 传送带位置控制 void Conveyor_Position_Control(float target_position) { static float current_position 0.0f; // 更新当前位置假设编码器每毫米产生4个脉冲 int16_t encoder_count (int16_t)TIM2-CNT; current_position encoder_count / 4.0f; // 计算位置误差 float error target_position - current_position; // 分段控制策略 if(fabsf(error) 100.0f) { // 远距离快速移动 Position_Control(target_position); } else if(fabsf(error) 10.0f) { // 中等距离精确移动 static PID_Controller pid; static uint8_t initialized 0; if(!initialized) { PID_Init(pid, 5.0f, 0.05f, 0.2f, 800.0f); initialized 1; } float dt 0.01f; // 100Hz控制周期 float speed PID_Update(pid, target_position, current_position, dt); if(speed 0) { Motor_CW((uint16_t)fabsf(speed)); } else { Motor_CCW((uint16_t)fabsf(speed)); } } else { // 精确定位阶段 Motor_Stop(); } }测试结果定位精度±3mm最大速度1.2m/s重复定位精度±1mm功耗平均35W峰值120W7.2 机器人关节控制应用案例背景关节类型旋转关节运动范围0-270度负载扭矩最大2Nm要求平稳运动低振动实现方案硬件配置电机12V/50W直流有刷电机减速箱20:1行星齿轮箱电位器10kΩ单圈电位器作为位置反馈控制算法// 读取电位器位置0-270度 float Read_Potentiometer(void) { HAL_ADC_Start(hadc1); HAL_ADC_PollForConversion(hadc1, 10); uint16_t adc_value HAL_ADC_GetValue(hadc1); return (adc_value / 4095.0f) * 270.0f; } // 平滑轨迹生成 float Trajectory_Generator(float target_angle) { static float current_target 0.0f; static float velocity 0.0f; const float max_accel 30.0f; // 度/秒² const float max_velocity 90.0f; // 度/秒 float dt 0.01f; // 100Hz控制周期 // 计算到目标的距离 float distance target_angle - current_target; // 计算需要的减速度距离 float decel_distance (velocity * velocity) / (2 * max_accel); // 决定加速、匀速或减速 if(fabsf(distance) decel_distance) { // 减速阶段 if(velocity 0) { velocity - max_accel * dt; if(velocity 0) velocity 0; } else { velocity max_accel * dt; if(velocity 0) velocity 0; } } else if(fabsf(velocity) max_velocity) { // 加速阶段 if(distance 0) { velocity max_accel * dt; if(velocity max_velocity) velocity max_velocity; } else { velocity - max_accel * dt; if(velocity -max_velocity) velocity -max_velocity; } } // 更新目标位置 current_target velocity * dt; return current_target; } // 关节控制主函数 void Joint_Control(float target_angle) { // 生成平滑轨迹 float smoothed_target Trajectory_Generator(target_angle); // 读取当前位置 float current_angle Read_Potentiometer(); // PID控制 static PID_Controller pid; static uint8_t initialized 0; if(!initialized) { PID_Init(pid, 8.0f, 0.1f, 0.5f, 1000.0f); initialized 1; } float dt 0.01f; // 100Hz控制周期 float output PID_Update(pid, smoothed_target, current_angle, dt); // 执行控制 if(output 0) { Motor_CW((uint16_t)fabsf(output)); } else { Motor_CCW((uint16_t)fabsf(output)); } }测试结果定位精度±0.5度最大速度80度/秒振动水平0.1度峰峰值重复定位精度±0.2度7.3 性能测试数据汇总在不同负载条件下的测试数据测试项目空载条件50%负载100%负载过载(120%)最大转速 (RPM)4500430040003800启动时间 (ms)5080120150效率 (%)85807565温升 (°C)15253545