Wednesday, September 23, 2015

Servo Controller: Binary Serial Communication (Arduino Side)

ASCII vs Binary


ASCII text communication is convenient for debugging, but it's not particular efficient. The microcontroller is having to take a string of characters, convert those strings into numbers, then process the instructions of those numbers.

For example, set servo 12 to a value of 50 percent:
s=12
p=50
Total 10 bytes as ASCII (including carriage returns).

Binary communication is much more efficient. A pointer can be used to interpret the array of bytes as a structure, so there is no CPU expensive conversions from strings to numbers. And where ASCII would take 5 bytes to represent 65535, binary would need just 2.

Let's define a structure for commands...

const int16_t commandLength_bytes = 5;

struct CommandSetServoValueStructure
{
  byte     command;
  uint16_t servoNumber;
  uint16_t newValue;
  // Must denote structure as packed for variables to be
  // properly interpreted from a byte array.
} __attribute__((__packed__));

We'll say command 1 is to set a percent value.
So now the byte array looks like:
[1][12][0][50][0]
Total 5 bytes as binary.

5 bytes instead of 10!

But let's make the example more interesting. We'll say command 2 is to set an upper pulse value. We want to control servo 10,000 to a pulse value of 1100.

That byte array looks like:
[2][16][39][76][4]
Total 5 bytes as binary.

As a string it would have been:
s=10000
u=1100
Total 15 bytes as ASCII (includes carriage returns).

5 bytes instead of 15!

Now imagine we were sending 10,000 servo commands, saving 10 bytes per command. 100 KB is a nice savings over a serial cable.

That saves on the amount of data we're sending over, but what about interpreting the data? String manipulation is a (relatively) slow process. In C, you can use a pointer to interpret that array of bytes as whatever structure you wish.

Say the incoming data is stored in an array...

byte *incomingCommand;

Use a pointer to interpret that array of bytes as our desired structure...

CommandSetServoValueStructure *commandSetServoPercent = (CommandSetServoValueStructure *)incomingCommand;

Then you can dereference the values from the structure to read them...

commandSetServoPercent->servoNumber
commandSetServoPercent->newValue

Reading Binary


In the examples above I listed out the byte array for values such as 12 - [12][0] and 10,000 - [16][39]. But why is it [12][0] and not [0][12] and where do you get 16 and 39 from 10,000?

Byte Order

With a variable consisting of more than 1 byte, there are two ways it can be ordered:
[Byte 1][Byte 2][Byte 3][Byte 4]
or
[Byte 4][Byte 3][Byte 2][Byte 1]

The order is known as Endianness. https://en.wikipedia.org/wiki/Endianness

The computer your code is running on could order variables either way. It doesn't matter which order is used, just so long as both ends of the communication use the same one. Swapping the order of the bytes is trivial, and there are plenty of examples around on how to do that. Since my .NET application and my Arduino use the same byte order, I didn't have to do any byte swapping.

Parsing a Number with >1 Bytes

Windows Calculator has a Programmer mode (under the View menu) that comes in very handy.

I've switched to Programmer mode, selected "Dec", and typed in 10000.

I then opened 2 more Calculators; entering the "more significant" byte into one and the "less significant" byte into the other. Now you can see where the 16 and 39 mentioned earlier come from!


Dual Input Modes


Rather than lose the ASCII debugging mode, I'm going to modify the code to support both kinds of input. When the microcontroller first starts, if the user presses the ENTER key (new line), then it goes into ASCII mode. If the program sends a character code of 128 (an extended code a user is unlikely to type), then it goes into binary mode.

Define an enumeration for the input modes and create a variable to keep track of what we're in...

enum inputModes
{
  InputModeUndefined = 0,
  InputModeASCII = 1,
  InputModeBinary = 2
};

int16_t inputMode = InputModeUndefined;

The main program loop of the Arduino continuously checks if data is available over the Serial line. If the input mode has never been defined, then it calls getSerialInputMode() to determine which input it is in. After the input mode has been determined, it will route all incoming Serial input to the appropriate parser - getSerialASCIICommand() or getSerialBinaryCommand().

void loop()
{
  // First input will be the mode to be in: binary or text
  // Once the flag is set we'll send the serial input to the
  // appropriate processor.
  if (Serial.available() > 0)
  {
    switch (inputMode)
    {
      case InputModeUndefined:
        getSerialInputMode();
        break;
      case InputModeASCII:
        getSerialASCIICommand();
        break;
      case InputModeBinary:
        getSerialBinaryCommand();
        break;
    }
  }
  return;
}

void getSerialInputMode()
{
  int16_t incomingByte = Serial.read();
  switch (incomingByte)
  {
    case 10: // New Line
      Serial.println("ASCII Input Mode");
      inputMode = InputModeASCII;
      break;
    case 128: // Extended ASCII Code
      Serial.println("Binary Input Mode");
      inputMode = InputModeBinary;
      break;
    default:
      Serial.println("Send 'New Line' for ASCII Input Mode.");
      break;
  }
  return;
}

The getSerialASCIICommand() function is basically how the commands were parsed before.

