PSoC 6 智能底盘控制系统完整工程：TB6612+JY61P+编码器+多路 UART

本项目基于英飞凌 PSoC 62 with CAPSENSE™ evaluation kit（CY8CEVAL-062S2）开发板，使用 ModusToolbox + cyhal HAL 库实现智能底盘控制系统。参加嵌入式设计大赛英飞凌赛道。

1. 项目简介

本项目是 STM32F407 底盘控制工程的英飞凌 PSoC 62 平台移植版，使用 cyhal HAL 库实现智能底盘控制系统的全部功能。系统包含：4 路 TB6612 电机驱动、JY61P 六轴姿态传感器、正交编码器测速、三路 UART 指令解析（雷达/K230 视觉/机械臂）、电动推杆继电器升降、状态 LED 心跳指示等模块。

1.1 硬件清单

text

一、硬件清单
────────────────────────────────────────────────────────────────
  1. PSoC 62 主控板  CY8CEVAL-062S2 ×1
  2. TB6612FNG 双H桥电机驱动模块 ×2 (共4路电机)
  3. 直流减速电机 ×4 (带霍尔编码器)
  4. JY61P 六轴姿态传感器 ×1 (TTL UART, 9600)
  5. K230 AI视觉模块 ×1 (UART通信, 9600)
  6. 机械臂 Arduino Nano/Uno ×1 (舵机×4 + 超声波 + 夹爪)
  7. 电动推杆 ×1 (12V, 2路继电器驱动升降)
  8. LD06/LD19 激光雷达 (接Linux主机, USB)
  9. 7.4V~12V 锂电池 + 5V/3A BEC (给PSoC6 + K230 + Arduino供电)

1.2 PSoC6 引脚接线表

text

二、PSoC6 引脚接线表 (Arduino 排针)
────────────────────────────────────────────────────────────────
  排针     PSoC引脚     接什么               备注
  ─────────────────────────────────────────────────────────────
  D0       P5[0]        USB串口RX (KitProg)  Linux主机/雷达 指令
  D1       P5[1]        USB串口TX (KitProg)  接电脑/树莓派USB
  D2       P9[2]        TB6612A IN1          左前轮方向1
  D3       P9[3]        TB6612A IN2          左前轮方向2
  D4       P9[4]        TB6612B IN1          右前轮方向1
  D5       P9[5]        TB6612A PWMA         左前轮PWM
  D6       P9[6]        TB6612B PWMB         右前轮PWM
  D7       P9[7]        TB6612B IN2          右前轮方向2
  D8       P6[0]        TB6612D IN1          右后轮方向1
  D9       P6[1]        TB6612C PWMC         左后轮PWM
  D10      P6[2]        TB6612D PWMD         右后轮PWM
  D11      P6[3]        编码器A相 (左/右主编码器)
  D12      P6[4]        编码器B相
  D13      P6[5]        状态LED (板载)       心跳指示
  A0       P10[0]       TB6612C IN1          左后轮方向1
  A1       P10[1]       TB6612C IN2          左后轮方向2
  A2       P10[2]       TB6612D IN2          右后轮方向2
  A3       P10[3]       继电器-升 (推杆)
  A4       P10[4]       继电器-降 (推杆)
  A5       P10[5]       空闲 (I2C SCL)       可接OLED

  J6扩展排针 (需焊排针):
  P5[2]    UART2 RX     JY61P TX
  P5[3]    UART2 TX     JY61P RX (可只接RX单向)
  P5[4]    UART3 RX     K230 UART2 TX
  P5[5]    UART3 TX     K230 UART2 RX
  P5[6]    UART4 RX     机械臂Arduino TX (可选)
  P5[7]    UART4 TX     机械臂Arduino RX (可选)

1.3 外部系统对接说明

text

三、外部系统对接
────────────────────────────────────────────────────────────────
  TB6612接线 (每个TB6612驱动2个电机):
    TB6612#1 (前桥):
      PWMA -> D5,  AIN1->D2, AIN2->D3   (左前轮)
      PWMB -> D6,  BIN1->D4, BIN2->D7   (右前轮)
      STBY -> 3V3 (常使能)
      VM   -> 电池+  (6~12V)
      VCC  -> 5V (逻辑电平)
      AO1/AO2 -> 左前电机;  BO1/BO2 -> 右前电机
    TB6612#2 (后桥):
      PWMA -> D9,  AIN1->A0, AIN2->A1   (左后轮)
      PWMB -> D10, BIN1->D8, BIN2->A2   (右后轮)
      STBY/VM/VCC 同上
      AO1/AO2 -> 左后电机;  BO1/BO2 -> 右后电机

  编码器:
    只接一个编码器(主编码器,测速度/里程)到D11/D12。
    若4轮都需要测速, 需扩展引脚: 可把前桥编码器接 P10[5]/Pxx (需要改pins.h)
    编码器VCC接3.3V或5V(看编码器), GND共地。

  JY61P:
    VCC->5V  GND->GND
    TX -> P5[2] (J6)  9600 8N1  TTL电平
    接线后JY61P会自动按10Hz输出角度帧, 程序会自动解析。

  K230视觉模块:
    K230 UART2 (FPIOA引脚11/12) <-> PSoC6 UART3 (P5[5]/P5[4], J6)
    K230 UART3 (FPIOA引脚50/51) <-> 机械臂Arduino RX/TX (默认方案)
    波特率9600, 协议:
      K230 -> PSoC6:  "0\n"(前进) "2\n"(左转) "3\n"(右转) "5\n"(停)
                      "zhong\n"(到达目标中心, 触发夹取)
      PSoC6 -> K230:  "k230\n"(启动下一个目标)  "ala1\n"(识别奥利奥)
                      "release\n"(通知释放)
    K230 -> Arduino:  "ce\n"(夹取) "release\n"(释放) (K230直连臂)

  机械臂 Arduino (两种接法二选一):
    方案A (默认, K230直控):
      K230 UART3 (TX=引脚50) -> Arduino D0(RX)
      波特率9600, K230直接发"ce\n"/"release\n"控制臂
      Arduino代码: 用原超声波版或者随附的串口协同版
    方案B (PSoC6统筹):
      main.c中 use_arm_uart4 = true;
      PSoC6 P5[7] (UART4 TX) -> Arduino D0(RX)
      K230"zhong"指令到达后PSoC6自动发"ce\n"给Arduino
      优点: PSoC6可以同时协调推杆升降+机械臂动作

  电动推杆继电器:
    两路5V继电器模块:
      IN1 -> A3 (升)   低电平吸合
      IN2 -> A4 (降)
      COM 接电源+, NO接推杆+
    注意: 推杆是直流电机, 升降时间到必须断电(软件切换回mode=0)。

  雷达 (LD06/LD19):
    雷达通过USB接Linux主机(树莓派/Jetson/笔记本)。
    雷达不直接连PSoC6, 由Linux运行ROS2 + 雷达Python工具做路径规划,
    规划结果通过USB串口发送单字节指令给PSoC6 UART1。
    Linux发送的指令: 与UART1协议一致 ('0' '1' '2' '3' '7' '8')
    PSoC6 UART1 通过KitProg USB口虚拟成 /dev/ttyACM0, 9600bps, 8N1。

2. 主程序入口（main.c）

主程序完成 BSP 初始化、1ms 系统时钟（用 TCPWM 不占 SysTick）、状态 LED、4 路 TB6612 电机初始化、编码器初始化、4 路 UART 初始化（UART1 雷达/上位机、UART2 JY61P IMU、UART3 K230 视觉、UART4 机械臂 Arduino 可选），然后进入主循环：每 10ms 采样编码器速度、调用 UartCmd_Loop() 处理指令状态机、每 500ms 通过 UART1 输出调试信息（编码器值、IMU 姿态、运行状态）。

/*
 * main.c - PSoC 62 (CY8CEVAL-062S2) 底盘控制主程序
 * 移植自 STM32F407 底盘控制工程
 *
 * 功能:
 *   - 4轮 TB6612 驱动 (前进/后退/左转/右转/停止)
 *   - 电动推杆继电器 (升/降)
 *   - 编码器测速 (GPIO 中断软件 X4 计数)
 *   - JY61P 六轴姿态读取 (UART2 中断接收)
 *   - 上位机/雷达导航指令接收 (UART1 KitProg USB, 9600, 单字节协议)
 *   - K230视觉模块双向通信 (UART3 J6扩展排针, 9600, 行协议)
 *   - 机械臂Arduino旁路控制 (UART4 J6扩展排针, 9600, 可选)
 *   - 调试输出: 编码器速度 / JY61P姿态 / 运行状态
 *
 * 原 STM32 -> PSoC6 对应:
 *   uwTick                   -> TCPWM 1ms 中断
 *   HAL_UART_RxCpltCallback  -> cyhal_uart 异步回调
 *   HAL_GPIO_WritePin        -> cyhal_gpio_write
 *   __HAL_TIM_SET_COMPARE    -> cyhal_pwm_set_duty_cycle
 *   HAL_Delay                -> cyhal_system_delay_ms
 */

#include "cyhal.h"
#include "cybsp.h"
#include <stdio.h>
#include <string.h>

