我们接下来继续看！紧跟着是encoder_driver编码器相关的文件，我们先来看下encoder_dirver.h编码器驱动头文件的内容，如下~

#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 PC4  //pin A4
  #define RIGHT_ENC_PIN_B PC5   //pin A5
#endif
   
long readEncoder(int i);	//读取编码器数值
void resetEncoder(int i);	//重置编码器数值
void resetEncoders();	//重置所有编码器数值

还是头文件的定义，基于Atmega328P芯片的Arduino，只有D2、D3两个引脚支持外部中断，A4、A5两个引脚通常是用作模拟量读取、数字量读取/写入、IIC通讯，这里将这两个两个当作编码器的中断使用。然后就是三个函数读取编码器、重置指定编码器、重置所有编码器。

这部分的内容实现在encoder_dirver.cpp文件内，我们继续分析~

#ifdef USE_BASE
	#ifdef ROBOGAIA
		#include "MegaEncoderCounter.h"
		
		MegaEncoderCounter encoders = MegaEncoderCounter(4);
      	long readEncoder(int i) 
      	{
          	if (i == LEFT)
            	  return encoders.YAxisGetCount();
          	else 
            	  return encoders.XAxisGetCount();
      	}

      	void resetEncoder(int i) 
        {
          	if (i == LEFT) 
            	  return encoders.YAxisReset();
          	else 
            	  return encoders.XAxisReset();
      	}
	
	//上面的不管，下面的是是Arduino编码器的实现
	#elif defined(ARDUINO_ENC_COUNTER)
    	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;
          	enc_last |= (PIND & (3 << 2)) >> 2;

          	left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
      	}

      	//中断程序为右编码器，实际计数
      	ISR (PCINT1_vect)
      	{
          	static uint8_t enc_last=0;

          	enc_last <<=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;
      	}

      	//重置指定编码器计数
      	void resetEncoder(int i) 
      	{
          	if (i == LEFT)
          	{
              	left_enc_pos=0L;
              	return;
          	}else 
          	{ 
              	right_enc_pos=0L;
              	return;
          	}
      	}
  	#else
    	#error A encoder driver must be selected!
  	#endif

	//重置所有编码器计数
	void resetEncoders() 
    {
  		resetEncoder(LEFT);
  		resetEncoder(RIGHT);
	}
#endif

Arduino编程使用ISR，这个确实是很冷门，对于不是很懂寄存器的同学来说确实很难受。但是这个逻辑，读取编码器计数，如果我们使用Arduino Mega的话，是完全可以把这部分程序迁移过去的，当然你使用UNO或者Nano的情况下，也可以直接使用这个函数来实现。最后的重置所有编码器计数，完全就是套娃！

到这里基本上就已经讲完了的，ros_arduino_bridge所实现的也就是控制电机转速+读取编码器数值，这是最为核心的功能。我们继续来看舵机驱动是怎么实现的！首先是servos.h舵机驱动头文件的内容~

#ifndef SERVOS_H
#define SERVOS_H

#define N_SERVOS 2	//舵机数量声明

//这个以毫秒为单位的延迟决定了伺服每一步行程之间的停顿 
//增加这个数字将使伺服扫描更慢。
//减少这个数字将使伺服扫描更快。
//0是默认的数字，将使伺服旋转全速;150毫秒使它们旋转得非常慢。
int stepDelay [N_SERVOS] = { 0, 0 }; //延迟时间

//舵机引脚声明
byte servoPins [N_SERVOS] = { 3, 4 };

//舵机位置初始化
byte servoInitPosition [N_SERVOS] = { 90, 90 }; // [0, 180] degrees

class SweepServo
{
  public:
    SweepServo();
    void initServo(
        int servoPin,
        int stepDelayMs,
        int initPosition);
    void doSweep();
    void setTargetPosition(int position);
    Servo getServo();

  private:
    Servo servo;
    int stepDelayMs;
    int currentPositionDegrees;
    int targetPositionDegrees;
    long lastSweepCommand;
};

