如何搭建ROS小车底盘（三）

上一节已经介绍下位机Arduino mega2560上的控制程序编码器反馈、电机控制和PID算法，这一节，我们介绍下主程序。它的工作流程如下：

• 实时读取上位机的指令并作出对应的响应， 支持的指令见源文件commands.h
  
  • GET_BAUDRATE 获取串口通讯的比特率
  • READ_ENCODERS 读取左右轮编码器的计数
  • MOTOR_SPEEDS 设置左右轮的期望速度
  • RESET_ENCODERS 重置编码器的计数
  • UPDATE_PID 更新PID参数
  • READ_PIDOUT 读取PID计算的PWM值，为后续调整PID参数提供参考
  • READ_PIDIN 读取一个PID周期内编码器的计数，为后续调整PID参数提供参考
• 在每个PID周期内，比较左右轮期望速度和实际速度的差异，调整PWM的。
• 如果AUTO_STOP_INTERVAL毫秒内上位机没有期望速度，就会停止小车运动。

源代码如下：

ROSArduinoBridge.ino

#define BAUDRATE 115200
#define MAX_PWM 255

#include "Arduino.h"
#include "commands.h"
#include "motor_driver.h"
#include "encoder_driver.h"
#include "diff_controller.h"
#define PID_RATE 30 // Hz
const int PID_INTERVAL = 1000 / PID_RATE;
unsigned long nextPID = PID_INTERVAL;
#define AUTO_STOP_INTERVAL 2000
long lastMotorCommand = AUTO_STOP_INTERVAL;

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

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 READ_PIDIN:
      Serial.print( readPidIn(LEFT));
      Serial.print(" ");
      Serial.println( readPidIn(RIGHT));
      break;      
    case READ_PIDOUT:
      Serial.print( readPidOut(LEFT));
      Serial.print(" ");
      Serial.println( readPidOut(RIGHT));
      break;
    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);
        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;
    default:
      Serial.println("Invalid Command");
      break;
  }
}

unsigned long time = 0, old_time = 0;
void setup() {
  Serial.begin(BAUDRATE);
  initEncoders();
  initMotorController();
  resetPID();
}


void loop() {
  while (Serial.available() > 0) {
    chr = Serial.read();
    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++;
      }
    }
  }

  if (millis() > nextPID) {
    updatePID();
    nextPID += PID_INTERVAL;
  }

  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {
    ;
    setMotorSpeeds(0, 0);
    moving = 0;
  }
}

commands.h

#define GET_BAUDRATE 'b'
#define READ_ENCODERS 'e'
#define MOTOR_SPEEDS 'm'
#define RESET_ENCODERS 'r'
#define UPDATE_PID 'u'
#define READ_PIDOUT 'f'
#define READ_PIDIN 'i'

#define LEFT 0
#define RIGHT 1
#define FORWARDS true
#define BACKWARDS false