#include "pins.h"
#include "tb6612.h"
#include "encoder.h"
#include "jy61p.h"
#include "uart_cmd.h"

/* ── 毫秒系统时钟 (替代 uwTick), 用 TCPWM 不占 SysTick ── */
static cyhal_timer_t ms_timer;
static volatile uint32_t g_sys_ms = 0;

static void ms_timer_isr(void *arg, cyhal_timer_event_t ev)
{
    (void)arg; (void)ev;
    g_sys_ms++;
}

static void ms_timer_init(void)
{
    cy_rslt_t r;
    const cyhal_timer_cfg_t cfg = {
        .compare_value = 0,
        .period        = 0xFFFF,
        .direction     = CYHAL_TIMER_DIR_UP,
        .is_compare    = false,
        .is_continuous = true,
        .value         = 0
    };
    r = cyhal_timer_init(&ms_timer, NC, NULL);
    CY_ASSERT(r == CY_RSLT_SUCCESS);
    r = cyhal_timer_configure(&ms_timer, &cfg);
    CY_ASSERT(r == CY_RSLT_SUCCESS);
    r = cyhal_timer_set_frequency(&ms_timer, 1000);     /* 1kHz = 1ms */
    CY_ASSERT(r == CY_RSLT_SUCCESS);
    cyhal_timer_register_callback(&ms_timer, ms_timer_isr, NULL);
    cyhal_timer_enable_event(&ms_timer, CYHAL_TIMER_IRQ_TERMINAL_COUNT,
                             CYHAL_ISR_PRIORITY_DEFAULT, true);
    cyhal_timer_start(&ms_timer);
}

uint32_t sys_get_ms(void)
{
    Cy_SysLib_EnterCriticalSection();
    uint32_t t = g_sys_ms;
    Cy_SysLib_ExitCriticalSection();
    return t;
}

/* ── UART 对象 ── */
static cyhal_uart_t uart1;     /* 雷达/上位机 (KitProg) */
static cyhal_uart_t uart2;     /* JY61P IMU */
static cyhal_uart_t uart3;     /* K230 视觉 */
static cyhal_uart_t uart4;     /* 机械臂 Arduino (可选) */

static char dbg_buf[160];

int main(void)
{
    cy_rslt_t r;

    /* 1. BSP 初始化 */
    r = cybsp_init();
    CY_ASSERT(r == CY_RSLT_SUCCESS);
    __enable_irq();

    /* 2. 1ms 系统时钟 */
    ms_timer_init();

    /* 3. 状态LED */
    cyhal_gpio_init(STATUS_LED_PIN, CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, 0);

    /* 4. 电机 + 推杆继电器 */
    TB6612_Init();

    /* 5. 编码器 */
    Encoder_Init();

    /* 6. UART1: 雷达/上位机 (KitProg USB-UART, 9600 8N1) */
    r = cyhal_uart_init(&uart1, UART1_TX_PIN, UART1_RX_PIN, NC, NC,
                        NULL, UART1_BAUDRATE);
    CY_ASSERT(r == CY_RSLT_SUCCESS);
    cyhal_uart_set_format(&uart1, 8, CYHAL_UART_PARITY_NONE, 1);

    /* 7. UART2: JY61P IMU (9600 8N1) */
    bool uart2_ok = (cyhal_uart_init(&uart2, UART2_TX_PIN, UART2_RX_PIN,
                                     NC, NC, NULL, UART2_BAUDRATE)
                     == CY_RSLT_SUCCESS);
    if (uart2_ok) {
        cyhal_uart_set_format(&uart2, 8, CYHAL_UART_PARITY_NONE, 1);
        JY61P_Init(&uart2);
    }

    /* 8. UART3: K230 视觉 (9600 8N1, 行协议) */
    bool uart3_ok = (cyhal_uart_init(&uart3, UART3_TX_PIN, UART3_RX_PIN,
                                     NC, NC, NULL, UART3_BAUDRATE)
                     == CY_RSLT_SUCCESS);
    if (uart3_ok) {
        cyhal_uart_set_format(&uart3, 8, CYHAL_UART_PARITY_NONE, 1);
    }

    /* 9. UART4: 机械臂 Arduino (9600 8N1, 可选)
     *    如果机械臂由K230直连, 设为 NULL (uart4_ok=false)
     *    如果希望PSoC6统一控制, 把UART4_TX接Arduino RX, uart4_ok=true */
    bool use_arm_uart4 = false;   /* <--- 改为 true 启用UART4机械臂控制 */
    bool uart4_ok = false;
    if (use_arm_uart4) {
        uart4_ok = (cyhal_uart_init(&uart4, UART4_TX_PIN, UART4_RX_PIN,
                                    NC, NC, NULL, UART4_BAUDRATE)
                    == CY_RSLT_SUCCESS);
        if (uart4_ok) {
            cyhal_uart_set_format(&uart4, 8, CYHAL_UART_PARITY_NONE, 1);
        }
    }

    /* 10. 指令系统初始化 */
    UartCmd_Init(&uart1,
                 uart3_ok ? &uart3 : NULL,
                 uart4_ok ? &uart4 : NULL);

    /* 11. 启动指示: LED闪2下 + 串口欢迎信息 */
    for (int i = 0; i < 4; i++) {
        cyhal_gpio_toggle(STATUS_LED_PIN);
        cyhal_system_delay_ms(150);
    }
    cyhal_gpio_write(STATUS_LED_PIN, 0);

    const char *welcome =
        "\r\n==========================================\r\n"
        "  PSoC62 Chassis Control Ready\r\n"
        "  Board: CY8CEVAL-062S2 (Infineon)\r\n"
        "  UART1:9600 (radar/host)  UART3:9600 (K230)\r\n"
        "  Cmds: 0=Fw 1=Bk 2=L 3=R 5=Stp 7=Up 8=Dn\r\n"
        "==========================================\r\n";
    size_t wlen = strlen(welcome);
    cyhal_uart_write(&uart1, (void*)welcome, &wlen);

    /* ── 主循环 ── */
    uint32_t last_enc_tick = sys_get_ms();
    uint32_t last_dbg_tick = sys_get_ms();
    int32_t  enc_speed     = 0;

    for (;;) {
        uint32_t now = sys_get_ms();

        /* 编码器采样 (10ms) */
        if (now - last_enc_tick >= ENCODER_SAMPLE_MS) {
            last_enc_tick = now;
            enc_speed = Encoder_ReadSpeed();
        }

        /* 指令状态机: 超时停车 / 推杆继电器 */
        UartCmd_Loop();

        /* 调试输出 (每 500ms) */
        if (now - last_dbg_tick >= 500) {
            last_dbg_tick = now;
            cyhal_gpio_toggle(STATUS_LED_PIN);

            JY61P_Data_t imu = {0};
            if (uart2_ok) JY61P_GetData(&imu);

            int len = snprintf(dbg_buf, sizeof(dbg_buf),
                "[DBG] enc=%ld  X=%6.1f Y=%6.1f Z=%6.1f  lift=%u  run=%u  ms=%lu\r\n",
                (long)enc_speed,
                imu.angle_x, imu.angle_y, imu.angle_z,
                (unsigned)UartCmd_GetLiftMode(),
                (unsigned)UartCmd_IsRunning(),
                (unsigned long)sys_get_ms());
            size_t wl = (len > 0) ? (size_t)len : 0;
            if (wl >= sizeof(dbg_buf)) wl = sizeof(dbg_buf) - 1;
            cyhal_uart_write(&uart1, dbg_buf, &wl);
        }
    }
}

/* [] END OF FILE */

3. 引脚定义（pins.h）

该文件集中定义了所有 PSoC 62 (CY8CEVAL-062S2) 引脚分配。开发板使用 Arduino Uno 兼容排针，前桥/后桥电机方向引脚 + PWM、电动推杆继电器、编码器 A/B 相、4 路 UART（UART1 走 KitProg USB、UART2/3/4 走 J6 扩展排针）的引脚号都集中在此处统一管理。

/*
 * pins.h - PSoC 62 (CY8CEVAL-062S2) 引脚定义
 * 适配原 STM32F407 底盘控制板
 *
 * 开发板: CY8CEVAL-062S2 (PSoC™ 62 with CAPSENSE™ evaluation kit)
 * MCU:    CY8C624ABZI-S2D44 (Cortex-M4 @150MHz + Cortex-M0+ @100MHz)
 * 环境:   ModusToolbox 3.x + PDL 3.x / HAL (cyhal)
 *
 * ── Arduino Uno 兼容排针引脚映射 (CY8CEVAL-062S2 默认BSP) ──
 *   D0  = P5[0]  SCB5 UART RX  (KitProg 调试串口)
 *   D1  = P5[1]  SCB5 UART TX  (KitProg 调试串口)
 *   D2  = P9[2]
 *   D3  = P9[3]  TCPWM capable
 *   D4  = P9[4]
 *   D5  = P9[5]  TCPWM PWM
 *   D6  = P9[6]  TCPWM PWM
 *   D7  = P9[7]
 *   D8  = P6[0]
 *   D9  = P6[1]  TCPWM PWM
 *   D10 = P6[2]  TCPWM PWM / SPI CS
 *   D11 = P6[3]  TCPWM QuadDec A / SPI MOSI
 *   D12 = P6[4]  TCPWM QuadDec B / SPI MISO
 *   D13 = P6[5]  SPI SCK / LED4
 *   A0  = P10[0]
 *   A1  = P10[1]
 *   A2  = P10[2]
 *   A3  = P10[3]
 *   A4  = P10[4]  I2C SDA
 *   A5  = P10[5]  I2C SCL
 *
 * ── J6 扩展排针 (在开发板侧边, 焊排针后可用) ──
 *   P5[2], P5[3], P5[4], P5[5], P5[6], P5[7] 等
 *
 * ── 最终引脚分配 (电机+编码器+UART1走Arduino排针; UART2/3走J6排针) ──
 */

