总接线图1.BTS7960 接电源和电机BTS7960连接B12V电源正极B-12V电源负极M电机动力线1M-电机动力线22.BTS7960 接 STM32F103ZET6BTS7960引脚STM32引脚功能RPWMPA6 / TIM3_CH1正转PWMLPWMPA7 / TIM3_CH2反转PWMR_ENPB0右桥使能L_ENPB1左桥使能VCCLM2596的5V输出驱动模块逻辑供电GNDSTM32 GND信号参考地3.37GB545D 编码器接 STM32F103ZET637GB545D 编码器STM32F103ZET6功能说明编码器 VCC红5V编码器供电功耗约 10mA可直接从 STM32 的 5V 引脚取电编码器 GND黑GND与 STM32、BTS7960 驱动板单点共地编码器 A 相PA0 / TIM2_CH1编码器 A 相正交信号输入编码器 B 相PA1 / TIM2_CH2编码器 B 相正交信号输入4.LM2596 降压模块接线12V电源 → LM2596 IN 12V电源 - → LM2596 IN- LM2596 OUT → STM32 5V LM2596 OUT- → STM32 GND先不要连接STM32给LM2596输入端接入12V用万用表测量OUT与OUT-调节电位器将输出调到约5.0V确认稳定后再接STM325.FSR402压力传感器接线STM32 3.3V │ FSR402 │ ├── PA2 / ADC1_IN2 │ 10kΩ │ STM32 GND完整代码Z当前位置设置为张开零点 C夹持 T剪切 R复位张开 S立即停止1. 人工将机构完全张开 2. 发送Z将当前位置设置为零点 3. 发送C执行夹持 4. 等待状态显示CLAMPED 5. 发送T继续运动完成剪切 6. 发送R反转回到张开零点/* USER CODE BEGIN Includes */ #include stdbool.h #include stdint.h #include stdlib.h #include stdio.h #include stdarg.h #include string.h extern TIM_HandleTypeDef htim2; // 编码器定时器 extern TIM_HandleTypeDef htim3; // PWM输出定时器 extern ADC_HandleTypeDef hadc1; // FSR采样ADC extern UART_HandleTypeDef huart1; // 调试串口 /* USER CODE BEGIN PV */ /* * BTS7960引脚 * */ #define MOTOR_REN_PORT GPIOB #define MOTOR_REN_PIN GPIO_PIN_0 #define MOTOR_LEN_PORT GPIOB #define MOTOR_LEN_PIN GPIO_PIN_1 /* * 电机与编码器方向极性 * 方向反了直接改 ±1不用改接线 * */ #define MOTOR_POLARITY 1 // 1正转夹持-1反转夹持 #define ENCODER_POLARITY 1 // 1夹持计数增加-1夹持计数减少 /* * 位置参数(必须根据实际机械行程逐步标定) * */ #define CLAMP_MAX_POSITION 300L // 最大夹持位置 #define CUT_EXTRA_COUNTS 120L // 夹持后剪切增量行程 #define SAFETY_MAX_POSITION 600L // 绝对安全行程上限 #define POSITION_DEAD_ZONE 5L // 位置误差死区 /* * 电机PWM参数范围01000 * */ #define MOTOR_MIN_PWM 100 // 克服静摩擦最低PWM #define CLAMP_MAX_PWM 280 // 夹持阶段最大PWM #define CUT_MAX_PWM 380 // 剪切阶段最大PWM #define RESET_MAX_PWM 300 // 复位阶段最大PWM #define CUT_HOLD_PWM 0 // 剪切到位保持PWM调试建议先设0 #define KP_NUM 2 // 比例控制系数 P 误差*KP_NUM/KP_DEN #define KP_DEN 1 /* * 时间保护参数 * */ #define CLAMP_TIMEOUT_MS 4000UL #define CUT_TIMEOUT_MS 3000UL #define RESET_TIMEOUT_MS 5000UL #define CUT_HOLD_TIME_MS 200UL /* * FSR402压力传感器参数 * */ #define FSR_CALIBRATION_SAMPLES 100U // 空载校准采样次数 #define FSR_TRIGGER_OFFSET 300U // 触发偏移量ADC值 #define FSR_CONFIRM_TIMES 5U // 连续触发确认次数 /* * 状态机 * */ typedef enum { STATE_STOPPED 0, STATE_IDLE, STATE_CLAMPING, STATE_CLAMPED, STATE_CUTTING, STATE_CUT_HOLD, STATE_CUT_DONE, STATE_RESETTING, STATE_ERROR } GripperState; static GripperState gripperState STATE_STOPPED; /* * 编码器变量 * */ static uint16_t encoderLastCounter 0; static int32_t encoderPosition 0; static int32_t actualClampPosition 0; // 实际夹持位置 static int32_t currentCutTarget 0;// 当前剪切目标位置 /* * FSR变量 * */ static uint16_t fsrBaseValue 0; static uint16_t fsrFilteredValue 0; static uint8_t fsrConfirmCounter 0; /* * 状态和时间变量 * */ static uint32_t stateStartTime 0; static uint32_t lastFSRTime 0; static uint32_t lastPrintTime 0; static bool zeroValid false; /* * 串口变量 * */ static uint8_t uartRxByte 0; static volatile uint8_t receivedCommand 0; static volatile bool commandReady false; /* USER CODE BEGIN 0 */ /* * 串口打印 * */ static void UART_Printf(const char *format, ...) { char buffer[180]; va_list args; va_start(args, format); vsnprintf(buffer, sizeof(buffer), format, args); va_end(args); HAL_UART_Transmit(huart1, (uint8_t *)buffer, strlen(buffer), 100); } /* * BTS7960使能控制 * */ static void Motor_Enable(bool enable) { GPIO_PinState pinState; pinState enable ? GPIO_PIN_SET : GPIO_PIN_RESET; HAL_GPIO_WritePin(MOTOR_REN_PORT, MOTOR_REN_PIN, pinState); HAL_GPIO_WritePin(MOTOR_LEN_PORT, MOTOR_LEN_PIN, pinState); } /* * BTS7960 PWM控制 * * logicalPWM正数夹持/剪切方向;负数张开/复位方向;0停止 * 范围-10001000 * */ static void Motor_SetPWM(int16_t logicalPWM) { int32_t physicalPWM; uint32_t period; uint32_t compareValue; if (logicalPWM 1000) { logicalPWM 1000; } else if (logicalPWM -1000) { logicalPWM -1000; } physicalPWM (int32_t)logicalPWM * MOTOR_POLARITY; period __HAL_TIM_GET_AUTORELOAD(htim3); if (physicalPWM 0) { compareValue ((uint32_t)physicalPWM * period) / 1000U; /* RPWM输出电机向夹持方向运动 */ __HAL_TIM_SET_COMPARE(htim3, TIM_CHANNEL_1, compareValue); __HAL_TIM_SET_COMPARE(htim3, TIM_CHANNEL_2, 0); } else if (physicalPWM 0) { compareValue ((uint32_t)(-physicalPWM) * period) / 1000U; /* LPWM输出电机向张开方向运动 */ __HAL_TIM_SET_COMPARE(htim3, TIM_CHANNEL_1, 0); __HAL_TIM_SET_COMPARE(htim3, TIM_CHANNEL_2, compareValue); } else { __HAL_TIM_SET_COMPARE(htim3, TIM_CHANNEL_1, 0); __HAL_TIM_SET_COMPARE(htim3, TIM_CHANNEL_2, 0); } } static void Motor_Stop(void) { Motor_SetPWM(0); } /* * 编码器位置更新 * * TIM2为16位循环计数器。 * int16_t差值可以处理65535到0之间的正常回绕。 * */ static void Encoder_Update(void) { uint16_t currentCounter; int16_t counterDifference; currentCounter (uint16_t)__HAL_TIM_GET_COUNTER(htim2); counterDifference (int16_t)(currentCounter - encoderLastCounter); encoderLastCounter currentCounter; encoderPosition (int32_t)counterDifference * ENCODER_POLARITY; } static int32_t Encoder_GetPosition(void) { return encoderPosition; } static void Encoder_SetZero(void) { Motor_Stop(); __HAL_TIM_SET_COUNTER(htim2, 0); encoderLastCounter 0; encoderPosition 0; } /* * ADC读取FSR402 * */ static uint16_t FSR_ReadRaw(void) { uint16_t adcValue 0; HAL_ADC_Start(hadc1); if (HAL_ADC_PollForConversion(hadc1, 10) HAL_OK) { adcValue (uint16_t)HAL_ADC_GetValue(hadc1); } HAL_ADC_Stop(hadc1); return adcValue; } /* 上电空载校准 */ static void FSR_Calibrate(void) { uint32_t sum 0; UART_Printf(\r\nFSR开始空载校准请勿按压传感器。\r\n); for (uint16_t i 0; i FSR_CALIBRATION_SAMPLES; i) { sum FSR_ReadRaw(); HAL_Delay(10); } fsrBaseValue (uint16_t)(sum / FSR_CALIBRATION_SAMPLES); fsrFilteredValue fsrBaseValue; UART_Printf(FSR空载值%u\r\n, fsrBaseValue); UART_Printf(FSR触发值%u\r\n, (unsigned int)(fsrBaseValue FSR_TRIGGER_OFFSET)); } /* FSR低通滤波 */ static void FSR_Update(void) { uint16_t rawValue; rawValue FSR_ReadRaw(); /* * 一阶低通滤波 * 新值 7/8旧值 1/8当前值 */ fsrFilteredValue (uint16_t)(((uint32_t)fsrFilteredValue * 7U rawValue) / 8U); } /* 连续多次达到阈值才确认夹持 */ static bool FSR_TriggerConfirmed(void) { uint32_t triggerThreshold; triggerThreshold (uint32_t)fsrBaseValue FSR_TRIGGER_OFFSET; if (fsrFilteredValue triggerThreshold) { if (fsrConfirmCounter FSR_CONFIRM_TIMES) { fsrConfirmCounter; } } else { fsrConfirmCounter 0; } if (fsrConfirmCounter FSR_CONFIRM_TIMES) { fsrConfirmCounter 0; return true; } return false; } /* * 位置比例控制 * * 返回true达到目标位置 * 返回false仍在运行 * */ static bool Position_MoveTo(int32_t targetPosition, int16_t maximumPWM) { int32_t currentPosition; int32_t positionError; int32_t absoluteError; int32_t pwmOutput; currentPosition Encoder_GetPosition(); positionError targetPosition - currentPosition; absoluteError positionError 0 ? positionError : -positionError; if (absoluteError POSITION_DEAD_ZONE) { Motor_Stop(); return true; } pwmOutput positionError * KP_NUM / KP_DEN; if (pwmOutput maximumPWM) { pwmOutput maximumPWM; } else if (pwmOutput -maximumPWM) { pwmOutput -maximumPWM; } /* * 误差较小时比例输出可能无法克服齿轮箱摩擦 * 因此增加最低有效PWM。 */ if (pwmOutput 0 pwmOutput MOTOR_MIN_PWM) { pwmOutput MOTOR_MIN_PWM; } else if (pwmOutput 0 pwmOutput -MOTOR_MIN_PWM) { pwmOutput -MOTOR_MIN_PWM; } Motor_SetPWM((int16_t)pwmOutput); return false; } /* * 状态切换 * */ static void Enter_State(GripperState newState) { gripperState newState; stateStartTime HAL_GetTick(); fsrConfirmCounter 0; } static void Enter_Error(const char *message) { Motor_Stop(); Motor_Enable(false); gripperState STATE_ERROR; UART_Printf(\r\n故障%s\r\n, message); UART_Printf(请检查机构再发送R或Z命令。\r\n); } static const char *Get_StateName(GripperState state) { switch (state) { case STATE_STOPPED: return STOPPED; case STATE_IDLE: return IDLE; case STATE_CLAMPING: return CLAMPING; case STATE_CLAMPED: return CLAMPED; case STATE_CUTTING: return CUTTING; case STATE_CUT_HOLD: return CUT_HOLD; case STATE_CUT_DONE: return CUT_DONE; case STATE_RESETTING: return RESETTING; case STATE_ERROR: return ERROR; default: return UNKNOWN; } } /* * 串口命令处理 * */ static void Handle_Command(uint8_t command) { /* 小写字母自动转换为大写 */ if (command a command z) { command (uint8_t)(command - a A); } switch (command) { /* --------------------------------------------- * Z人工张开后将当前位置设置为零点 * --------------------------------------------- */ case Z: Motor_Stop(); Motor_Enable(true); Encoder_SetZero(); zeroValid true; Enter_State(STATE_IDLE); UART_Printf(\r\n当前位置已设置为张开零点。\r\n); break; /* --------------------------------------------- * C夹持 * --------------------------------------------- */ case C: if (!zeroValid) { UART_Printf(\r\n请先人工完全张开机构再发送Z。\r\n); break; } if (gripperState ! STATE_IDLE) { UART_Printf(\r\n当前状态不允许执行夹持。\r\n); break; } Motor_Enable(true); Enter_State(STATE_CLAMPING); UART_Printf(\r\n开始夹持。\r\n); break; /* --------------------------------------------- * T剪切 * --------------------------------------------- */ case T: if (gripperState ! STATE_CLAMPED) { UART_Printf(\r\n必须先完成夹持才能剪切。\r\n); break; } currentCutTarget actualClampPosition CUT_EXTRA_COUNTS; if (currentCutTarget SAFETY_MAX_POSITION || currentCutTarget 0) { Enter_Error(剪切目标超出安全行程); break; } Motor_Enable(true); Enter_State(STATE_CUTTING); UART_Printf(\r\n开始剪切目标位置%ld\r\n, (long)currentCutTarget); break; /* --------------------------------------------- * R复位张开 * --------------------------------------------- */ case R: if (!zeroValid) { UART_Printf(\r\n零点无效请人工张开并发送Z。\r\n); break; } Motor_Stop(); Motor_Enable(true); Enter_State(STATE_RESETTING); UART_Printf(\r\n开始复位张开。\r\n); break; /* --------------------------------------------- * S停止 * --------------------------------------------- */ case S: Motor_Stop(); Motor_Enable(false); Enter_State(STATE_STOPPED); UART_Printf(\r\n电机已停止BTS7960已关闭。\r\n); break; default: break; } } /* * 夹持剪切状态机 * */ static void Gripper_Task(void) { uint32_t now; now HAL_GetTick(); /* 持续更新编码器累计位置 */ Encoder_Update(); /* 处理串口命令 */ if (commandReady) { uint8_t command; command receivedCommand; commandReady false; Handle_Command(command); } /* 每10ms采集一次FSR */ if (now - lastFSRTime 10UL) { lastFSRTime now; FSR_Update(); } switch (gripperState) { /* --------------------------------------------- * 停止状态 * --------------------------------------------- */ case STATE_STOPPED: Motor_Stop(); break; /* --------------------------------------------- * 待机 * --------------------------------------------- */ case STATE_IDLE: Motor_Stop(); break; /* --------------------------------------------- * 夹持 * --------------------------------------------- */ case STATE_CLAMPING: /* * 夹持停止条件 * 1.FSR连续检测到压力 * 2.到达最大夹持位置 * 3.夹持动作超时。 */ if (FSR_TriggerConfirmed()) { Motor_Stop(); actualClampPosition Encoder_GetPosition(); Enter_State(STATE_CLAMPED); UART_Printf( \r\nFSR达到阈值夹持完成位置%ld\r\n, (long)actualClampPosition); } else if (Position_MoveTo(CLAMP_MAX_POSITION, CLAMP_MAX_PWM)) { Motor_Stop(); actualClampPosition Encoder_GetPosition(); Enter_State(STATE_CLAMPED); UART_Printf( \r\n达到最大夹持位置位置%ld\r\n, (long)actualClampPosition); } else if (now - stateStartTime CLAMP_TIMEOUT_MS) { Enter_Error(夹持超时); } break; /* --------------------------------------------- * 已完成夹持 * --------------------------------------------- */ case STATE_CLAMPED: /* * 默认停止输出避免减速电机长时间堵转发热。 * 如果机构会明显回弹后续再增加压力闭环保压。 */ Motor_Stop(); break; /* --------------------------------------------- * 剪切 * --------------------------------------------- */ case STATE_CUTTING: if (Position_MoveTo(currentCutTarget, CUT_MAX_PWM)) { Motor_Stop(); Enter_State(STATE_CUT_HOLD); UART_Printf(\r\n到达剪切位置。\r\n); } else if (now - stateStartTime CUT_TIMEOUT_MS) { Enter_Error(剪切动作超时); } break; /* --------------------------------------------- * 剪切短时保持 * --------------------------------------------- */ case STATE_CUT_HOLD: if (CUT_HOLD_PWM 0) { Motor_SetPWM(CUT_HOLD_PWM); } else { Motor_Stop(); } if (now - stateStartTime CUT_HOLD_TIME_MS) { Motor_Stop(); Enter_State(STATE_CUT_DONE); UART_Printf( \r\n剪切完成发送R进行复位。\r\n); } break; /* --------------------------------------------- * 剪切完成 * --------------------------------------------- */ case STATE_CUT_DONE: Motor_Stop(); break; /* --------------------------------------------- * 复位回到零点 * --------------------------------------------- */ case STATE_RESETTING: if (Position_MoveTo(0, RESET_MAX_PWM)) { Motor_Stop(); Enter_State(STATE_IDLE); UART_Printf( \r\n复位完成机构已回到张开位置。\r\n); } else if (now - stateStartTime RESET_TIMEOUT_MS) { Enter_Error(复位动作超时); } break; /* --------------------------------------------- * 故障 * --------------------------------------------- */ case STATE_ERROR: Motor_Stop(); break; default: Enter_Error(未知状态); break; } /* 每500ms打印状态 */ if (now - lastPrintTime 500UL) { lastPrintTime now; UART_Printf( 状态%s 编码器%ld FSR%u\r\n, Get_StateName(gripperState), (long)Encoder_GetPosition(), fsrFilteredValue); } } /* * USART1单字节接收中断 * */ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { if (huart-Instance USART1) { receivedCommand uartRxByte; commandReady true; HAL_UART_Receive_IT(huart1, uartRxByte, 1); } } /* USER CODE BEGIN 2 */ /* ADC校准 */ HAL_ADCEx_Calibration_Start(hadc1); /* 启动TIM2编码器模式 */ HAL_TIM_Encoder_Start(htim2,TIM_CHANNEL_ALL); /* 编码器计数清零 */ __HAL_TIM_SET_COUNTER(htim2, 0); encoderLastCounter 0; encoderPosition 0; /* 启动TIM3双路PWM */ HAL_TIM_PWM_Start(htim3, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim3,TIM_CHANNEL_2); /* 上电时电机保持停止驱动器关闭 */ Motor_Stop(); Motor_Enable(false); /* FSR空载校准 */ FSR_Calibrate(); /* 启动USART1中断接收 */ HAL_UART_Receive_IT(huart1,uartRxByte,1); /* 开机提示 */ UART_Printf(\r\n); UART_Printf(\r\n); UART_Printf(葡萄夹剪一体末端执行器启动\r\n); UART_Printf(控制器STM32F103ZET6\r\n); UART_Printf(电机JGB37-545带磁性编码器\r\n); UART_Printf(------------------------------------\r\n); UART_Printf(请先人工将机构放到完全张开位置\r\n); UART_Printf(Z设置当前位置为零点\r\n); UART_Printf(C夹持\r\n); UART_Printf(T剪切\r\n); UART_Printf(R复位张开\r\n); UART_Printf(S立即停止\r\n); UART_Printf(\r\n); /* USER CODE BEGIN 3 */ while (1) { Gripper_Task(); HAL_Delay(1);// 控制周期约1ms }