void getSerialASCIICommand()
{
  String serialInputString = Serial.readStringUntil('\n');

  // Echo back the command.
  Serial.println(serialInputString);
  
  if (serialInputString.startsWith("s=")) // Servo
  {
    String servoNumberString = serialInputString.substring(2);
    debugServoNumber = servoNumberString.toInt();
    Serial.print("Debug servo:");
    Serial.println(debugServoNumber);
  }
  else if (serialInputString.startsWith("p=")) // Percent Value
  {
    String valuePercentString = serialInputString.substring(2);
    float newValue_percent = valuePercentString.toFloat();
    servos[debugServoNumber].setToValue_percent(newValue_percent);
    Serial.print("Set percent:");
    Serial.println(newValue_percent);
  }
  else if (serialInputString.startsWith("v=")) // Pulse Value
  {
    String pulseValueString = serialInputString.substring(2);
    uint16_t newPulseValue = pulseValueString.toInt();
    servos[debugServoNumber].setToValue_pulseLength(newPulseValue);
    Serial.print("Set pulse:");
    Serial.println(newPulseValue);
  }
  else if (serialInputString.startsWith("u=")) // Upper Pulse Value
  {
    String upperPulseValueString = serialInputString.substring(2);
    uint16_t newUpperPulseValue = upperPulseValueString.toInt();
    servos[debugServoNumber].setUpperPulseLength(newUpperPulseValue);
    servos[debugServoNumber].setToMaximumValue();
    Serial.print("Set upper pulse:");
    Serial.println(newUpperPulseValue);
  }
  else if (serialInputString.startsWith("l=")) // Lower Pulse Value
  {
    String lowerPulseValueString = serialInputString.substring(2);
    uint16_t newLowerPulseValue = lowerPulseValueString.toInt();
    servos[debugServoNumber].setLowerPulseLength(newLowerPulseValue);
    servos[debugServoNumber].setToMinimumValue();
    Serial.print("Set lower pulse:");
    Serial.println(newLowerPulseValue);
  }
  return;
}

The getSerialBinaryCommand() is the new code. We'll define a byte array and a variable to keep track of where we are in that array.

byte incomingBytes[commandLength_bytes];
int16_t incomingBytePosition = 0;

Upon receiving a byte over the Serial line, it stores it in the byte array and increments the position. The very first byte of the array is the command. So once we get that first byte, we can determine if we've received enough bytes for our structure and then process as appropriate. For now all the commands use the same structure and thus are the same length. However, that will very likely change in the future with more advanced commands.

void getSerialBinaryCommand()
{
  incomingBytes[incomingBytePosition] = Serial.read();
  incomingBytePosition += 1;

  switch(incomingBytes[0])
  {
    case 0:
      // Reset Commands
      // If the controller gets in an unsynchronized state with the
      // serial commands, then it can send a series of all 0s to
      // reset to a new command. The number of 0s required would vary
      // by the size of the command structure the controller thinks
      // we're populating (and how far into that structure it thinks
      // we have populated). Once we get out of populating the structure,
      // the subsequent "0" bytes just continue to fall into this code
      // and are handled fine.

      // Reset incoming array position for next command.
      incomingBytePosition = 0;
      break;
    case 1:
      if (incomingBytePosition >= commandLength_bytes)
      {
        processCommandSetServoPercent(&incomingBytes[0]);
        // Reset incoming array position for next command.
        incomingBytePosition = 0;
      }
      break;
    case 2:
      if (incomingBytePosition >= commandLength_bytes)
      {
        processCommandSetServoUpperLimit(&incomingBytes[0]);
        // Reset incoming array position for next command.
        incomingBytePosition = 0;
      }
      break;
    case 3:
      if (incomingBytePosition >= commandLength_bytes)
      {
        processCommandSetServoLowerLimit(&incomingBytes[0]);
        // Reset incoming array position for next command.
        incomingBytePosition = 0;
      }
      break;
  }
  
  return;
}

Processing each command is very efficient: Interpret the incoming bytes as a structure, then read the values.

void processCommandSetServoPercent(byte *incomingCommand)
{
  CommandSetServoValueStructure *commandSetServoPercent = (CommandSetServoValueStructure *)incomingCommand;
  if (commandSetServoPercent->servoNumber >= 0 && 
      commandSetServoPercent->servoNumber < numberOfServos)
  {
    servos[commandSetServoPercent->servoNumber].setToValue_percent((float)commandSetServoPercent->newValue);
  }
  return;
}

void processCommandSetServoUpperLimit(byte *incomingCommand)
{
  CommandSetServoValueStructure *commandSetServoUpperLimit = (CommandSetServoValueStructure *)incomingCommand;
  if (commandSetServoUpperLimit->servoNumber >= 0 && 
      commandSetServoUpperLimit->servoNumber < numberOfServos)
  {
    servos[commandSetServoUpperLimit->servoNumber].setUpperPulseLength((float)commandSetServoUpperLimit->newValue);
  }
  return;
}

void processCommandSetServoLowerLimit(byte *incomingCommand)
{
  CommandSetServoValueStructure *commandSetServoLowerLimit = (CommandSetServoValueStructure *)incomingCommand;
  if (commandSetServoLowerLimit->servoNumber >= 0 && 
      commandSetServoLowerLimit->servoNumber < numberOfServos)
  {
    servos[commandSetServoLowerLimit->servoNumber].setLowerPulseLength((float)commandSetServoLowerLimit->newValue);
  }
  return;
}

Copyright (c) 2015 Clinton Kam
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.