机器人

ROS机器人Diego 1#制作（二十四）4WD —编码器数据的读取。

robot
2020年4月30日
690

4驱底盘由于四个轮子可以独立控制，所以具有优秀的通过性，这篇文章介绍Diego 1#的四驱底盘，所有源代码都已经上传到github。
这里会分几篇文章来介绍4驱动版diego 1#的开发，这篇我们主要说明4驱动底盘4个马达编码器数据的读取。
1.1硬件说明

底盘材质：铝合金材质
轮胎：12cm 橡胶轮胎
电机：370直流电机，带霍尔码盘测速，输出AB项编码信号

1.2 控制器

主控制器 arduino UNO，使用uno分别对4个电机进行方向，PWM控制，并采用终端方式采集4个电机输出的编码信号
电机控制器 两块L298p, 网上采购，控制引脚不同
上位机：树莓派，或者mini pc

2.编码器数据读取
所以代码都基于diego 1# github上的代码进行修改，这里只说明4驱版本部分的代码
首先我们在ROSAduinoBridge_diego.ino文件中定义一个预编译符号，这个预编译符号可以启动或者关闭4驱的代码，这样方便我们开关4驱的功能

```
#define L298P_4WD
```
在diego 1 4wd中实现了使用arduino uno读取4个马达的AB项编码输出，一般的网上说明中arduino uno只有两个外部中断，好像只能读取一个马达的AB项编码输出，但事实上arudino中所有IO引脚都可以作为中断使用，通过操作中断寄存器的方式。
首先我们在encoder_driver.h文件中定义编码器连接的Arduino uno引脚：

```
#ifdef ARDUINO_ENC_COUNTER
  //below can be changed, but should be PORTD pins; 
  //otherwise additional changes in the code are required
  #define LEFT_ENC_PIN_A PD2  //pin 2
  #define LEFT_ENC_PIN_B PD3  //pin 3
  
  //below can be changed, but should be PORTC pins
  #define RIGHT_ENC_PIN_A PC2  //pin A2
  #define RIGHT_ENC_PIN_B PC3   //pin A3

#ifdef L298P_4WD
  #define LEFT_H_ENC_PIN_A PD4  //pin 4
  #define LEFT_H_ENC_PIN_B PD5  //pin 5
  
  //below can be changed, but should be PORTC pins
  #define RIGHT_H_ENC_PIN_A PC0  //pin A0
  #define RIGHT_H_ENC_PIN_B PC1   //pin A1
#endif  
#endif
```
从此文件中可以看到在原来2驱的基础上增加了4WD模式下的编码器连接引脚定义，其中左后方的电机AB项连接数字IO的D4和D5，而右后方电机AB项连接模拟IO的A0和A1，这样4个马达的编码器数据读取我们就用到8个IO口，加上PWM控制，转动方向的控制，在Diego 1# 4WD版本中，底盘控制一共用了16个IO，最后只剩D0，D1作为串口和上位机通讯，和A4，A5作为I2C的接口与I2C接口模块通讯，可以说Arduino UNO做到了充分利用。
在encoder_driver.ino文件中增加对4WD新增引脚的中断处理。
arduino uno中一共有3个引脚中断函数分别是

ISR (PCINT0_vect)对应 D8 to D13
ISR (PCINT1_vect) 对应 A0 to A5
ISR (PCINT2_vect) 对应 D0 to D7

diego1# 4wd中只需要用到两个中断处理函数 ISR(PCINT2_vect)和ISR(PCINT1_vect)，代码如下：

```
  ISR (PCINT2_vect){
     static uint8_t enc_last=0;
#ifdef L298P_4WD        
     static uint8_t enc_last_h=0;
#endif          
     enc_last <<=2; //shift previous state two places
     enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits

#ifdef L298P_4WD
     enc_last_h<<=2;
     enc_last_h |=(PIND & (3 << 4))>>4;
#endif 
  
     left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
#ifdef L298P_4WD   
     left_h_enc_pos +=ENC_STATES[(enc_last_h & 0x0f)];
#endif    
  }
  
  /* Interrupt routine for RIGHT encoder, taking care of actual counting */
  ISR (PCINT1_vect){
     static uint8_t enc_last=0;
#ifdef L298P_4WD        
     //uint8_t pinct=PINC;
     static uint8_t enc_last_h=0;
#endif   	
     enc_last <<=2; //shift previous state two places
     enc_last |= (PINC & (3 << 2)) >> 2; //read the current state into lowest 2 bits

#ifdef L298P_4WD
     enc_last_h<<=2;
     enc_last_h |=(PINC & 3);
#endif  
     right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
#ifdef L298P_4WD   
     right_h_enc_pos +=ENC_STATES[(enc_last_h & 0x0f)];
#endif
  }
```
这段代码中主要是针对中断寄存器PINC和PIND的操作，每个IO pin都对应PINC或者PIND的一位，IO有中断产生时，对应的位就会被置位，我们只需要读取相应为即可，如(PIND & (3 << 4))>>4读取的就是PIND的第4，5位，也就是D4,D5的数据。