#ifndef __PINS_H
#define __PINS_H

#include "cyhal.h"
#include "cybsp.h"

/* ============ TB6612 前桥电机 (左前轮A / 右前轮B) ============ */
#define MOTOR_A_PWM_PIN        P9_5     /* D5  - 左前轮 PWM  */
#define MOTOR_B_PWM_PIN        P9_6     /* D6  - 右前轮 PWM  */
#define MOTOR_A_IN1_PIN        P9_2     /* D2  - 左前轮方向1 */
#define MOTOR_A_IN2_PIN        P9_3     /* D3  - 左前轮方向2 */
#define MOTOR_B_IN1_PIN        P9_4     /* D4  - 右前轮方向1 */
#define MOTOR_B_IN2_PIN        P9_7     /* D7  - 右前轮方向2 */

/* ============ TB6612 后桥电机 (左后轮C / 右后轮D) ============ */
#define MOTOR_C_PWM_PIN        P6_1     /* D9  - 左后轮 PWM  */
#define MOTOR_D_PWM_PIN        P6_2     /* D10 - 右后轮 PWM  */
#define MOTOR_C_IN1_PIN        P10_0    /* A0  - 左后轮方向1 */
#define MOTOR_C_IN2_PIN        P10_1    /* A1  - 左后轮方向2 */
#define MOTOR_D_IN1_PIN        P6_0     /* D8  - 右后轮方向1 */
#define MOTOR_D_IN2_PIN        P10_2    /* A2  - 右后轮方向2 */

/* ============ 电动推杆继电器 (升降机) ============ */
#define RELAY_UP_PIN           P10_3    /* A3 - 推杆升  (mode=1) */
#define RELAY_DOWN_PIN         P10_4    /* A4 - 推杆降  (mode=2) */

/* ============ 编码器 (TCPWM 硬件正交解码) ============ */
#define ENCODER_A_PIN          P6_3     /* D11 - 编码器 A 相 */
#define ENCODER_B_PIN          P6_4     /* D12 - 编码器 B 相 */

/* ============ 用户LED (状态指示) ============ */
#define STATUS_LED_PIN         P6_5     /* D13 - 板载LED, 兼状态灯 */

/* ============ 串口 ============ */
/* UART1: Linux主机 / 雷达导航指令 (KitProg USB 虚拟串口, 主要指令通道) */
#define UART1_RX_PIN           P5_0     /* D0 - RX (KitProg) */
#define UART1_TX_PIN           P5_1     /* D1 - TX (KitProg) */
#define UART1_BAUDRATE         9600UL

/* UART2: JY61P 六轴姿态传感器 (9600bps, J6扩展排针)
 * 如果不焊J6, 可改为 P10_5(A5)/P6_5(D13), 编码器改软件计数 */
#define UART2_RX_PIN           P5_2     /* J6 - RX */
#define UART2_TX_PIN           P5_3     /* J6 - TX */
#define UART2_BAUDRATE         9600UL

/* UART3: K230 视觉模块 (9600bps, J6扩展排针)
 * K230 UART2 连接底盘, 发送单字符运动指令+换行: "0\n", "2\n", "3\n", "5\n"
 * K230 UART3 直连机械臂Arduino, 发送 "ce\n"/"release\n" 等, 默认不经过PSoC6
 * 如果希望 PSoC6 统筹控制机械臂, 见 UART4 */
#define UART3_RX_PIN           P5_4     /* J6 - RX (K230 TXD -> PSoC6 RX) */
#define UART3_TX_PIN           P5_5     /* J6 - TX (PSoC6 TX -> K230 RXD) */
#define UART3_BAUDRATE         9600UL

/* UART4: 机械臂 Arduino 旁路控制 (9600bps, J6扩展排针, 可选)
 * 如果机械臂直接连 K230 UART3, 则 PSoC6 不使用此通道, uart4=NULL 即可。
 * 若希望PSoC6统一管理(zhong到达时自动发"ce"), 把Arduino RX接这里, 并在
 * Device Configurator 中使能对应 SCB。
 * 使用 P5[6]/P5[7] (J6排针) */
#define UART4_RX_PIN           P5_6     /* J6 - RX (可仅接TX做单向控制) */
#define UART4_TX_PIN           P5_7     /* J6 - TX (-> Arduino RX) */
#define UART4_BAUDRATE         9600UL

/* ============ 电机参数 ============ */
#define MOTOR_PWM_FREQ_HZ      20000U   /* 20kHz 无音波 */
#define MOTOR_PWM_RANGE        1000U    /* 占空比 0-1000 (0.1% 精度) */

/* 速度百分比 (原STM32 CCR值188/200/220/235, ARR=1000, 换算为百分比) */
#define SPEED_FORWARD          20.0f    /* 直行速度 % */
#define SPEED_FORWARD_FAST     18.8f
#define SPEED_TURN             22.0f    /* 转弯内侧 */
#define SPEED_TURN_OUTER       23.5f    /* 转弯外侧 */
#define CMD_DURATION_MS        745U     /* 指令持续时间ms, 超时自动停 */

/* ============ 编码器参数 ============ */
#define ENCODER_SAMPLE_MS      10U      /* 采样间隔ms (对应原代码 uwTick 判断) */
#define ENCODER_SCALE          0.191f   /* 速度换算系数, 与原代码一致 */

#endif /* __PINS_H */

3.1 完整接线表参考

参见上方 1.2 节的接线表，关键映射：

D5/D6 = 前桥左右轮 PWM（P9[5]/P9[6]）
D9/D10 = 后桥左右轮 PWM（P6[1]/P6[2]）
A3/A4 = 推杆继电器升/降（P10[3]/P10[4]）
D11/D12 = 编码器 A/B 相（P6[3]/P6[4]）
D0/D1 = UART1 KitProg（P5[0]/P5[1]）
J6 P5[2]/P5[3] = UART2 JY61P；P5[4]/P5[5] = UART3 K230；P5[6]/P5[7] = UART4 机械臂

4. TB6612 电机 PWM 驱动

TB6612FNG 是常见双 H 桥电机驱动芯片，本项目用 2 片 TB6612 驱动 4 个直流减速电机（前桥 A/B、后桥 C/D）。每个电机有 IN1/IN2 方向引脚 + 一路 PWM 速度引脚。

方向真值表：

IN1	IN2	模式
0	0	滑行（MOTOR_STOP）
1	0	正转（MOTOR_FORWARD）
0	1	反转（MOTOR_BACKWARD）
1	1	刹车（MOTOR_BRAKE）

PWM 频率 20kHz（无音波），占空比 0~~100% 控制速度。文件中提供兼容原 STM32 接口的 TB6612_SetMotorSpeed() 宏（speed_raw 0~~1000 对齐原 CCR 值），方便代码迁移。

4.1 tb6612.h

/*
 * tb6612.h - TB6612FNG 双H桥电机驱动 (PSoC6 HAL 版本)
 * 原 STM32 版本使用 HAL_GPIO_WritePin + __HAL_TIM_SET_COMPARE
 * PSoC6 版本使用 cyhal_gpio_write + cyhal_pwm_set_duty_cycle
 *
 * 支持 4 路电机 (前桥左A/右B, 后桥左C/右D)
 * 方向引脚: IN1/IN2 组合: 正转(1,0)/反转(0,1)/刹车(1,1)/滑行(0,0)
 */

#ifndef __TB6612_H
#define __TB6612_H

#include "cyhal.h"
#include <stdbool.h>
#include <stdint.h>

/* 电机方向枚举 */
typedef enum {
    MOTOR_STOP = 0,    /* 滑行 (IN1=0, IN2=0) */
    MOTOR_FORWARD,     /* 正转 (IN1=1, IN2=0) */
    MOTOR_BACKWARD,    /* 反转 (IN1=0, IN2=1) */
    MOTOR_BRAKE        /* 刹车 (IN1=1, IN2=1) */
} MotorDir_t;

/* 初始化所有电机 PWM 和 GPIO */
void TB6612_Init(void);

/* 单独设置某路电机: 方向 + 速度(0~100.0%) */
void TB6612_SetMotorA(MotorDir_t dir, float speed_pct);
void TB6612_SetMotorB(MotorDir_t dir, float speed_pct);
void TB6612_SetMotorC(MotorDir_t dir, float speed_pct);
void TB6612_SetMotorD(MotorDir_t dir, float speed_pct);

/* 兼容原 STM32 接口: 用宏/内联映射到原接口风格
 * 原接口: TB6612_SetMotorSpeed(TB6612_FORWARD, 188, TB6612_FORWARD, 200);
 * 这里提供前桥/后桥两组接口 */
