ROS小车底盘5-底层主程序

commands.h

#define GET_BAUDRATE   'b'//获取串口波特率
#define READ_ENCODERS  'e'//读取左右轮编码器的计数
#define MOTOR_SPEEDS   'm'//设置左右轮的期望速度
#define RESET_ENCODERS 'r'//重置编码器计数
#define UPDATE_PID     'u'//更新PID 参数
#define READ_PIDOUT  'f'//读取PID计算的PWM值，为后续调整PID参数提供参考
#define READ_PIDIN  'i'//读取一个PID周期内编码器的计数，为后续调整PID参数提供参考

#define PIN_MODE       'c'//定义引脚模式
#define DIGITAL_READ   'd'//数字读取
#define ANALOG_READ    'a'//模拟读取
#define DIGITAL_WRITE  'w'//数字写入
#define ANALOG_WRITE   'x'//模拟写入

#define LEFT            0//LEFT代表数值0
#define RIGHT           1//RIGHT代表数值1
#define FORWARDS true   //FORWARDS前进代表bool真值true
#define BACKWARDS false //BACKWARDS后退代表bool假值false

在每个PID周期内，比较左右轮期望速度和实际速度的差异，调整PWM的。

如果AUTO_STOP_INTERVAL毫秒内上位机没有期望速度，就会停止小车运动

源代码如下：

ROSArduinoBridge.ino

#define BAUDRATE     115200  //通信波特率115200
#define MAX_PWM        255   //PWM最大值是255

#include "Arduino.h"        //这个是ROS环境下的一个有关arduino使用的头文件
#include "commands.h"      //上面有介绍，是是个宏定义头文件
#include "motor_driver.h"  //电机驱动库，这部分在使用不同的电机的时候可能需要修改
#include "encoder_driver.h" //编码器驱动头文件
#include "diff_controller.h"//这个目前不知道是哪个头文件，暂定，会及时更新
#define PID_RATE           30     // Hz //PID频率是30Hz,目前不知道这个宏定义的用途
const int PID_INTERVAL = 1000 / PID_RATE;//这个目前不知道作用
unsigned long nextPID = PID_INTERVAL;//目前不知道作用
#define AUTO_STOP_INTERVAL 2000  //目前不知道作用
long lastMotorCommand = AUTO_STOP_INTERVAL;//目前不知道作用

/* 变量初始化 */
int arg = 0;
int index = 0;
char chr;
char cmd;
char argv1[16];
char argv2[16];
long arg1;
long arg2;

void resetCommand() {
  cmd = NULL;//在编程中，变量以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;
    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;
    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;
  }
}