Working frequencies.

Arduino is used as a pwm signal generator for the servos and software pwm doesn't really produce a pwm signal, the main loop must run at the duty frequency of the servos (in my case 50-300Hz, 20-4 ms loop). Of course, arduino is more powerful than that, so after sending a first pulse and before the next pulse is sent, arduino will read the different sensors, send their data and recive the next pulse command from the raspberry pi (via bidirectional serial commands).

As the arduino loop is limited by the frequency of the servos, the main python code will also work at that frequency. As far as i know, this is because if the model gives a solution every 5ms and the actuators can only read a solution every 20ms, there will be 'useless' solutions given, resulting with the system not behaving as desired.

Arduino sketch.

The arduino code is very simple, but a bit long. Basically, it is composed by two files, main program: arduino_com.ino and the IMU functions: gy_521_send_serial.ino (Code by "Krodal"), this includes a low pass filter for the orientation. It works very well, without any fiffo problems but there is lot of drift on the yaw angle. Which is normal for these gyro+acc sensors.

About the main code:

void loop() {
  unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= interval) {
      previousMillis = currentMillis;

      readAngles();          
      recvWithStartEndMarkers();
      newData = false;
      moveServos(pulse0, pulse1, pulse2, pulse3, pulse4, pulse5, pulse6, pulse7, pulse8, pulse9, pulse10, pulse11);
  }
}

 Is basically what i exaplined, it only works when the interval time has passed and then it reads the angles from IMU, read the incoming pulses via serial command, switch newData flag off and write the new pulses command.

The pulses are given on microseconds, the translation of angles to microseconds is done in the python code.

recvWithStartEndMarkers().

How commands are read and then converted to an integer is made with this function, which is from: Serial Input Basics - updated.

Here there is the example i used:

// Example 3 - Receive with start- and end-markers

const byte numChars = 32;
char receivedChars[numChars];

boolean newData = false;

void setup() {
    Serial.begin(9600);
    Serial.println("<Arduino is ready>");
}

void loop() {
    recvWithStartEndMarkers();
    showNewData();
}

void recvWithStartEndMarkers() {
    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) {
                receivedChars[ndx] = rc;
                ndx++;
                if (ndx >= numChars) {
                    ndx = numChars - 1;
                }
            }
            else {
                receivedChars[ndx] = '\0'; // terminate the string
                recvInProgress = false;
                ndx = 0;
                newData = true;
            }
        }

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

 What i have done is to add an spacer ('#') to differentiate each pulse and a counter, that is in charge of saving the numeric string to its correspoding integer pulse.