#define TB6612_FORWARD   MOTOR_FORWARD
#define TB6612_BACKWARD  MOTOR_BACKWARD
#define TB6612_BRAKE     MOTOR_BRAKE
#define TB6612_STOP      MOTOR_STOP

/* 同时设置前桥 A(左) / B(右)
 * speed_raw: 0~1000 (与原 STM32 CCR 值对齐, 内部除以10转百分比) */
static inline void TB6612_SetMotorSpeed(MotorDir_t dirA, uint16_t spdA,
                                        MotorDir_t dirB, uint16_t spdB)
{
    TB6612_SetMotorA(dirA, (float)spdA / 10.0f);
    TB6612_SetMotorB(dirB, (float)spdB / 10.0f);
}

/* 同时设置后桥 C(左) / D(右) */
static inline void TB6612_SetMotorSpeed_H(MotorDir_t dirC, uint16_t spdC,
                                          MotorDir_t dirD, uint16_t spdD)
{
    TB6612_SetMotorC(dirC, (float)spdC / 10.0f);
    TB6612_SetMotorD(dirD, (float)spdD / 10.0f);
}

/* 全部停止 */
void TB6612_StopAll(void);

#endif /* __TB6612_H */

4.2 tb6612.c

/*
 * tb6612.c - TB6612FNG 电机驱动实现 (PSoC6 cyhal)
 */

#include "tb6612.h"
#include "pins.h"

/* ========== PWM 对象 (每个电机一路PWM) ========== */
static cyhal_pwm_t pwm_motorA;   /* 左前 */
static cyhal_pwm_t pwm_motorB;   /* 右前 */
static cyhal_pwm_t pwm_motorC;   /* 左后 */
static cyhal_pwm_t pwm_motorD;   /* 右后 */

/* 内部辅助: 设置一对方向引脚 */
static void set_dir(cyhal_gpio_t in1, cyhal_gpio_t in2, MotorDir_t dir)
{
    switch (dir) {
    case MOTOR_FORWARD:
        cyhal_gpio_write(in1, 1);
        cyhal_gpio_write(in2, 0);
        break;
    case MOTOR_BACKWARD:
        cyhal_gpio_write(in1, 0);
        cyhal_gpio_write(in2, 1);
        break;
    case MOTOR_BRAKE:
        cyhal_gpio_write(in1, 1);
        cyhal_gpio_write(in2, 1);
        break;
    case MOTOR_STOP:
    default:
        cyhal_gpio_write(in1, 0);
        cyhal_gpio_write(in2, 0);
        break;
    }
}

/* 内部辅助: 设置PWM占空比 (做边界裁剪) */
static void set_pwm(cyhal_pwm_t *pwm, float pct)
{
    if (pct < 0.0f)   pct = 0.0f;
    if (pct > 100.0f) pct = 100.0f;
    cyhal_pwm_set_duty_cycle(pwm, pct, MOTOR_PWM_FREQ_HZ);
}

void TB6612_Init(void)
{
    cy_rslt_t rslt;

    /* ── 方向引脚: 全部推挽输出, 默认低(滑行) ── */
    /* 前桥 */
    cyhal_gpio_init(MOTOR_A_IN1_PIN, CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, 0);
    cyhal_gpio_init(MOTOR_A_IN2_PIN, CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, 0);
    cyhal_gpio_init(MOTOR_B_IN1_PIN, CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, 0);
    cyhal_gpio_init(MOTOR_B_IN2_PIN, CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, 0);
    /* 后桥 */
    cyhal_gpio_init(MOTOR_C_IN1_PIN, CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, 0);
    cyhal_gpio_init(MOTOR_C_IN2_PIN, CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, 0);
    cyhal_gpio_init(MOTOR_D_IN1_PIN, CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, 0);
    cyhal_gpio_init(MOTOR_D_IN2_PIN, CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, 0);

    /* ── PWM 引脚初始化 (20kHz, 0%占空比) ── */
    rslt = cyhal_pwm_init(&pwm_motorA, MOTOR_A_PWM_PIN, NULL);
    CY_ASSERT(rslt == CY_RSLT_SUCCESS);
    rslt = cyhal_pwm_init(&pwm_motorB, MOTOR_B_PWM_PIN, NULL);
    CY_ASSERT(rslt == CY_RSLT_SUCCESS);
    rslt = cyhal_pwm_init(&pwm_motorC, MOTOR_C_PWM_PIN, NULL);
    CY_ASSERT(rslt == CY_RSLT_SUCCESS);
    rslt = cyhal_pwm_init(&pwm_motorD, MOTOR_D_PWM_PIN, NULL);
    CY_ASSERT(rslt == CY_RSLT_SUCCESS);

    cyhal_pwm_set_duty_cycle(&pwm_motorA, 0.0f, MOTOR_PWM_FREQ_HZ);
    cyhal_pwm_set_duty_cycle(&pwm_motorB, 0.0f, MOTOR_PWM_FREQ_HZ);
    cyhal_pwm_set_duty_cycle(&pwm_motorC, 0.0f, MOTOR_PWM_FREQ_HZ);
    cyhal_pwm_set_duty_cycle(&pwm_motorD, 0.0f, MOTOR_PWM_FREQ_HZ);

    cyhal_pwm_start(&pwm_motorA);
    cyhal_pwm_start(&pwm_motorB);
    cyhal_pwm_start(&pwm_motorC);
    cyhal_pwm_start(&pwm_motorD);

    /* ── 推杆继电器 ── */
    cyhal_gpio_init(RELAY_UP_PIN,   CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, 0);
    cyhal_gpio_init(RELAY_DOWN_PIN, CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, 0);
}

void TB6612_SetMotorA(MotorDir_t dir, float speed_pct)
{
    set_dir(MOTOR_A_IN1_PIN, MOTOR_A_IN2_PIN, dir);
    set_pwm(&pwm_motorA, speed_pct);
}

void TB6612_SetMotorB(MotorDir_t dir, float speed_pct)
{
    set_dir(MOTOR_B_IN1_PIN, MOTOR_B_IN2_PIN, dir);
    set_pwm(&pwm_motorB, speed_pct);
}

void TB6612_SetMotorC(MotorDir_t dir, float speed_pct)
{
    set_dir(MOTOR_C_IN1_PIN, MOTOR_C_IN2_PIN, dir);
    set_pwm(&pwm_motorC, speed_pct);
}

void TB6612_SetMotorD(MotorDir_t dir, float speed_pct)
{
    set_dir(MOTOR_D_IN1_PIN, MOTOR_D_IN2_PIN, dir);
    set_pwm(&pwm_motorD, speed_pct);
}

void TB6612_StopAll(void)
{
    set_dir(MOTOR_A_IN1_PIN, MOTOR_A_IN2_PIN, MOTOR_STOP);
    set_dir(MOTOR_B_IN1_PIN, MOTOR_B_IN2_PIN, MOTOR_STOP);
    set_dir(MOTOR_C_IN1_PIN, MOTOR_C_IN2_PIN, MOTOR_STOP);
    set_dir(MOTOR_D_IN1_PIN, MOTOR_D_IN2_PIN, MOTOR_STOP);
    set_pwm(&pwm_motorA, 0.0f);
    set_pwm(&pwm_motorB, 0.0f);
    set_pwm(&pwm_motorC, 0.0f);
    set_pwm(&pwm_motorD, 0.0f);
}

5. JY61P 姿态传感器 UART 解析

JY61P 是维特智能的六轴姿态传感器模块，通过 UART 输出 11 字节的角度数据帧。本项目使用 cyhal_uart 异步读（IRQ callback）逐字节接收，状态机组帧解析。

帧格式（11 字节）：

偏移	长度	含义
0	1	帧头 0x55
1	1	包标识 0x53（角度数据包）
2-3	2	Roll (X) int16
4-5	2	Pitch (Y) int16
6-7	2	Yaw (Z) int16
8-9	2	温度/版本（忽略）
10	1	校验和 sum[0..9] & 0xFF

解析后角度换算：angle = raw_int16 / 32768.0 * 180.0（度）。

5.1 jy61p.h

/*
 * jy61p.h - JY61P 六轴姿态传感器驱动 (PSoC6 cyhal UART 版本)
 * 原 STM32 版本使用 USART2 中断接收11字节帧, 解析角度 X/Y/Z。
 * PSoC6 版本使用 cyhal uart 异步读 (irq callback)。
 *
 * JY61P 协议 (角度输出帧):
 *   字节0: 0x55          帧头
 *   字节1: 0x53          角度数据包标识
 *   字节2-3: Roll(X)     int16,  /32768*180 度
 *   字节4-5: Pitch(Y)    int16
 *   字节6-7: Yaw(Z)      int16
 *   字节8-9: 温度/版本    (忽略)
 *   字节10:  校验和       sum[0..9] & 0xFF
 */

#ifndef __JY61P_H
#define __JY61P_H

#include "cyhal.h"

typedef struct {
    float angle_x;   /* Roll  */
    float angle_y;   /* Pitch */
    float angle_z;   /* Yaw   */
} JY61P_Data_t;

