init
This commit is contained in:
248
stm32f103_drv8870_servo_main.c
Normal file
248
stm32f103_drv8870_servo_main.c
Normal file
@ -0,0 +1,248 @@
|
||||
/*
|
||||
* STM32F103C8T6 firmware core for the new PCB:
|
||||
* - PC <-> STM32: USART1, PA9 TX, PA10 RX, 115200 8N1
|
||||
* - Linear actuator driver: DRV8870, IN1/IN2
|
||||
* - Servo outputs: TIM2 CH4/CH3 on PB11/PB10, 50 Hz PWM
|
||||
*
|
||||
* Put this logic into a CubeMX/HAL project. Keep the MX_* init functions
|
||||
* generated by CubeMX, then add the user code below.
|
||||
*/
|
||||
|
||||
#include "main.h"
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
extern UART_HandleTypeDef huart1;
|
||||
extern TIM_HandleTypeDef htim2;
|
||||
|
||||
/* PCB V1.0 measured netlist. */
|
||||
#define LM_IN1_GPIO_Port GPIOB
|
||||
#define LM_IN1_Pin GPIO_PIN_12
|
||||
#define LM_IN2_GPIO_Port GPIOB
|
||||
#define LM_IN2_Pin GPIO_PIN_13
|
||||
|
||||
#define SERVO1_TIM htim2
|
||||
#define SERVO1_CHANNEL TIM_CHANNEL_4 /* PB11 on STM32F103 TIM2_CH4 */
|
||||
#define SERVO2_TIM htim2
|
||||
#define SERVO2_CHANNEL TIM_CHANNEL_3 /* PB10 on STM32F103 TIM2_CH3 */
|
||||
|
||||
#define UART_RX_BUF_SIZE 96
|
||||
#define FAILSAFE_MS 1000U
|
||||
|
||||
static uint8_t rx_byte;
|
||||
static char rx_line[UART_RX_BUF_SIZE];
|
||||
static uint8_t rx_len = 0;
|
||||
static volatile bool line_ready = false;
|
||||
static uint32_t last_cmd_tick = 0;
|
||||
// ====== 新增：电机运行超时保护变量 ======
|
||||
static uint32_t motor_start_tick = 0;
|
||||
static uint8_t motor_state = 0; // 0=停止, 1=张开, 2=闭合
|
||||
|
||||
static void reply(const char *text)
|
||||
{
|
||||
HAL_UART_Transmit(&huart1, (uint8_t *)text, strlen(text), 100);
|
||||
}
|
||||
|
||||
static void motor_stop(void)
|
||||
{
|
||||
HAL_GPIO_WritePin(LM_IN1_GPIO_Port, LM_IN1_Pin, GPIO_PIN_RESET);
|
||||
HAL_GPIO_WritePin(LM_IN2_GPIO_Port, LM_IN2_Pin, GPIO_PIN_RESET);
|
||||
motor_state = 0; // 新增
|
||||
motor_start_tick = 0; // 新增
|
||||
}
|
||||
|
||||
static void motor_open(void)
|
||||
{
|
||||
HAL_GPIO_WritePin(LM_IN1_GPIO_Port, LM_IN1_Pin, GPIO_PIN_RESET);
|
||||
HAL_GPIO_WritePin(LM_IN2_GPIO_Port, LM_IN2_Pin, GPIO_PIN_SET);
|
||||
}
|
||||
static void motor_close(void)
|
||||
{
|
||||
HAL_GPIO_WritePin(LM_IN2_GPIO_Port, LM_IN2_Pin, GPIO_PIN_RESET);
|
||||
HAL_GPIO_WritePin(LM_IN1_GPIO_Port, LM_IN1_Pin, GPIO_PIN_SET);
|
||||
}
|
||||
|
||||
static uint16_t servo_angle_to_us(int angle)
|
||||
{
|
||||
if (angle < 0) {
|
||||
angle = 0;
|
||||
}
|
||||
if (angle > 180) {
|
||||
angle = 180;
|
||||
}
|
||||
|
||||
/* MG996R/common servo: 500 us to 2500 us at 50 Hz. */
|
||||
return (uint16_t)(500 + (angle * 2000) / 180);
|
||||
}
|
||||
|
||||
static void servo_set_angle(uint8_t id, int angle)
|
||||
{
|
||||
uint16_t pulse_us = servo_angle_to_us(angle);
|
||||
|
||||
if (id == 1) {
|
||||
__HAL_TIM_SET_COMPARE(&SERVO1_TIM, SERVO1_CHANNEL, pulse_us);
|
||||
} else if (id == 2) {
|
||||
__HAL_TIM_SET_COMPARE(&SERVO2_TIM, SERVO2_CHANNEL, pulse_us);
|
||||
}
|
||||
}
|
||||
|
||||
static void apply_config(uint8_t mode)
|
||||
{
|
||||
switch (mode) {
|
||||
case 0:
|
||||
servo_set_angle(1, 90);
|
||||
servo_set_angle(2, 90);
|
||||
break;
|
||||
case 1:
|
||||
servo_set_angle(1, 30);
|
||||
servo_set_angle(2, 150);
|
||||
break;
|
||||
case 2:
|
||||
servo_set_angle(1, 120);
|
||||
servo_set_angle(2, 60);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void handle_command(char *cmd)
|
||||
{
|
||||
last_cmd_tick = HAL_GetTick();
|
||||
|
||||
if (strcmp(cmd, "PING") == 0) {
|
||||
reply("OK:PONG\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if (strcmp(cmd, "M:OPEN") == 0) {
|
||||
motor_open();
|
||||
reply("OK:M:OPEN\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if (strcmp(cmd, "M:CLOSE") == 0) {
|
||||
motor_close();
|
||||
reply("OK:M:CLOSE\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if (strcmp(cmd, "M:STOP") == 0) {
|
||||
motor_stop();
|
||||
reply("OK:M:STOP\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if (strncmp(cmd, "S1:", 3) == 0) {
|
||||
servo_set_angle(1, atoi(cmd + 3));
|
||||
reply("OK:S1\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if (strncmp(cmd, "S2:", 3) == 0) {
|
||||
servo_set_angle(2, atoi(cmd + 3));
|
||||
reply("OK:S2\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if (strncmp(cmd, "CFG:", 4) == 0) {
|
||||
apply_config((uint8_t)atoi(cmd + 4));
|
||||
reply("OK:CFG\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
reply("ERR:UNKNOWN\r\n");
|
||||
}
|
||||
|
||||
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
|
||||
{
|
||||
if (huart->Instance != USART1) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (rx_byte == '\n' || rx_byte == '\r') {
|
||||
if (rx_len > 0) {
|
||||
rx_line[rx_len] = '\0';
|
||||
line_ready = true;
|
||||
rx_len = 0;
|
||||
}
|
||||
} else if (rx_len < UART_RX_BUF_SIZE - 1) {
|
||||
rx_line[rx_len++] = (char)rx_byte;
|
||||
} else {
|
||||
rx_len = 0;
|
||||
reply("ERR:LINE_TOO_LONG\r\n");
|
||||
}
|
||||
|
||||
HAL_UART_Receive_IT(&huart1, &rx_byte, 1);
|
||||
}
|
||||
|
||||
/*
|
||||
* Call this after MX_GPIO_Init(), MX_USART1_UART_Init(), MX_TIM2_Init().
|
||||
* CubeMX TIM2 recommendation:
|
||||
* channels: PB10 = TIM2_CH3, PB11 = TIM2_CH4
|
||||
* enable TIM2 remap for PB10/PB11 if CubeMX does not do it automatically
|
||||
* clock = 72 MHz, prescaler = 71, counter period = 19999
|
||||
* PWM pulse units are microseconds.
|
||||
*/
|
||||
void app_init(void)
|
||||
{
|
||||
motor_stop();
|
||||
|
||||
// ====== 强制使能 TIM2 部分重映射（PB10/PB11） ======
|
||||
// 不依赖 CubeMX 宏定义，直接写寄存器
|
||||
__HAL_RCC_AFIO_CLK_ENABLE(); // 开启 AFIO 时钟
|
||||
AFIO->MAPR |= AFIO_MAPR_TIM2_REMAP_PARTIALREMAP2; // 部分重映射 2
|
||||
|
||||
HAL_TIM_PWM_Start(&SERVO1_TIM, SERVO1_CHANNEL);
|
||||
HAL_TIM_PWM_Start(&SERVO2_TIM, SERVO2_CHANNEL);
|
||||
servo_set_angle(1, 90);
|
||||
servo_set_angle(2, 90);
|
||||
last_cmd_tick = HAL_GetTick();
|
||||
HAL_UART_Receive_IT(&huart1, &rx_byte, 1);
|
||||
reply("READY:DRV8870_SERVO\r\n");
|
||||
}
|
||||
|
||||
void app_loop(void)
|
||||
{
|
||||
if (line_ready) {
|
||||
char cmd[UART_RX_BUF_SIZE];
|
||||
__disable_irq();
|
||||
strncpy(cmd, rx_line, sizeof(cmd));
|
||||
cmd[sizeof(cmd) - 1] = '\0';
|
||||
line_ready = false;
|
||||
__enable_irq();
|
||||
handle_command(cmd);
|
||||
}
|
||||
|
||||
// 空闲超时停止（原有）
|
||||
if ((HAL_GetTick() - last_cmd_tick) > FAILSAFE_MS) {
|
||||
motor_stop();
|
||||
}
|
||||
|
||||
// ====== 新增：电机运行超时保护（3秒） ======
|
||||
if (motor_state != 0) {
|
||||
if ((HAL_GetTick() - motor_start_tick) > 3000U) {
|
||||
motor_stop();
|
||||
reply("ERR:MOTOR_TIMEOUT\r\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* In generated main.c, use:
|
||||
*
|
||||
* int main(void)
|
||||
* {
|
||||
* HAL_Init();
|
||||
* SystemClock_Config();
|
||||
* MX_GPIO_Init();
|
||||
* MX_USART1_UART_Init();
|
||||
* MX_TIM2_Init();
|
||||
* app_init();
|
||||
* while (1) {
|
||||
* app_loop();
|
||||
* }
|
||||
* }
|
||||
*/
|
||||
Reference in New Issue
Block a user