如何修改ROS-Arduino桥接编码器代码适配4轮麦克纳姆驱动？

我跟着Articulated Robotics的ROS系列教程，用ros_arduino_bridge实现ROS2与Arduino Mega 2560的串口电机控制。机器人采用4轮麦克纳姆驱动，其他部分代码修改都很顺利，但编码器部分涉及Arduino底层的引脚中断操作，我不太熟悉，现在需要扩展成4个编码器的支持，对位掩码、移位逻辑还有中断资源的问题搞不清，求帮忙。

原核心代码片段 #

1. Setup函数中的编码器初始化 #

void setup() {
...
    // 设置为输入模式
    DDRD &= ~(1<<LEFT_ENC_PIN_A);
    DDRD &= ~(1<<LEFT_ENC_PIN_B);
    DDRC &= ~(1<<RIGHT_ENC_PIN_A);
    DDRC &= ~(1<<RIGHT_ENC_PIN_B);
    
    // 启用内部上拉电阻
    PORTD |= (1<<LEFT_ENC_PIN_A);
    PORTD |= (1<<LEFT_ENC_PIN_B);
    PORTC |= (1<<RIGHT_ENC_PIN_A);
    PORTC |= (1<<RIGHT_ENC_PIN_B);
    
    // 配置引脚变化中断掩码，监听左编码器引脚
    PCMSK2 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
    // 配置引脚变化中断掩码，监听右编码器引脚
    PCMSK1 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
    
    // 启用PCINT1和PCINT2中断
    PCICR |= (1 << PCIE1) | (1 << PCIE2);
...
}

2. encoder_driver.h的引脚定义 #

...
  // 左编码器需用PORTD引脚，否则要改代码逻辑
  #define LEFT_ENC_PIN_A PD2  // 对应Arduino引脚2
  #define LEFT_ENC_PIN_B PD3  // 对应Arduino引脚3
  
  // 右编码器需用PORTC引脚，否则要改代码逻辑
  #define RIGHT_ENC_PIN_A PC4  // 对应Arduino引脚A4
  #define RIGHT_ENC_PIN_B PC5   // 对应Arduino引脚A5
...

3. encoder_driver.ino的中断与计数逻辑 #

volatile long left_enc_pos = 0L;
  volatile long right_enc_pos = 0L;
  // 编码器状态查找表，根据前后两次状态组合返回计数增量
  static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};
    
  /* 左编码器中断服务函数 */
  ISR (PCINT2_vect){
    static uint8_t enc_last=0;
        
    enc_last <<=2; // 将前一次状态左移2位，腾出低2位放当前状态
    // 读取PORTD的PD2、PD3引脚状态，右移2位后放到低2位
    enc_last |= (PIND & (3 << 2)) >> 2;
  
    // 取低4位作为索引，累加计数增量
    left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
  }
  
  /* 右编码器中断服务函数 */
  ISR (PCINT1_vect){
        static uint8_t enc_last=0;
            
    enc_last <<=2; // 将前一次状态左移2位
    // 读取PORTC的PC4、PC5引脚状态，右移4位后放到低2位
    enc_last |= (PINC & (3 << 4)) >> 4;
  
    right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
  }
  
  /* 编码器读数封装函数 */
  long readEncoder(int i) {
    if (i == LEFT) return left_enc_pos;
    else return right_enc_pos;
  }
...

扩展4编码器的实现步骤 #

1. 新增编码器引脚定义 #

Arduino Mega的引脚变化中断（PCINT）分为3组，每组对应一个端口：

• PCINT0（PCIE0）→ PORTA（Arduino引脚22-29）
• PCINT1（PCIE1）→ PORTC（Arduino引脚A0-A5）
• PCINT2（PCIE2）→ PORTD（Arduino引脚0-7）

在encoder_driver.h中新增两个编码器的引脚定义，选PORTA的引脚（对应PCINT0组）：