/* 初始化 JY61P UART (9600 8N1), 启动异步接收 */
void JY61P_Init(cyhal_uart_t *huart);

/* 获取最新解析的姿态数据 (线程安全拷贝) */
void JY61P_GetData(JY61P_Data_t *out);

/* UART 事件回调, 在 cyhal_uart_register_callback 中注册, 内部处理字节流 */
void JY61P_UartCallback(void *callback_arg, cyhal_uart_event_t event);

#endif /* __JY61P_H */

5.2 jy61p.c

/*
 * jy61p.c - JY61P 六轴姿态传感器解析实现 (PSoC6 cyhal)
 *
 * 与原 STM32 版本逻辑一致:
 *   逐字节接收 -> 帧头0x55同步 -> 收满11字节 -> 校验0x53 -> 解析角度。
 *   使用 cyhal_uart_read_async 做异步接收, 每次收1字节,
 *   在 RX IRQ 回调中按状态机组帧。
 */

#include "jy61p.h"
#include "pins.h"
#include <string.h>

#define JY61P_FRAME_HEADER   0x55
#define JY61P_ANGLE_PACKET   0x53
#define JY61P_FRAME_LEN      11

static cyhal_uart_t *g_uart = NULL;
static uint8_t rx_byte;                        /* 异步读缓冲 (单字节) */
static uint8_t rx_buf[JY61P_FRAME_LEN];        /* 帧接收缓冲区 */
static uint8_t rx_idx = 0;                     /* 当前写入位置 */
static volatile JY61P_Data_t g_data = {0};     /* 最新解析结果 */

/* 内部: 校验整帧和 (sum of bytes 0..9 == byte10) */
static uint8_t checksum(uint8_t *buf)
{
    uint8_t sum = 0;
    for (int i = 0; i < JY61P_FRAME_LEN - 1; i++) {
        sum += buf[i];
    }
    return sum;
}

/* 内部: 解析一帧角度数据 */
static void parse_angle(uint8_t *buf)
{
    if (buf[1] != JY61P_ANGLE_PACKET) return;
    if (checksum(buf) != buf[10])   return;

    int16_t rx = (int16_t)((uint16_t)buf[3] << 8 | buf[2]);
    int16_t ry = (int16_t)((uint16_t)buf[5] << 8 | buf[4]);
    int16_t rz = (int16_t)((uint16_t)buf[7] << 8 | buf[6]);

    /* 临界区保护数据 */
    __disable_irq();
    g_data.angle_x = (float)rx / 32768.0f * 180.0f;
    g_data.angle_y = (float)ry / 32768.0f * 180.0f;
    g_data.angle_z = (float)rz / 32768.0f * 180.0f;
    __enable_irq();
}

/* IRQ 回调: 每收到一个字节触发一次 (由 cyhal uart async 机制) */
void JY61P_UartCallback(void *callback_arg, cyhal_uart_event_t event)
{
    (void)callback_arg;

    if (event & CYHAL_UART_IRQ_RX_DONE) {
        uint8_t b = rx_byte;

        if (rx_idx == 0) {
            /* 等待帧头 */
            if (b == JY61P_FRAME_HEADER) {
                rx_buf[rx_idx++] = b;
            }
        } else {
            rx_buf[rx_idx++] = b;
            if (rx_idx >= JY61P_FRAME_LEN) {
                /* 收满一帧 */
                parse_angle(rx_buf);
                rx_idx = 0;
            }
        }

        /* 重新发起下一字节异步读 */
        cyhal_uart_read_async(g_uart, &rx_byte, 1);
    }
}

void JY61P_Init(cyhal_uart_t *huart)
{
    g_uart = huart;
    rx_idx = 0;
    memset(&g_data, 0, sizeof(g_data));

    /* 注册回调并使能 RX 中断 */
    cyhal_uart_register_callback(g_uart, JY61P_UartCallback, NULL);
    cyhal_uart_enable_event(g_uart,
                            CYHAL_UART_IRQ_RX_DONE,
                            CYHAL_ISR_PRIORITY_DEFAULT,
                            true);

    /* 开始接收第一字节 */
    cyhal_uart_read_async(g_uart, &rx_byte, 1);
}

void JY61P_GetData(JY61P_Data_t *out)
{
    __disable_irq();
    *out = g_data;
    __enable_irq();
}

6. 正交编码器 GPIO 软件计数

原 STM32 版本使用 TIM1 硬件正交解码，PSoC6 版本改为 A 相边沿中断 + B 相电平判断的软件计数方式，实现 X4 计数（每个脉冲 4 次沿变）。

判定规则：

A 上升沿 + B=低 → 正转（pulse_count++）
A 上升沿 + B=高 → 反转（pulse_count--）
A 下降沿 + B=高 → 正转
A 下降沿 + B=低 → 反转

每 10ms 采样一次，调用 Encoder_ReadSpeed() 返回该周期速度（脉冲数 × ENCODER_SCALE），同时清零计数器。Encoder_GetTotalCount() 用于里程统计。

6.1 encoder.h

/*
 * encoder.h - 正交编码器读取 (PSoC6 GPIO 中断软件计数版)
 * 原 STM32 版本使用 TIM1 硬件正交解码, 这里改为 A/B 相边沿中断
 * 软件计数方式: 在 A 相边沿中断里读取 B 相电平判断方向, 累加计数。
 * 采样周期 10ms, 每周期清零计数器换算为速度, 与原代码 Read_Speed 逻辑对齐。
 */

#ifndef __ENCODER_H
#define __ENCODER_H

#include <stdint.h>

/* 初始化编码器 GPIO 中断 */
void Encoder_Init(void);

/* 获取最近一个采样周期内的编码器计数 (带方向), 调用后内部清零
 * 返回值: 计数值 * ENCODER_SCALE 即为速度, 单位与原 STM32 版本一致 */
int32_t Encoder_ReadSpeed(void);

/* 获取累计总脉冲数 (用于里程统计) */
int32_t Encoder_GetTotalCount(void);

#endif /* __ENCODER_H */

6.2 encoder.c

/*
 * encoder.c - 正交编码器 GPIO 中断软件计数实现
 *
 * 原理: 编码器 A 相接外部中断 (双沿触发), 在 ISR 中读取 B 相电平:
 *       - A 上升沿 且 B=低  => 正转
 *       - A 上升沿 且 B=高  => 反转
 *       - A 下降沿 且 B=高  => 正转
 *       - A 下降沿 且 B=低  => 反转
 *       此为标准 X4 计数 (每脉冲4次沿变), 与硬件正交解码分辨率相同。
 */

#include "encoder.h"
#include "pins.h"
#include "cyhal.h"

static volatile int32_t pulse_count = 0;       /* 采样周期内脉冲计数 */
static volatile int32_t total_count = 0;       /* 累计脉冲 */
static volatile uint8_t last_a = 0;

/* A 相 GPIO 中断回调: 边沿触发, 判断方向计数 */
static void encoder_a_isr(void *arg, cyhal_gpio_event_t event)
{
    (void)arg;
    uint8_t a = cyhal_gpio_read(ENCODER_A_PIN);
    uint8_t b = cyhal_gpio_read(ENCODER_B_PIN);

    if (a != last_a) {
        /* A 相发生跳变, 根据 B 相判断方向 */
        if (a == 1) {
            /* 上升沿 */
            if (b == 0) { pulse_count++; total_count++; }
            else        { pulse_count--; total_count--; }
        } else {
            /* 下降沿 */
            if (b == 1) { pulse_count++; total_count++; }
            else        { pulse_count--; total_count--; }
        }
        last_a = a;
    }
}

void Encoder_Init(void)
{
    /* 初始化 A/B 相为输入 (带上拉, 编码器输出为推挽或开漏) */
    cyhal_gpio_init(ENCODER_A_PIN, CYHAL_GPIO_DIR_INPUT,
                    CYHAL_GPIO_DRIVE_PULLUP, 0);
    cyhal_gpio_init(ENCODER_B_PIN, CYHAL_GPIO_DIR_INPUT,
                    CYHAL_GPIO_DRIVE_PULLUP, 0);

    last_a = cyhal_gpio_read(ENCODER_A_PIN);

    /* A 相双沿中断 (上升 + 下降) */
    cyhal_gpio_register_callback(ENCODER_A_PIN, encoder_a_isr, NULL);
    cyhal_gpio_enable_event(ENCODER_A_PIN,
                            CYHAL_GPIO_IRQ_RISE | CYHAL_GPIO_IRQ_FALL,
                            CYHAL_ISR_PRIORITY_DEFAULT, true);
}

int32_t Encoder_ReadSpeed(void)
{
    __disable_irq();
    int32_t count = pulse_count;
    pulse_count = 0;
    __enable_irq();
    return (int32_t)((float)count * ENCODER_SCALE);
}

int32_t Encoder_GetTotalCount(void)
{
    __disable_irq();
    int32_t t = total_count;
    __enable_irq();
    return t;
}

7. 三路 UART 指令解析

系统通过 3 路 UART 接收不同来源的指令：

