PekingGripper/stm32f103_drv8870_servo_main.c

/*
 * 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 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);
}

static void motor_open(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 void motor_close(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 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();

#ifdef __HAL_AFIO_REMAP_TIM2_PARTIAL_2
    __HAL_RCC_AFIO_CLK_ENABLE();
    __HAL_AFIO_REMAP_TIM2_PARTIAL_2();
#endif

    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();
    }
}

/*
 * 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();
 *   }
 * }
 */