SweepServo servos [N_SERVOS];

#endif

哈哈，这代码写的相当有意思~

我们平时在Arduino下使用舵机，是通过模拟输出引脚D3、D5、D6、D9、D10、D11六个引脚。在ros_arduino_bridge当中，我们却不需要！

从这个头文件定义可以看出，我们需要声明舵机的数量、速度、引脚、初始位置在这个头文件进行声明。在SweepServo这类里面，我们只需要去关注的只有两个函数——setTargetPosition函数和getServo函数。我们来看下这部分是怎么实现的~

#ifdef USE_SERVOS

//构造函数
SweepServo::SweepServo()
{
  this->currentPositionDegrees = 0;
  this->targetPositionDegrees = 0;
  this->lastSweepCommand = 0;
}


//初始化
void SweepServo::initServo(
    int servoPin,
    int stepDelayMs,
    int initPosition)
{
  this->servo.attach(servoPin);
  this->stepDelayMs = stepDelayMs;
  this->currentPositionDegrees = initPosition;
  this->targetPositionDegrees = initPosition;
  this->lastSweepCommand = millis();
}


// 执行扫描
void SweepServo::doSweep()
{
  int delta = millis() - this->lastSweepCommand;	//获取时间差值

  //判定时间差值，每经过一定时间执行
  if (delta > this->stepDelayMs) {
    //检差当前角度值和目标角度值
    if (this->targetPositionDegrees > this->currentPositionDegrees) {
      this->currentPositionDegrees++;
      this->servo.write(this->currentPositionDegrees);	//舵机执行
    }
    else if (this->targetPositionDegrees < this->currentPositionDegrees) {
      this->currentPositionDegrees--;
      this->servo.write(this->currentPositionDegrees);	//舵机执行
    }
    
    this->lastSweepCommand = millis();	//迭代计时
  }
}

//设置舵机角度值
void SweepServo::setTargetPosition(int position)
{
  this->targetPositionDegrees = position;
}

//读取舵机角度值
Servo SweepServo::getServo()
{
  return this->servo;
}

#endif

在这里我看到了熟悉的servo.attach设定舵机引脚和servo.write设定舵机角度值~

又是一个百思不得姐的地方！但是确实是可以实现非模拟输出引脚驱动舵机~

代码单独看每行都认知，放到一起来解读大佬的思想，确实是很懵逼。不知道有没有小伙伴试一下非模拟输出引脚能否驱动舵机呢~

最后是sensors.h传感器驱动头文件，我们来看下内容~

float microsecondsToCm(long microseconds)
{
  return microseconds / 29 / 2;
}

long Ping(int pin) {
  long duration, range;

  pinMode(pin, OUTPUT);
  digitalWrite(pin, LOW);
  delayMicroseconds(2);
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, LOW);

  //同样的引脚用于从PING读取信号))):HIGH
  //脉冲的持续时间是发送的时间(以微秒为单位)
  // ping到接收到它的回波的对象。
  pinMode(pin, INPUT);
  duration = pulseIn(pin, HIGH);

  range = microsecondsToCm(duration);
  
  return(range);
}

好家伙，这不就是我们超声波测距传感器的代码~

但是我是万万没想到，超声波传感器的发射引脚和接收引脚是同一个！

这真的是把Atmega328P的资源给用到了极致，牛逼！

整体代码的骚操作有很多，真的是佩服！不过个人感觉，这份代码最低可能是两个人完成的，感受了两种完全不同的代码规范！

最后就是我们的主程序，也是我们编译烧录的主体部分内容！

#define USE_BASE      // 启用基本控制器代码
//#undef USE_BASE     // 禁用基本控制器代码

/ /定义你正在使用的电机控制器和编码器库
#ifdef USE_BASE
   /* The Pololu VNH5019 dual motor driver shield */
   //#define POLOLU_VNH5019

   /* The Pololu MC33926 dual motor driver shield */
   //#define POLOLU_MC33926

   /* The RoboGaia encoder shield */
   //#define ROBOGAIA
   
   /*编码器直接连接到Arduino板*/
   #define ARDUINO_ENC_COUNTER

   /*L298电机驱动器*/
   #define L298_MOTOR_DRIVER
