sending vast amounts of data via Serial + Timer Interrupt

Hey guys.
Im currently building a 6 axis robot arm, its already working kind off but i want to get it to run a bit smoother. i use my own variation of the TeensyStep Library to control the motors, and this involves an Interrupt Service Routine to trigger the steps.

every single axis got a rotary encoder, those get read out by two lower Level MCUs ( arduino Pro Mini) and those send the data to my main MCU ( Teensy3.5). each encoder needs to transmit one 14 bit value, eg. "16384"

those get collected in my Teensy 3.5 and that send those to my processing sketch, on my PC.

for now i construct Strings like the following

S-96033-50001-14312-12421-15367-78723-E and send that as often as possible to my pc, to calculate the inverseKinematics, and send the same string, but with different angle information back on my teensy (also as often as possible)

but this seems to interfere with the ISR and doesnt seem to be very stable.

are there maybe better ways to send the information more efficiently? this is my code i use for now:

#include "StepControl.h"

Stepper motor1(24, 2, 360, 4); //Step Dir
Stepper motor2(25, 3, 360, 50);
Stepper motor3(26, 4, 360, 20);
Stepper motor4(27, 5, 360, 15);
Stepper motor5(28, 7, 360, 15);
Stepper motor6(29, 6, 360, 5);
StepControl <> controller;
Stepper* motors[] = {&motor1, &motor2, &motor3, &motor4, &motor5, &motor6};


String msg;
String msg1 = "";
String msg2 = "";

float q[6];
float qdot[6];
float p[6];
int EnablePin = 16;
int i = 15;
int pos[6];
boolean dat1 = false, dat2 = false, dat3 = false;
//int pos1 = 0, pos2 = 0, pos3 = 0, pos4 = 0, pos5 = 0, pos6 = 0;


void setup() {
  
  Serial1.begin(500000);
  Serial4.begin(500000);
  Serial.begin(2000000); // Start serial communication at 20000000 bps

  pinMode(EnablePin, OUTPUT);
  digitalWrite(EnablePin, 1);

  //connect to lower Level MCUS

  while ( !dat1 || !dat2 || !dat3) {
    updateSerial();
    updateAngles();
  }
  for (int i = 0; i < 6; i++) {
    motors[i] ->setLastAngle(pos[i]);
    motors[i] ->setRawRotation(pos[i]);
    motors[i]->updatePosition();
    motors[i]->setDesiredState(0, 0);
    motors[i]-> setZeroPosition();
  }
}


void loop() {

  updateSerial();
  updateAngles();

  for (int i = 0; i < 6; i++) {
    motors[i]->setRawRotation(pos[i]);
    motors[i]->updatePosition();
    motors[i]->setDesiredState(qdot[i], q[i]);
  }
  controller.move(motors);
  
}

void updateAngles() {
  
  // read lowerlvl MCUs to obtain actual Jointangles

  while (Serial1.available() > 0) {
    cli();
    char c1 =  Serial1.read();
    msg1 += c1;
    sei();
    if (msg1.length() == 28 && msg1.charAt(0) == 'A' && msg1.charAt(26) == 'O' ) {
      dat1 = true;
      pos[3] = -(msg1.substring(2, 7).toInt() - 10000);
      pos[4] = -(msg1.substring(8, 13).toInt() - 10000);
      pos[2] = -(msg1.substring(14, 19).toInt() - 10000);
      pos[5] = (msg1.substring(20, 25).toInt() - 10000);
      // Serial.println(2);
      msg1 = "";

    }
    if (msg1.length() > 28) {
      msg1 = "";
    }
  }


  while (Serial4.available() > 0) {
    cli();
    char c2 =  Serial4.read();
    msg2 += c2;
    sei();
    //Serial.println(msg2);
    if (msg2.length() == 15 && msg2.charAt(0) == 'A' && msg2.charAt(14) == 'O') {
      //Serial.println(msg2);
      dat2 = true;
      pos[0] = -(msg2.substring(2, 7).toInt() - 10000);
      pos[1] = -(msg2.substring(8, 13).toInt() - 10000);
      msg2 = "";

    }
    if (msg2.length() > 16) {
      msg2 = "";
    }
  }
}

void updateSerial() {

//read transmitted desired Jointangles

  while (Serial.available() > 0) { // If data is available to read,
    cli();
    char c = Serial.read();
    msg += c;
    sei();
    if (msg.length() == 39 && msg.charAt(0) == 'S' && msg.charAt(38) == 'E') {
      dat3 = true;
      String sq1 = msg.substring(2, 7);
      String sq2 = msg.substring(8, 13);
      String sq3 = msg.substring(14, 19);
      String sq4 = msg.substring(20, 25);
      String sq5 = msg.substring(26, 31);
      String sq6 = msg.substring(32, 37);

      //reverse encryption algorithm
      q[0] = (sq1.toInt() - 50000) * 0.01;
      q[1] = (sq2.toInt() - 50000) * 0.01;
      q[2] = (sq3.toInt() - 50000) * 0.01;
      q[3] = (sq4.toInt() - 50000) * 0.01;
      q[4] = ((sq5.toInt() - 50000) * 0.01) + 0.33333 * q[3];
      q[5] = (sq6.toInt() - 50000) * 0.02;
      // Serial.println(1);
      msg = "";
    }


    // send actual Joint angles back
    String praefix = "A-";
    String comma = "-";
    String suffix = "-O";
    for (int i = 0; i < 6; i++) {
      p[i] = motors[i]->getJointPosition();
    }

    //encrypt data in one string
    String dataS = praefix + round((p[0] * 100) + 50000) + comma + round((p[1] * 100) + 50000) + comma + round((p[2] * 100) + 50000) +
                   comma + round((p[3] * 100) + 50000) + comma + round((p[4] * 100) + 50000) + comma + round((p[5] * 100) + 50000)  + suffix;

    byte bytes[dataS.length() + 1];
    dataS.getBytes(bytes, dataS.length() + 1);
    Serial.write(bytes, dataS.length() + 1);

  }

}

so i know this is a bit more advanced but maybe people can help me out here.

it just doesnt run as smooth as i want.

but this seems to interfere with the ISR and doesnt seem to be very stable.

Define what exactly this means or how it manifests.

Please explain why you disable interrupts while reading from the serial buffer:

    cli();
    char c1 =  Serial1.read();
    msg1 += c1;
    sei();

RWTH_MASCHI:
Im currently building a 6 axis robot arm, its already working kind off but i want to get it to run a bit smoother. i use my own variation of the TeensyStep Library to control the motors, and this involves an Interrupt Service Routine to trigger the steps.

every single axis got a rotary encoder, those get read out by two lower Level MCUs ( arduino Pro Mini) and those send the data to my main MCU ( Teensy3.5). each encoder needs to transmit one 14 bit value, eg. "16384"

those get collected in my Teensy 3.5 and that send those to my processing sketch, on my PC.

I can't understand all that. What is the purpose of the rotary encoders?

Why are the ProMinis sending data to the Teensy? How often must the data be sent?

How many steps per second must the motors operate at? Do they all need to move at the same time?

Also, it is not a good idea to use the String (capital S) class on an Arduino as it can cause memory corruption in the small memory on an Arduino. This can happen after the program has been running perfectly for some time. Just use cstrings - char arrays terminated with '\0' (NULL).

If performance is important then the data from the ProMInis should be sent in binary form.

...R