通道	来源	协议	用途
UART1	雷达/上位机（KitProg USB）	单字节 ASCII	'0'前进 '1'后退 '2'左转 '3'右转 '7'推杆升 '8'推杆降
UART3	K230 视觉模块	行协议（以 \n 结尾）	"0"/"1"/"2"/"3"/"5" 控制 + "zhong" 到达中心触发夹取
UART4（可选）	机械臂 Arduino	行协议	"ce"夹取 "release"释放

指令执行后会启动 CMD_DURATION_MS（745ms）超时定时器，超时自动停车，避免指令丢失导致小车失控。推杆继电器采用持续生效模式，直到再次切换。

7.1 uart_cmd.h

/*
 * uart_cmd.h - 串口指令接收与分发 (PSoC6 版本)
 *
 * 三路串口:
 *   UART1 (9600): 上位机/雷达主机 (KitProg USB虚拟串口)
 *                 单字节指令: '0'前进 '1'后退 '2'左转 '3'右转
 *                            '7'推杆升 '8'推杆降
 *
 *   UART3 (9600): K230 视觉模块
 *                 行协议:  "0\n"前进 "2\n"左转 "3\n"右转 "5\n"停
 *                 支持反向发送: "k230\n"启动识别, "release\n"通知释放,
 *                              颜色目标名(如 "ala1\n")触发颜色寻物
 *
 *   UART4 (可选, 9600): 机械臂 Arduino (预留)
 *                 PSoC6 发送 "ce\n"夹取, "release\n"释放, "ready\n"到位
 *                 注意: 原方案中K230 UART3直连机械臂, 此通道为旁路可选
 *
 * 指令执行:
 *   '0'/'1'/'2'/'3'/'5': 控制底盘电机, 持续 CMD_DURATION_MS 后自动停
 *   '7'/'8':              推杆升/降 (持续生效, 直到再次切换)
 */

#ifndef __UART_CMD_H
#define __UART_CMD_H

#include "cyhal.h"
#include <stdint.h>
#include <stdbool.h>

/* 初始化指令串口系统
 * u1 : UART1 (雷达/上位机), 必须非NULL
 * u3 : UART3 (K230视觉), 可NULL表示未连接
 * u4 : UART4 (机械臂Arduino), 可NULL表示直连K230不走PSoC */
void UartCmd_Init(cyhal_uart_t *u1, cyhal_uart_t *u3, cyhal_uart_t *u4);

/* 主循环调用: 处理指令超时自动停、继电器状态刷新 */
void UartCmd_Loop(void);

/* 获取当前推杆模式: 0=停, 1=升, 2=降 */
uint8_t UartCmd_GetLiftMode(void);

/* 手动急停 (底盘电机+推杆) */
void UartCmd_EmergencyStop(void);

/* 向 K230 发送指令 (触发颜色识别等)
 * cmd: 命令字符串, 自动补 '\n'
 * 例如: UartCmd_K230_Send("k230");  -> 发送 "k230\n" 启动下一个目标
 *       UartCmd_K230_Send("ala1");  -> 发送 "ala1\n" 识别奥利奥 */
void UartCmd_K230_Send(const char *cmd);

/* 向机械臂发送夹取/释放指令 (如果UART4接了Arduino) */
void UartCmd_Arm_Clamp(void);
void UartCmd_Arm_Release(void);

/* 状态查询 */
bool UartCmd_IsRunning(void);   /* 底盘正在执行运动指令 */

#endif /* __UART_CMD_H */

7.2 uart_cmd.c

/*
 * uart_cmd.c - 串口指令解析与执行 (PSoC6 cyhal)
 * 对应原 STM32 HAL_UART_RxCpltCallback + main 中的 cmd_flag/cmd_start_time 逻辑,
 * 同时扩展支持 K230 行协议 (以\n结尾) 和机械臂 UART4 旁路控制。
 */

#include "uart_cmd.h"
#include "tb6612.h"
#include "pins.h"
#include <string.h>
#include <stdio.h>

extern uint32_t sys_get_ms(void);

/* ── UART 对象 ── */
static cyhal_uart_t *g_uart1 = NULL;
static cyhal_uart_t *g_uart3 = NULL;
static cyhal_uart_t *g_uart4 = NULL;

/* ── UART1 单字节接收 (雷达/上位机) ── */
static uint8_t rx1_byte;

/* ── UART3 行缓冲接收 (K230) ── */
#define K230_LINE_BUF_SIZE  32
static char     k230_line[K230_LINE_BUF_SIZE];
static uint8_t  k230_idx = 0;
static uint8_t  rx3_byte;

/* ── 控制状态 (与原 STM32 变量一一对应) ── */
static volatile uint8_t cmd_flag  = 0;
static uint32_t          cmd_start = 0;
static volatile uint8_t lift_mode = 0;   /* 0=停, 1=升, 2=降 */

/* ── 指令执行 ── */
static void execute_cmd(char cmd)
{
    /* '5' 是 K230 发的"停止"指令 */
    if (cmd == '5') {
        TB6612_StopAll();
        cmd_flag = 0;
        return;
    }

    switch (cmd) {
    case '7':
        lift_mode = 1;
        return;
    case '8':
        lift_mode = 2;
        return;
    case '0':
        TB6612_SetMotorSpeed(MOTOR_FORWARD,  188, MOTOR_FORWARD,  200);
        TB6612_SetMotorSpeed_H(MOTOR_FORWARD, 188, MOTOR_FORWARD, 200);
        break;
    case '1':
        TB6612_SetMotorSpeed(MOTOR_BACKWARD, 188, MOTOR_BACKWARD, 200);
        TB6612_SetMotorSpeed_H(MOTOR_BACKWARD, 188, MOTOR_BACKWARD, 200);
        break;
    case '2':
        TB6612_SetMotorSpeed(MOTOR_BACKWARD, 220, MOTOR_FORWARD,  235);
        TB6612_SetMotorSpeed_H(MOTOR_FORWARD,  220, MOTOR_BACKWARD, 235);
        break;
    case '3':
        TB6612_SetMotorSpeed(MOTOR_FORWARD,  220, MOTOR_BACKWARD, 235);
        TB6612_SetMotorSpeed_H(MOTOR_BACKWARD, 220, MOTOR_FORWARD,  235);
        break;
    default:
        return;   /* 未知指令, 忽略 */
    }
    cmd_flag  = 1;
    cmd_start = sys_get_ms();
}

/* ── K230 行缓冲处理 ── */
static void k230_process_line(const char *line)
{
    if (line[0] == '\0') return;

    /* 纯数字指令: "0","1","2","3","5" */
    if (line[0] >= '0' && line[0] <= '9' && line[1] == '\0') {
        execute_cmd(line[0]);
        return;
    }
    /* "zhong" 表示到达中心 (K230颜色寻物完成), 可扩展自动推杆 */
    if (strcmp(line, "zhong") == 0) {
        TB6612_StopAll();
        cmd_flag = 0;
        /* 触发夹取: 通过UART4发"ce"给机械臂 */
        UartCmd_Arm_Clamp();
        return;
    }
    /* 其他文本指令可按需扩展 */
}

/* ── UART1 IRQ (雷达/上位机, 单字节立即执行) ── */
static void uart1_cb(void *arg, cyhal_uart_event_t ev)
{
    (void)arg;
    if (ev & CYHAL_UART_IRQ_RX_DONE) {
        if (!cmd_flag) {
            execute_cmd((char)rx1_byte);
        }
        cyhal_uart_read_async(g_uart1, &rx1_byte, 1);
    }
}

/* ── UART3 IRQ (K230, 行缓冲) ── */
static void uart3_cb(void *arg, cyhal_uart_event_t ev)
{
    (void)arg;
    if (ev & CYHAL_UART_IRQ_RX_DONE) {
        char c = (char)rx3_byte;
        if (c == '\n' || c == '\r') {
            if (k230_idx > 0) {
                k230_line[k230_idx] = '\0';
                if (!cmd_flag) {
                    k230_process_line(k230_line);
                }
                k230_idx = 0;
            }
        } else {
            if (k230_idx < K230_LINE_BUF_SIZE - 1) {
                k230_line[k230_idx++] = c;
            } else {
                /* 行过长, 丢弃 */
                k230_idx = 0;
            }
        }
        cyhal_uart_read_async(g_uart3, &rx3_byte, 1);
    }
}

/* ── 初始化 ── */
void UartCmd_Init(cyhal_uart_t *u1, cyhal_uart_t *u3, cyhal_uart_t *u4)
{
    g_uart1 = u1;
    g_uart3 = u3;
    g_uart4 = u4;
    cmd_flag  = 0;
    lift_mode = 0;
    k230_idx  = 0;

    /* UART1: 单字节中断 */
    cyhal_uart_register_callback(g_uart1, uart1_cb, NULL);
    cyhal_uart_enable_event(g_uart1, CYHAL_UART_IRQ_RX_DONE,
                            CYHAL_ISR_PRIORITY_DEFAULT, true);
    cyhal_uart_read_async(g_uart1, &rx1_byte, 1);

    /* UART3: K230 行缓冲中断 */
    if (g_uart3) {
        cyhal_uart_register_callback(g_uart3, uart3_cb, NULL);
        cyhal_uart_enable_event(g_uart3, CYHAL_UART_IRQ_RX_DONE,
                                CYHAL_ISR_PRIORITY_DEFAULT, true);
        cyhal_uart_read_async(g_uart3, &rx3_byte, 1);
    }
}

