Reading/parsing incoming serial data to string

Projects Programming
1

Hello, I have a project I'm working on where I have a virtual representation of a 5-joint robotic arm in Unity, and it sends the angles of the joint (in float form) to serial in the format "[yaw],[shoulder],[elbow],[wrist],[hand]|" so a typical update might look like "90,75.43,0,54.2,180|". The Arduino then parses this data and, using Adafruit's 16-channel PWM output, drives the 5 servos to those positions.

I know Arduino is receiving the data just fine through some debugging, so the problem almost certainly isn't on Unity's end. However, I tried running it and manually sending an update (I used "90,90,90,0,90|", the arm's home position, as a test) through the serial monitor and told it to spit back what it received, and it only gave me "9". Here's my code you can look over...

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // this is the 'maximum' pulse length count (out of 4096)

float yaw, shoulder, elbow, wrist, hand;
// (7-digit float + 1 decimal + 1 endmark) * 5 servos = 45 char max
char data [45];

void receiveData () {
  // reset data
  for (int i = 0; i < 45; i++) {
    data [i] = ' ';
  }
  //Serial.readBytesUntil ('|', data, 45);
  Serial.readStringUntil('|').toCharArray(data, 45);
  Serial.println(data); // when given the test "90,90,90,0,90|", this prints out "9" for some reason.
  // parse data
  char tmp [7];
  int i;
  int place;
  // cycle through to get the individual angle values
  for (i; data [i] != ','; i++) {
    tmp [i - place] = data [i];
  }
  yaw = atof (tmp);
  place = i;
  for (i; data [i] != ','; i++) {
    tmp [i - place] = data [i];
  }
  shoulder = atof (tmp);
  place = i;
  for (i; data [i] != ','; i++) {
    tmp [i - place] = data [i];
  }
  elbow = atof (tmp);
  place = i;
  for (i; data [i] != ','; i++) {
    tmp [i - place] = data [i];
  }
  wrist = atof (tmp);
  place = i;
  for (i; data [i] != '|'; i++) {
    tmp [i - place] = data [i];
  }
  hand = atof (tmp);
}

void writeServos () {
  //order is 0=yaw, 1=shoulder, 2=elbow, 3=wrist, 4=hand
  pwm.setPWM (0, 0, map(yaw, 0, 180, SERVOMIN, SERVOMAX));
  pwm.setPWM (1, 0, map(shoulder, 0, 180, SERVOMIN, SERVOMAX));
  pwm.setPWM (2, 0, map(elbow, 0, 180, SERVOMIN, SERVOMAX));
  pwm.setPWM (3, 0, map(wrist, 0, 180, SERVOMIN, SERVOMAX));
  pwm.setPWM (4, 0, map(hand, 0, 180, SERVOMIN, SERVOMAX));
}

void setup () {
  Serial.begin (9600);

  pwm.begin();
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
}

void loop () {
  if (Serial.available() > 0) {
    receiveData ();
  }
  writeServos ();
  //delay (10); // do I need this here? For stability and stuff?
}
2

if (Serial.available() > 0) {
receiveData ();
}

You check if you have at least 1 byte.
Then you try and read all the data.
Need to ensure you have enough data to parse it up first.

if (Serial.available() > 44) {
receiveData ();
}

3 
  if (Serial.available() > 44)

Or add each byte to an array as it is received and only parse the input when it is complete.

See Serial input basics

4

I suggest that you read the serial input basics - updated thread for ideas.

5

CrossRoads:
if (Serial.available() > 0) {
receiveData ();
}

You check if you have at least 1 byte.
Then you try and read all the data.
Need to ensure you have enough data to parse it up first.

if (Serial.available() > 44) {
receiveData ();
}

I suspected this might be the case shortly after I posted this, but the problem is that the incoming data is hardly ever the full 45 chars long -- it could be as little as 10 chars, like "0,0,0,0,0|". However, the serial input basics thread provided by sterretje is looking good. I'll see if I can use the third example in it for my purposes; thanks for the link!

6

First, replace this loop:

  for (int i = 0; i < 45; i++) {
    data [i] = ' ';
  }

with:

   memset(data, 32, sizeof(data));   // Fill data's memory with spaces.

which is probably as efficient as you can write it.

In this piece of code:

  int i;
  int place;
  // cycle through to get the individual angle values
  for (i; data [i] != ','; i++) {
    tmp [i - place] = data [i];
  }

what are i and place equal to when the loop starts? (Hint: Neither you nor I know.)

Finally, what are you trying to do here:

  • Serial.readStringUntil('|').toCharArray(data, 45);*

data is already a char array. Also, instead of 45, shouldn't you read sizeof(data) - 1 as a maximum, leaving one space for the termination character?

7

Hey, update: example 3 in the serial basics thread was a real help -- it basically scrapped most of what I had. Here's the new code if you're interested:

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define YAWMIN  132
#define YAWMAX  555
#define SHOULDERMIN  127
#define SHOULDERMAX  555
#define ELBOWMIN  152
#define ELBOWMAX  572
#define WRISTMIN  138
#define WRISTMAX  569
#define HANDMIN  154
#define HANDMAX  620

float yaw, shoulder, elbow, wrist, hand;
// ((7-digit float + 1 decimal + 1 endmark) * 5 servos) + 1 startmark + 1 endmark = 47 char max
const byte numChars = 47;
char data [numChars];
boolean newData = false;

void receiveData () {
  static boolean recvInProgress = false;
  static byte ndx = 0;
  char startMarker = '<';
  char endMarker = '>';
  char rc;
 
  while (Serial.available() > 0 && newData == false) {
    rc = Serial.read();

    if (recvInProgress == true) {
      if (rc != endMarker) {
        data[ndx] = rc;
        ndx++;
        if (ndx >= numChars) {
          ndx = numChars - 1;
        }
      }
      else {
        data[ndx] = '\0'; // terminate the string
        recvInProgress = false;
        ndx = 0;
        newData = true;
      }
    }

    else if (rc == startMarker) {
      recvInProgress = true;
    }
  }
}

void parseData () {
  /*
  // reset data
  for (int i = 0; i < 45; i++) {
    data [i] = ' ';
  }
  //Serial.readBytesUntil ('|', data, 45);
  Serial.readStringUntil('|').toCharArray(data, 45);
  Serial.println(data); // when given the test "90,90,90,0,90|", this prints out "9" for some reason.
  */
  // parse data
  char yawTmp [8], shoulderTmp [8], elbowTmp [8], wristTmp [8], handTmp [8];
  int i;
  int place;
  // cycle through to get the individual angle values
  for (i; data [i] != ','; i++) {
    if (data [i] != ',') {
      yawTmp [i - place] = data [i];
    }
  }
  i++;
  yaw = 180 - atof (yawTmp);
  place = i;
  for (i; data [i] != ','; i++) {
    shoulderTmp [i - place] = data [i];
  }
  i++;
  shoulder = 180 - atof (shoulderTmp);
  place = i;
  for (i; data [i] != ','; i++) {
    elbowTmp [i - place] = data [i];
  }
  i++;
  elbow = 180 - atof (elbowTmp);
  place = i;
  for (i; data [i] != ','; i++) {
    wristTmp [i - place] = data [i];
  }
  i++;
  wrist = atof (wristTmp);
  place = i;
  for (i; data [i] != '|'; i++) {
    handTmp [i - place] = data [i];
  }
  hand = atof (handTmp);
}

void writeServos () {
  //order is 0=yaw, 1=shoulder, 2=elbow, 3=wrist, 4=hand
  pwm.setPWM (0, 0, map(yaw, 0, 180, YAWMIN, YAWMAX));
  pwm.setPWM (1, 0, map(shoulder, 0, 180, SHOULDERMIN, SHOULDERMAX));
  pwm.setPWM (2, 0, map(elbow, 0, 180, ELBOWMIN, ELBOWMAX));
  pwm.setPWM (3, 0, map(wrist, 0, 180, WRISTMIN, WRISTMAX));
  pwm.setPWM (4, 0, map(hand, 0, 180, HANDMIN, HANDMAX));
}

void setup () {
  Serial.begin (115200);

  pwm.begin();
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
}

void loop () {
  receiveData ();
  if (newData) {
    Serial.println(data);
    parseData ();
    Serial.println(yaw);
    Serial.println(shoulder);
    Serial.println(elbow);
    Serial.println(wrist);
    Serial.println(hand);
    newData = false;
  }
  writeServos ();
  //delay (10); // do I need this here? For stability and stuff?
}

This now works just fine.

8 
  for (int i = 0; i < 45; i++) {
    data [i] = ' ';
  }

This illustrates the fact that you do not understand what "NULL-terminated" means.

  int i;
  int place;
  // cycle through to get the individual angle values
  for (i; data [i] != ','; i++) {
    if (data [i] != ',') {
      yawTmp [i - place] = data [i];
    }
  }

This is STILL garbage. Local variables are NOT initialized unless YOU initialize them.