#endif

#define USE_SERVOS  // 使能使用PWM伺服定义在servos.h
//#undef USE_SERVOS     // 禁用PWM伺服的使用

在代码的开头，我们要启用基本控制器代码，基本控制器我们是编码器连接到Arduino、L298的电机驱动器，如果你有舵机要有的话，需要打开舵机的PWM伺服使能。这里使用预编译 处理，好评！

/*串口波特率*/
#define BAUDRATE     57600

/*最大PWM信号*/
#define MAX_PWM        255

#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif

/*包含串行命令的定义
#include "commands.h"

/* 传感器函数 */
#include "sensors.h"

/*包括伺服支持，如果需要*/
#ifdef USE_SERVOS
   #include <Servo.h>
   #include "servos.h"
#endif

#ifdef USE_BASE
  /*电机驱动函数定义*/
  #include "motor_driver.h"

  /*编码器驱动函数的定义*/
  #include "encoder_driver.h"

  /* PID参数和函数*/
  #include "diff_controller.h"

  /* PID循环每秒运行30次*/
  #define PID_RATE           30     // Hz

  /*将速率转换为间隔
  const int PID_INTERVAL = 1000 / PID_RATE;
  
  /*跟踪下一次我们做PID计算的时候
  unsigned long nextPID = PID_INTERVAL;

  /*停止机器人，如果它没有收到一个移动命令在这个毫秒数*/
  #define AUTO_STOP_INTERVAL 2000
  long lastMotorCommand = AUTO_STOP_INTERVAL;
#endif

/* 变量初始化 */

// 一对变量来帮助解析串行命令
int arg = 0;
int index = 0;

// 变量来保存输入字符
char chr;

// 变量来保存当前的单字符命令
char cmd;

// 存放第一个和第二个参数的字符数组
char argv1[16];
char argv2[16];

// 参数转换为整数
long arg1;
long arg2;

/*清除当前命令参数*/
void resetCommand() {
  cmd = NULL;
  memset(argv1, 0, sizeof(argv1));
  memset(argv2, 0, sizeof(argv2));
  arg1 = 0;
  arg2 = 0;
  arg = 0;
  index = 0;
}

/* 运行一个命令，命令在Commands.h中定义 */
int runCommand() {
  int i = 0;
  char *p = argv1;
  char *str;
  int pid_args[4];
  arg1 = atoi(argv1);
  arg2 = atoi(argv2);
  
  switch(cmd) {
  case GET_BAUDRATE:
    Serial.println(BAUDRATE);
    break;
  case ANALOG_READ:
    Serial.println(analogRead(arg1));
    break;
  case DIGITAL_READ:
    Serial.println(digitalRead(arg1));
    break;
  case ANALOG_WRITE:
    analogWrite(arg1, arg2);
    Serial.println("OK"); 
    break;
  case DIGITAL_WRITE:
    if (arg2 == 0) digitalWrite(arg1, LOW);
    else if (arg2 == 1) digitalWrite(arg1, HIGH);
    Serial.println("OK"); 
    break;
  case PIN_MODE:
    if (arg2 == 0) pinMode(arg1, INPUT);
    else if (arg2 == 1) pinMode(arg1, OUTPUT);
    Serial.println("OK");
    break;
  case PING:
    Serial.println(Ping(arg1));
    break;
#ifdef USE_SERVOS
  case SERVO_WRITE:
    servos[arg1].setTargetPosition(arg2);
    Serial.println("OK");
    break;
  case SERVO_READ:
    Serial.println(servos[arg1].getServo().read());
    break;
#endif
    
#ifdef USE_BASE
  case READ_ENCODERS:
    Serial.print(readEncoder(LEFT));
    Serial.print(" ");
    Serial.println(readEncoder(RIGHT));
    break;
   case RESET_ENCODERS:
    resetEncoders();
    resetPID();
    Serial.println("OK");
    break;
  case MOTOR_SPEEDS:
    /* 重置自动停止定时器 */
    lastMotorCommand = millis();
    if (arg1 == 0 && arg2 == 0) {
      setMotorSpeeds(0, 0);
      resetPID();
      moving = 0;
    }
    else moving = 1;
    leftPID.TargetTicksPerFrame = arg1;
    rightPID.TargetTicksPerFrame = arg2;
    Serial.println("OK"); 
    break;
  case UPDATE_PID:
    while ((str = strtok_r(p, ":", &p)) != '\0') {
       pid_args[i] = atoi(str);
       i++;
    }
    Kp = pid_args[0];
    Kd = pid_args[1];
    Ki = pid_args[2];
    Ko = pid_args[3];
    Serial.println("OK");
    break;
#endif
  default:
    Serial.println("Invalid Command");
    break;
  }
}