// 新增前右、后左编码器引脚（PORTA对应PCINT0中断组）
#define FRONT_RIGHT_ENC_PIN_A PA0  // Arduino引脚22
#define FRONT_RIGHT_ENC_PIN_B PA1  // Arduino引脚23
#define BACK_LEFT_ENC_PIN_A PA2    // Arduino引脚24
#define BACK_LEFT_ENC_PIN_B PA3    // Arduino引脚25

2. 初始化新增编码器引脚 #

在Setup函数的编码器初始化部分后添加：

// 设置新增编码器引脚为输入模式
DDRA &= ~(1<<FRONT_RIGHT_ENC_PIN_A);
DDRA &= ~(1<<FRONT_RIGHT_ENC_PIN_B);
DDRA &= ~(1<<BACK_LEFT_ENC_PIN_A);
DDRA &= ~(1<<BACK_LEFT_ENC_PIN_B);

// 启用新增引脚的内部上拉电阻
PORTA |= (1<<FRONT_RIGHT_ENC_PIN_A);
PORTA |= (1<<FRONT_RIGHT_ENC_PIN_B);
PORTA |= (1<<BACK_LEFT_ENC_PIN_A);
PORTA |= (1<<BACK_LEFT_ENC_PIN_B);

// 配置PCMSK0掩码，监听新增的4个编码器引脚
PCMSK0 |= (1 << FRONT_RIGHT_ENC_PIN_A)|(1 << FRONT_RIGHT_ENC_PIN_B)|
          (1 << BACK_LEFT_ENC_PIN_A)|(1 << BACK_LEFT_ENC_PIN_B);

// 启用PCINT0中断组
PCICR |= (1 << PCIE0);

3. 扩展中断服务与计数变量 #

在encoder_driver.ino中添加新增编码器的计数变量和中断服务函数：

// 新增编码器计数变量，volatile确保中断中读写的原子性
volatile long front_right_enc_pos = 0L;
volatile long back_left_enc_pos = 0L;

/* PCINT0中断服务函数，处理PORTA上的两个编码器 */
ISR (PCINT0_vect){
    static uint8_t fr_enc_last=0;  // 前右编码器的前一次状态
    static uint8_t bl_enc_last=0;  // 后左编码器的前一次状态
    
    // 处理前右编码器（PA0、PA1）
    fr_enc_last <<=2;
    // 直接读取PA0、PA1的状态（掩码0x03对应低2位）
    fr_enc_last |= (PINA & 0x03);
    front_right_enc_pos += ENC_STATES[(fr_enc_last & 0x0f)];
    
    // 处理后左编码器（PA2、PA3）
    bl_enc_last <<=2;
    // 读取PA2、PA3的状态，右移2位后放到低2位
    bl_enc_last |= (PINA & 0x0C) >> 2;
    back_left_enc_pos += ENC_STATES[(bl_enc_last & 0x0f)];
}

// 修改readEncoder函数，支持4个编码器
#define LEFT 0
#define RIGHT 1
#define FRONT_RIGHT 2
#define BACK_LEFT 3

long readEncoder(int i) {
    switch(i){
        case LEFT: return left_enc_pos;
        case RIGHT: return right_enc_pos;
        case FRONT_RIGHT: return front_right_enc_pos;
        case BACK_LEFT: return back_left_enc_pos;
        default: return 0;
    }
}

4. 关键逻辑解释 #

• 引脚模式与上拉电阻：DDRx &= ~(1<<PIN)将引脚设为输入；PORTx |= (1<<PIN)启用内部上拉，避免编码器信号悬空。
• 引脚变化中断：PCINT监听端口内任意引脚的电平变化，通过PCMSKx掩码选择要监听的引脚，PCICR启用对应中断组。
• 编码器计数逻辑：每次中断时，把前一次的2位状态左移，拼接当前的2位状态，形成4位的状态组合，用这个组合作为索引查ENC_STATES数组，得到计数增量（+1正转、-1反转、0防抖）。

5. Arduino Mega中断资源说明 #

Mega的3组PCINT每组支持最多8个引脚，完全能覆盖4个编码器的8个引脚，不存在资源不足的问题。如果用外部中断（INT0-INT5）才会有限制，但这里用的PCINT足够满足需求。

内容的提问来源于stack exchange，提问作者Bravish