/* ── 主循环 ── */
void UartCmd_Loop(void)
{
    /* 推杆继电器刷新 */
    switch (lift_mode) {
    case 1:
        cyhal_gpio_write(RELAY_UP_PIN,   0);
        cyhal_gpio_write(RELAY_DOWN_PIN, 1);
        break;
    case 2:
        cyhal_gpio_write(RELAY_UP_PIN,   1);
        cyhal_gpio_write(RELAY_DOWN_PIN, 0);
        break;
    default:
        cyhal_gpio_write(RELAY_UP_PIN,   1);
        cyhal_gpio_write(RELAY_DOWN_PIN, 1);
        break;
    }

    /* 超时自动停车 */
    if (cmd_flag && (sys_get_ms() - cmd_start >= CMD_DURATION_MS)) {
        TB6612_StopAll();
        cmd_flag = 0;
    }
}

uint8_t UartCmd_GetLiftMode(void) { return lift_mode; }

bool UartCmd_IsRunning(void) { return cmd_flag != 0; }

void UartCmd_EmergencyStop(void)
{
    TB6612_StopAll();
    cmd_flag  = 0;
    lift_mode = 0;
}

/* ── 向 K230 发命令 (自动补 \n) ── */
void UartCmd_K230_Send(const char *cmd)
{
    if (!g_uart3 || !cmd) return;
    char buf[48];
    int n = snprintf(buf, sizeof(buf), "%s\n", cmd);
    if (n <= 0) return;
    size_t len = (size_t)n;
    cyhal_uart_write(g_uart3, buf, &len);
}

/* ── 机械臂控制 (UART4) ── */
void UartCmd_Arm_Clamp(void)
{
    if (g_uart4) {
        size_t len = 3;
        cyhal_uart_write(g_uart4, "ce\n", &len);
    }
    /* 如果没接UART4, K230 UART3 直连机械臂, 可以由 K230 自己发 "ce\n" */
}

void UartCmd_Arm_Release(void)
{
    if (g_uart4) {
        size_t len = 8;
        cyhal_uart_write(g_uart4, "release\n", &len);
    }
    if (g_uart3) {
        /* 同时通知K230让它也给机械臂发release */
        UartCmd_K230_Send("release");
    }
}

8. 机械臂 Arduino 控制版（独立子系统）

机械臂使用 Arduino Uno/Nano 独立控制 4 路 PWM 舵机（底座/大臂/小臂/夹爪）+ HC-SR04 超声波测距 + LCD1602 I2C 显示。它与 PSoC6 通过 UART4 串口协同（"ce"夹取 / "release"释放 / "home"复位 / "rest"低功耗），也可以由 K230 UART3 直接控制（旁路 PSoC6）。

夹取动作序列：超声波测距 → 大臂下放（角度根据距离 map 计算）→ 闭合夹爪 → 抬起复位。距离异常（<20mm 或 >300mm）返回错误码 ERR_DIST。

cpp

/*
 * mech_arm_psoc6.ino - 机械臂 Arduino 适配版 (与 PSoC6 协同)
 *
 * 接线 (Arduino Uno/Nano, 与原超声波版本一致):
 *   Servo base/shoulder/elbow/gripper: 引脚 9, 6, 5, 3 (PWM)
 *   HC-SR04 Trig = 7, Echo = 8
 *   LCD SDA/SCL = A4/A5
 *   UART 通信:
 *     - RX(Pin0 D0) 接 PSoC6 UART4_TX (P5_7, J6排针)  <-- 接收 "ce" / "release"
 *     - TX(Pin1 D1) 可悬空 (或接回PSoC6做状态回报)
 *
 * 指令协议(ASCII, 以\n结尾):
 *   "ce\n"       -> 执行夹取动作序列(超声波检测 -> 降下 -> 夹取 -> 抬起)
 *   "release\n"  -> 释放物体
 *   "home\n"     -> 回到初始姿态
 *   "rest\n"     -> 低功耗姿态
 *
 * 注意: Arduino IDE 烧录时务必断开 RX/TX 与PSoC6的接线, 烧完再接。
 */

#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

/* ============ 舵机引脚 ============ */
Servo servoBase;      /* 底座旋转 */
Servo servoShoulder;  /* 大臂 */
Servo servoElbow;     /* 小臂 */
Servo servoGripper;   /* 夹爪 */

#define BASE_PIN      9
#define SHOULDER_PIN  6
#define ELBOW_PIN     5
#define GRIPPER_PIN   3

/* ============ 超声波 ============ */
#define TRIG_PIN 7
#define ECHO_PIN 8

/* ============ 夹爪开合角度 ============ */
#define GRIP_OPEN   150
#define GRIP_CLOSE  60

/* ============ 默认姿态 ============ */
#define BASE_HOME     90
#define SHOULDER_HOME 120
#define ELBOW_HOME    90

/* ============ LCD ============ */
LiquidCrystal_I2C lcd(0x27, 16, 2);

/* ============ 指令缓冲 ============ */
String cmdBuf = "";
bool cmdReady = false;
String lastCmd = "";

void setup()
{
    Serial.begin(9600);   /* 与 PSoC6 UART4 同波特率 */
    cmdBuf.reserve(16);

    /* 舵机 */
    servoBase.attach(BASE_PIN);
    servoShoulder.attach(SHOULDER_PIN);
    servoElbow.attach(ELBOW_PIN);
    servoGripper.attach(GRIPPER_PIN);

    /* 超声波 */
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);

    /* LCD */
    lcd.init();
    lcd.backlight();
    lcd.setCursor(0, 0);
    lcd.print("MechArm Ready");
    lcd.setCursor(0, 1);
    lcd.print("Wait PSoC6 cmd");

    /* 初始姿态 + 打开夹爪 */
    moveHome();
    openGripper();
}

void loop()
{
    /* 读取串口指令 (非阻塞) */
    while (Serial.available() > 0) {
        char c = (char)Serial.read();
        if (c == '\n' || c == '\r') {
            if (cmdBuf.length() > 0) {
                lastCmd = cmdBuf;
                cmdReady = true;
                cmdBuf = "";
            }
        } else {
            cmdBuf += c;
            if (cmdBuf.length() > 16) cmdBuf = "";   /* 溢出丢弃 */
        }
    }

    if (cmdReady) {
        cmdReady = false;
        handleCmd(lastCmd);
    }

    /* 无指令时维持姿态即可, 不做超声波自动动作 (由PSoC6/K230决策) */
    delay(20);
}

/* ============ 指令处理 ============ */
void handleCmd(const String &cmd)
{
    lcd.setCursor(0, 1);
    lcd.print("                ");
    lcd.setCursor(0, 1);
    lcd.print("CMD: ");
    lcd.print(cmd);

    if (cmd == "ce")         doClamp();
    else if (cmd == "release") doRelease();
    else if (cmd == "home")    moveHome();
    else if (cmd == "rest")    moveRest();
}

/* ============ 夹取动作序列 ============ */
void doClamp()
{
    long dist = readDistance();
    lcd.setCursor(0, 0);
    lcd.print("Dist:"); lcd.print(dist); lcd.print("mm");

    if (dist > 300 || dist < 20) {
        /* 距离异常: 保持原位并发错误码 */
        Serial.println("ERR_DIST\n");
        return;
    }

    /* 1. 大臂下放: 根据距离调整 */
    int shoulderAngle = map(constrain(dist, 50, 250), 50, 250, 150, 90);
    int elbowAngle    = map(constrain(dist, 50, 250), 50, 250, 60,  120);

    slowMove(servoShoulder, shoulderAngle, 15);
    slowMove(servoElbow,    elbowAngle,    15);
    delay(300);

    /* 2. 夹取 */
    closeGripper();
    delay(500);

    /* 3. 抬起 */
    slowMove(servoShoulder, SHOULDER_HOME, 15);
    slowMove(servoElbow,    ELBOW_HOME,    15);

    lcd.setCursor(0, 0);
    lcd.print("Clamped!        ");
    Serial.println("OK\n");
}

void doRelease()
{
    openGripper();
    delay(300);
    moveHome();
    lcd.setCursor(0, 0);
    lcd.print("Released        ");
    Serial.println("OK\n");
}

void moveHome()
{
    slowMove(servoBase,     BASE_HOME,     10);
    slowMove(servoShoulder, SHOULDER_HOME, 15);
    slowMove(servoElbow,    ELBOW_HOME,    15);
    openGripper();
}

void moveRest()
{
    slowMove(servoBase,     90, 10);
    slowMove(servoShoulder, 160, 15);
    slowMove(servoElbow,    40,  15);
    closeGripper();
}

/* ============ 动作辅助 ============ */
void openGripper()
{
    slowMove(servoGripper, GRIP_OPEN, 10);
}
void closeGripper()
{
    slowMove(servoGripper, GRIP_CLOSE, 10);
}

/* 平滑缓动到指定角度 */
void slowMove(Servo &s, int target, int step_delay_ms)
{
    int cur = s.read();
    if (cur == target) return;
    int dir = (target > cur) ? 1 : -1;
    while (cur != target) {
        cur += dir;
        s.write(cur);
        delay(step_delay_ms);
    }
}

