omniWheelCareRobot/arduinoCode/mobileBase/mobileBase.ino

/*********************************************************************
  Description: A set of simple serial commands to control a omniWheel drive
  robot and receive back sensor and odometry data. Default configuration
  assumes use of an Arduino Mega 2560 + motor driver board. Edit the
  readEncoder() and setMotorSpeed() wrapper functions if using different
  motor controller or encoder method.
  Author: www.corvin.cn
  History: 20171129:init code;
         20180209:增加代码版本号,定义初始版本号1.0,通过v命令来获取;
         20180515:更新到1.1版本,整理代码架构,精简代码，使流程更清晰;
*********************************************************************/
#include "sound.h"
#include "commands.h"
#include "sensors.h"
#include "motor_driver.h"
#include "encoder_driver.h"
#include "pid_controller.h"


/******************** USER AREAR ********************/
/* current code version */
#define VERSION    1.1

/* Stop the robot if it hasn't received a movement command
  in this number of milliseconds */
#define  AUTO_STOP_INTERVAL  260
/******************** USER END **********************/

/* Serial port baud rate */
#define BAUDRATE   57600

/* Run the PID loop at 30 times per second -Hz */
#define PID_RATE   30

/* Convert the rate into an interval */
const int PID_INTERVAL = 1000 / PID_RATE;

/* Track the next time we make a PID calculation */
unsigned long nextPID = PID_INTERVAL;

long lastMotorCommand = AUTO_STOP_INTERVAL;

/* Variable initialization */
// A pair of varibles to help parse serial commands (thanks Fergs)
int arg   = 0;
int index = 0;

// Variable to hold an input character
char chr;

// Variable to hold the current single-character command
char cmd;

// Character arrays to hold the first,second,third arguments
char argv1[48];
char argv2[5];
char argv3[5];

// The arguments converted to integers
int arg1 = 0;
int arg2 = 0;
int arg3 = 0;

/* Clear the current command parameters */
void resetCommand()
{
  cmd = '\0';
  memset(argv1, 0, sizeof(argv1));
  memset(argv2, 0, sizeof(argv2));
  memset(argv3, 0, sizeof(argv3));

  arg1  = 0;
  arg2  = 0;
  arg3  = 0;
  arg   = 0;
  index = 0;
}

/* Run a command.  Commands are defined in commands.h */
int runCommand()
{
  int i   = 0;
  char *p = argv1; //p pointer for update pid parameter
  char *str;
  int pid_args[12];

  if (cmd != 'u') //cmd don't match update pid will convert
  {
    arg1 = atoi(argv1);
    arg2 = atoi(argv2);
    arg3 = atoi(argv3);
  }
  
  switch (cmd)
  {
    case GET_BAUDRATE: //'b'
      Serial.println(BAUDRATE);
      break;

    case ANALOG_READ:  //'a'
      Serial.println(analogRead(arg1));
      break;

    case DIGITAL_READ: //'d'
      Serial.println(digitalRead(arg1));
      break;

    case ANALOG_WRITE:
      analogWrite(arg1, arg2);
      Serial.println("OK");
      break;

    case DIGITAL_WRITE: //'w'
      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 READ_ENCODERS:  //'e'
      Serial.print(readEncoder(A_WHEEL));
      Serial.print(" ");
      Serial.print(readEncoder(B_WHEEL));
      Serial.print(" ");
      Serial.println(readEncoder(C_WHEEL));
      break;

    case RESET_ENCODERS: //'r'
      resetEncoders();
      resetPID();
      Serial.println("OK");
      break;

    case MOTOR_SPEEDS: //'m'
      lastMotorCommand = millis();  /* Reset the auto stop timer */
      if (arg1 == 0 && arg2 == 0 && arg3 == 0)
      {
        setMotorSpeeds(0, 0, 0);
        resetPID();
        setMoveStatus(0);
      }
      else
      {
        setMoveStatus(1);
      }
      setWheelPIDTarget(arg1, arg2, arg3);
      Serial.println("OK");
      break;

    case UPDATE_PID: //'u'
      while ((str = strtok_r(p, ":", &p)) != '\0')
      {
        pid_args[i] = atoi(str);
        i++;
      }
      updatePIDParam(A_WHEEL, pid_args[0], pid_args[1], pid_args[2],  pid_args[3]);
      updatePIDParam(B_WHEEL, pid_args[4], pid_args[5], pid_args[6],  pid_args[7]);
      updatePIDParam(C_WHEEL, pid_args[8], pid_args[9], pid_args[10], pid_args[11]);
      Serial.println("OK");
      break;

    case SOUND_BEEP: //'f'
      if (arg1 == BASE_POWERON_BEEP)
      {
        basePowerOnBeep();
      }
      else if (arg1 == BASE_POWEROFF_BEEP)
      {
        basePowerOffBeep();
      }
      Serial.println("OK");
      break;

    case READ_PIDIN:
      Serial.print(readPidIn(A_WHEEL));
      Serial.print(" ");
      Serial.print(readPidIn(B_WHEEL));
      Serial.print(" ");
      Serial.println(readPidIn(C_WHEEL));
      break;

    case READ_PIDOUT:
      Serial.print(readPidOut(A_WHEEL));
      Serial.print(" ");
      Serial.print(readPidOut(B_WHEEL));
      Serial.print(" ");
      Serial.println(readPidOut(C_WHEEL));
      break;

    case CODE_VERSION:
      Serial.println(VERSION);
      break;

    default:
      Serial.println("Invalid Command");
      break;
  }

  return 0;
}

/* Setup function--runs once at startup. */
void setup()
{
  Serial.begin(BAUDRATE);

  initSensors();
  initSoundPin();
  initEncoders();
  initMotorsPinMode();
  resetPID();
}

/* Enter the main loop.  Read and parse input from the serial port
   and run any valid commands. Run a PID calculation at the target
   interval and check for auto-stop conditions.*/
void loop()
{
  while (Serial.available() > 0)
  {
    chr = Serial.read();  //Read the next character

    if (chr == 13)  //Terminate a command with a CR
    {
      if (arg == 1)
      {
        argv1[index] = '\0';
      }
      else if (arg == 2)
      {
        argv2[index] = '\0';
      }
      else if (arg == 3)
      {
        argv3[index] = '\0';
      }
      runCommand();
      resetCommand();
    }
    else if (chr == ' ') // Use spaces to delimit parts of the command
    {
      // Step through the arguments
      if (arg == 0)
      {
        arg = 1;
      }
      else if (arg == 1)
      {
        argv1[index] = '\0';
        arg   = 2;
        index = 0;
      }
      else if (arg == 2)
      {
        argv2[index] = '\0';
        arg   = 3;
        index = 0;
      }
      continue;
    }
    else // process single-letter command
    {
      if (arg == 0)
      {
        cmd = chr;  //The first arg is the single-letter command
      }
      else if (arg == 1)
      {
        // Subsequent arguments can be more than one character
        argv1[index] = chr;
        index++;
      }
      else if (arg == 2)
      {
        argv2[index] = chr;
        index++;
      }
      else if (arg == 3)
      {
        argv3[index] = chr;
        index++;
      }
    }
  }//end while()

  //run a PID calculation at the appropriate intervals
  if (millis() > nextPID)
  {
    updatePID();
    nextPID += PID_INTERVAL;
  }

  //Check to see if we have exceeded the auto-stop interval
  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL)
  {
    setMotorSpeeds(0, 0, 0); //stop motors
    setMoveStatus(0);
  }
}