这部分的代码是很核心的一个内容，我们串口控制部分的指令解析就是由这一部分的switch...case语句来完成的。同时在这部分也可以看到波特率、数据处理的频率等信息。我们根据这部分的内容，可以推导出怎么来用这些指令，当然你也可以去阅读官方的手册！

/* 初始化函数 */
void setup() {
  Serial.begin(BAUDRATE);

// 初始化电机控制器(如果使用) */
#ifdef USE_BASE
  #ifdef ARDUINO_ENC_COUNTER
    //设置为输入模式
    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);
  #endif
  initMotorController();
  resetPID();
#endif

/* 如果使用舵机 */
  #ifdef USE_SERVOS
    int i;
    for (i = 0; i < N_SERVOS; i++) {
      servos[i].initServo(
          servoPins[i],
          stepDelay[i],
          servoInitPosition[i]);
    }
  #endif
}

对不起，请原谅我的无知！

看到了这setup函数，看到了关于电机编码器中断部分的内容，算是把之前留的坑给补回来！牛逼！

/* 进入主循环。
   从串口读取和解析输入，并运行任何有效的命令。
   在目标时间间隔运行PID计算并检查自动停止条件。
*/
void loop() {
  while (Serial.available() > 0) {
    
    chr = Serial.read();

    //使用CR终止符
    if (chr == 13) {
      if (arg == 1) argv1[index] = NULL;
      else if (arg == 2) argv2[index] = NULL;
      runCommand();
      resetCommand();
    }
    //使用空格分隔命令的各个部分
    else if (chr == ' ') {
      //遍历参数
      if (arg == 0) arg = 1;
      else if (arg == 1)  {
        argv1[index] = NULL;
        arg = 2;
        index = 0;
      }
      continue;
    }
    else {
      if (arg == 0) {
        //第一个参数是单字母命令
        cmd = chr;
      }
      else if (arg == 1) {
        //后续参数可以是多个字符
        argv1[index] = chr;
        index++;
      }
      else if (arg == 2) {
        argv2[index] = chr;
        index++;
      }
    }
  }
  
//如果我们使用基础控制，在适当的时间间隔运行PID计算
#ifdef USE_BASE
  if (millis() > nextPID) {
    updatePID();
    nextPID += PID_INTERVAL;
  }
  
  //检查是否超过了自动停止间隔
  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
    setMotorSpeeds(0, 0);
    moving = 0;
  }
#endif

//舵机扫描执行
#ifdef USE_SERVOS
  int i;
  for (i = 0; i < N_SERVOS; i++) {
    servos[i].doSweep();
  }
#endif
}

这是程序的最后一部分内容，可以看到loop主函数部分只有串口读取数据，然后就是把数据传到runCommand函数执行即可，而最后的内容是更新PID以及检测是否存在AUTO_STOP_INTERVAL（前文有，2000ms）内没有数据（意味着断开连接），如果出现后者，机器人则会执行停止。

不过关于舵机和超声波传感器的内容，还是有待考究~

怪哉~