/* 读取超声波距离 (mm) */
long readDistance()
{
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    long dur = pulseIn(ECHO_PIN, HIGH, 30000);   /* 最长30ms */
    if (dur == 0) return 9999;
    return dur * 0.1715f;   /* mm */
}

9. ModusToolbox 工程构建配置（Makefile）

使用 ModusToolbox 3.x 构建，目标板 CY8CEVAL-062S2，主核 Cortex-M4。源文件包含 main.c / tb6612.c / encoder.c / jy61p.c / uart_cmd.c，依赖 mtb-hal-cat1（HAL 库，含 uart/pwm/gpio/timer）、mtb-pdl-cat1（PDL 驱动）、retarget-io（可选 printf 重定向）。

9.1 工程创建步骤

text

四、ModusToolbox 工程创建步骤
────────────────────────────────────────────────────────────────
  1. 安装 ModusToolbox 3.2+ (英飞凌官网下载, 需注册账号)
  2. 打开 Eclipse IDE for ModusToolbox
  3. New Application -> CY8CEVAL-062S2 -> Empty_App
  4. Library Manager 中确认已启用:
       * mtb-hal-cat1  (HAL库, 含uart/pwm/gpio/timer)
       * mtb-pdl-cat1  (底层PDL驱动)
       * retarget-io   (可选, 若要用printf)
  5. 删除自动生成的 main.c, 把以下文件拷入工程根目录:
       main.c
       pins.h
       tb6612.c  tb6612.h
       encoder.c encoder.h
       jy61p.c   jy61p.h
       uart_cmd.c uart_cmd.h
       Makefile  (覆盖原来的)
  6. (可选) 打开 Device Configurator (design.modus),
     确认所有 UART/PWM 引脚没有冲突, 不用额外配置,
     cyhal API 运行时会自动 mux 引脚。
  7. 右键工程 -> Build, 生成 .elf 文件
  8. 连接板子USB (KitProg口), Quick Panel -> Program

9.2 Makefile

makefile

#
# PSoC 62 (CY8CEVAL-062S2) 底盘控制工程 - ModusToolbox Makefile
# 适配原 STM32F407 底盘控制代码
#
# 使用方法:
#   1. 打开 ModusToolbox 3.x, Project Creator -> New Application
#   2. 选择 CY8CEVAL-062S2 BSP, 选 "Empty App" 模板
#   3. Library Manager 中勾选: retarget-io, uart, pwm, timer, gpio (默认已包含)
#   4. 把本目录所有 .c/.h 文件拷贝到项目根目录
#   5. 用本 Makefile 覆盖项目自动生成的 Makefile
#   6. Library Manager 确保勾选: retarget-io (如果需要printf到串口)
#   7. make -j build ; make program
#

##############################################################################
# Basic Configuration
##############################################################################

# Target board - CY8CEVAL-062S2
TARGET=CY8CEVAL-062S2

# Default build config
CONFIG=Release

# Core to build for (CM4 is main app core)
CORE=CM4

##############################################################################
# Source Files
##############################################################################

SOURCES=\
main.c \
tb6612.c \
encoder.c \
jy61p.c \
uart_cmd.c

# Add all startup / BSP / HAL sources via CY_IGNORE rules (handled by MTB)

##############################################################################
# Include Paths
##############################################################################

INCLUDES=\
-I. \
-I./include

##############################################################################
# Compiler / Linker Flags
##############################################################################

CFLAGS=-Wall -Wextra -O2 -ffunction-sections -fdata-sections
LDFLAGS=-Wl,--gc-sections

DEFINES=
# Enable asserts in debug
ifeq ($(CONFIG),Debug)
DEFINES+= -DDEBUG -DCY_ASSERT_LEVEL=2
CFLAGS+=-g -O0
endif

##############################################################################
# MTB core makefile (自动引入 ModusToolbox 构建系统)
##############################################################################

CY_MAKE_DIR=$(SEARCH_MTB_CORE_API)
include $(CY_MAKE_DIR)/recipe.mk

构建命令：

bash

make -j build     # 编译
make program     # 烧录(KitProg USB)

10. 调试与验证

10.1 上电调试

text

五、调试
────────────────────────────────────────────────────────────────
  烧录后打开串口终端 (TeraTerm/串口助手):
    端口: KitProg USB->COMx /dev/ttyACM0
    波特率: 9600 8N1
  上电后应看到:
    ==========================================
      PSoC62 Chassis Control Ready
      ...
    ==========================================
  然后每500ms输出: [DBG] enc=X  X=X.X Y=X.X Z=X.X lift=X run=X ms=X
  可手动输入单个字符(0/1/2/3/5/7/8)测试电机动作。

10.2 与 Linux 雷达导航对接

text

六、Linux雷达导航对接
────────────────────────────────────────────────────────────────
  在原有雷达工具 Python 代码 (雷达工具_1.py~10.py) 中,
  将串口发送目标从 STM32 的 ttyUSBx 改为 PSoC6 KitProg 对应的
  /dev/ttyACM0, 发送指令格式完全兼容(单字节 '0'-'8'),
  无需修改Python逻辑, 仅改串口号即可。

  雷达建图/导航的计算机侧ROS2命令与原文档一致:
    # 启动雷达驱动
    ros2 launch ydlidar_ros2_driver ydlidar_launch.py
    # 启动建图
    ros2 launch slam_toolbox online_async_launch.py
    # 启动导航
    ros2 launch nav2_bringup navigation_launch.py

  导航输出的/cmd_vel转PSoC6指令:
    写一个简单ros2节点订阅/cmd_vel, 根据速度正负映射成 '0'(前进)
    '1'(后退) '2'(左转) '3'(右转) '5'(停), 通过pyserial写到/dev/ttyACM0。

10.3 K230 视觉代码改动

text

七、K230视觉代码改动
────────────────────────────────────────────────────────────────
  K230视觉代码(原"K230视觉检测(串口集成版).txt")默认波特率9600,
  引脚映射(FPIOA)不需要改, 直接用原来的11/12脚(UART2)接PSoC6即可。
  K230发送指令"0\n"/"2\n"/"3\n"/"5\n"PSoC6已经支持。
  K230 UART3 接机械臂Arduino直接发"ce\n"/"release\n"不需要PSoC6参与。
  如果用方案B(PSoC6统控臂), 把K230 UART3不接, K230发"zhong\n"时
  PSoC6会自动通过UART4发"ce\n"给Arduino。

10.4 常见问题排查

现象	可能原因	解决方案
烧录失败	KitProg 驱动未装	安装 ModusToolbox 自带 KitProg 驱动
UART1 无输出	串口号选错	设备管理器查看 KitProg USB->COMx，波特率 9600
电机不转	PWM 引脚冲突	检查 design.modus 中 P9[5]/P9[6] 等 TCPWM 引脚未被他用
JY61P 无数据	J6 排针未焊	焊接 J6 排针，或改用 P10[5]/P6[5] 引脚
编码器计数反向	A/B 相接反	交换 ENCODER_A_PIN 和 ENCODER_B_PIN 定义，或反转电机线
推杆失控	继电器常闭	检查 RELAY_UP/DOWN_PIN 默认电平，低电平吸合型需修改默认值为 1
K230 指令无响应	UART3 引脚未连	检查 K230 UART2 TX 是否接到 PSoC6 P5[4] (J6 RX)

总结

本项目展示了如何在英飞凌 PSoC 62 平台上完整实现一个嵌入式智能底盘控制系统：从硬件选型、引脚分配、外设驱动（TB6612 PWM 电机、JY61P UART 姿态、正交编码器 GPIO 中断）、到多路 UART 协议解析（雷达导航、K230 视觉、机械臂 Arduino）、再到电动推杆继电器控制、ModusToolbox 工程构建、调试与 Linux ROS2 雷达对接。完整代码不依赖 STM32 HAL，使用纯 cyhal API，可作为 PSoC 6 嵌入式开发的参考模板。

项目源码已开源，作者：hjz0810。基于 STM32F407 底盘控制工程移植，参加嵌入式设计大赛英飞凌赛道。

PSoC 6 智能底盘控制系统：TB6612+JY61P+编码器+多路 UART 完整工程实战

1. 项目简介

1.1 硬件清单

1.2 PSoC6 引脚接线表

1.3 外部系统对接说明

2. 主程序入口（main.c）

3. 引脚定义（pins.h）

3.1 完整接线表参考

4. TB6612 电机 PWM 驱动

4.1 tb6612.h

4.2 tb6612.c

5. JY61P 姿态传感器 UART 解析

5.1 jy61p.h

5.2 jy61p.c

6. 正交编码器 GPIO 软件计数

6.1 encoder.h

6.2 encoder.c

7. 三路 UART 指令解析

7.1 uart_cmd.h

7.2 uart_cmd.c

8. 机械臂 Arduino 控制版（独立子系统）

9. ModusToolbox 工程构建配置（Makefile）

9.1 工程创建步骤

9.2 Makefile

10. 调试与验证

10.1 上电调试

10.2 与 Linux 雷达导航对接

10.3 K230 视觉代码改动

10.4 常见问题排查

总结

评论