I2C produces jumps in the time sample

[jgomez109]

I am trying to make a PID Controller for a Gimbal 2DOF (Here is the Gimbal I am using: https://i5.walmartimages.com/asr/0c3e25 ... nBg=ffffff) . I am meassuere the angle with a MPU6050 via I2C (Wire). To control the brushless motors I am Using the ESP32 who sents pwm signals to a L6234 Driver. The code uses a Hardware Timer Group to achive the time sample for the controller, however, when the motors start to work, the read I2C algorithm sometimes take a lot of time.

The time sample it is shown below in the first column, and in the second, the I2C read Time. When the motors are working, sometimes the I2C read is too long producing jumps in the time sample.

2700 1825
2710 1818
2720 1804
2730 1818
2740 1826
2750 1816
2760 1827
2770 1830
2780 1824
2790 1822
2800 1820
2810 1828
2820 1831
2830 1824
3830 1001713
3840 1822
3850 1817
3860 1832
3870 1829
3880 1824
3890 1833
3900 1819
3910 1829
4920 1001568
4930 1826
4940 1756
4950 1822
4960 1821
4970 1824
4980 1815
4990 1827
5000 1826

This do not happend when the motors are turned off.

Showing up next is the full code I am using. I have used Wire.setTimeout(), yield() but it keep being the same thing. (The switch case makes a sequence for the motors):

#include <driver/timer.h>
#include <SimpleKalmanFilter.h>
#include <Wire.h>

#define MPU 0x68
#define A_C 16384.0
#define frecuenciaPWM 20000 // Frecuencia de trabajo PWM
#define resolucionPWM 8 // Resolución del PWM
#define canalPWM0 0 // Canal 0 PWM
#define canalPWM1 1 // Canal 1 PWM
#define canalPWM2 2 // Canal 2 PWM
#define canalPWM3 3 // Canal 3 PWM
#define canalPWM4 4 // Canal 4 PWM
#define canalPWM5 5 // Canal 5 PWM
#define EN1_motor1 27 // Pin
#define EN2_motor1 13 // Pin
#define EN3_motor1 25 // Pin
#define IN1_motor1 14 // Pin
#define IN2_motor1 12 // Pin
#define IN3_motor1 26 // Pin
#define EN1_motor2 19 // Pin
#define EN2_motor2 23 // Pin
#define EN3_motor2 18 // Pin
#define IN1_motor2 15 // Pin
#define IN2_motor2 4 // Pin
#define IN3_motor2 5 // Pin
#define N_SIN 256 // cantidad de muestras de la señal senoidal
// VARIABLES
byte xDuty_a_motor1, xDuty_b_motor1, xDuty_c_motor1, xDuty_a_motor2, xDuty_b_motor2, xDuty_c_motor2;
unsigned long xTiempoInicialUS,xTiUS;
unsigned long xTiempoInicialMS,xTiMS;
unsigned long xTiempoUS,xTUS;
unsigned long xTiempoMS,xTMS;
unsigned long xTiempo;
SemaphoreHandle_t timerSemaphore1;
float xAngleAxf, xAngleAyf;
int suich1 = 0;
int xIndex1,xIndex2;
unsigned long xCountSec;
String m = "**";
unsigned int tm = 10; // time sample in ms
byte maxPWM1 = 153;// Max. PWM (DutyCycle) Motor 1
byte maxPWM2 = 153; // Max. PWM (DutyCycle) Motor 2

//--
int16_t rowAx, rowAy,rowAz;
float angleAx, angleAy;
float rowAgx, rowAgy,rowAgz;
int pwmSinMotor[256]; // Array donde se guardaran las muestras (# elementos = N_SIN)
uint16_t pwm_a_motor1, pwm_b_motor1, pwm_c_motor1, pwm_a_motor2, pwm_b_motor2, pwm_c_motor2;
uint16_t xPosStep1, xPosStep2; // variable para indexar el array pwmSinMotor

SimpleKalmanFilter filtroKalmanX(1.1,1.1,0.01);
SimpleKalmanFilter filtroKalmanY(1,1,0.01);

static intr_handle_t s_timer_handle1;

void IRAM_ATTR timer_tg1_isr(void* arg)
{
//Reset irq and set for next time
TIMERG1.int_clr_timers.t0 = 1;
TIMERG1.hw_timer[0].config.alarm_en = 1;

//----- HERE EVERY #uS -----
// uint64_t tiempoUS = esp_timer_get_time();
// uint64_t tiempoMS = tiempoUS/1000;
// Serial.println(int(tiempoMS - tiempoInicialMS));
// xFlagTimer = true;
xTiempoUS = esp_timer_get_time();
xTiempoMS = xTiempoUS/1000;
xTiempo = xTiempoMS - xTiempoInicialMS;
xSemaphoreGiveFromISR(timerSemaphore1, NULL);
}

void inicializarTimer(int timer_period_us)
{
timer_config_t config = {
.alarm_en = true, //Alarm Enable
.counter_en = false, //If the counter is enabled it will start incrementing / decrementing immediately after calling timer_init()
.intr_type = TIMER_INTR_LEVEL, //Is interrupt is triggered on timer’s alarm (timer_intr_mode_t)
.counter_dir = TIMER_COUNT_UP, //Does counter increment or decrement (timer_count_dir_t)
.auto_reload = true, //If counter should auto_reload a specific initial value on the timer’s alarm, or continue incrementing or decrementing.
.divider = 80 //Divisor of the incoming 80 MHz (12.5nS) APB_CLK clock. E.g. 80 = 1uS per timer tick
};

timer_init(TIMER_GROUP_1, TIMER_0, &config);
timer_set_counter_value(TIMER_GROUP_1, TIMER_0, 0);
timer_set_alarm_value(TIMER_GROUP_1, TIMER_0, timer_period_us);
timer_enable_intr(TIMER_GROUP_1, TIMER_0);
timer_isr_register(TIMER_GROUP_1, TIMER_0, &timer_tg1_isr, NULL, 0, &s_timer_handle1);
timer_start(TIMER_GROUP_1, TIMER_0);
timerSemaphore1 = xSemaphoreCreateBinary();
xTiempoInicialUS = esp_timer_get_time();
xTiempoInicialMS = xTiempoInicialUS/1000;
}



void setup_IMU(){
//Despertar MPU (Activar)
Wire.begin(21,22,400000); // A4=SDA / A5=SCL
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission();
//Definir Rango del Giroscopo
Wire.beginTransmission(MPU);
Wire.write(0x1B); //Pedir el registro 0x1B - corresponde al FS_SEL
Wire.write(0b00000000); //Escribir este valor al registro FS_SEL
Wire.endTransmission(); //Rango Giroscopo (°/s) FS_SEL
//0 +- 250 Sen 131
//1 +- 500 Sen 65.5
//2 +- 1000 Sen 32.8
//3 +- 2000 Sen 16.4
//Definir Rango del Acelerometro
Wire.beginTransmission(MPU);
Wire.write(0x1C); //Pedir el registro 0x1B - corresponde al FS_SEL
Wire.write(0b00000000); //Escribir este valor al registro FS_SEL
Wire.endTransmission(); // Dirección Rango Acelerómetro (g) AFS_SEL
// 0 +- 2 Sen 16384
// 1 +- 4 Sen 8192
// 2 +- 8 Sen 4096
// 3 +- 16 Sen 2048

}
static void leerIMU(){
//Leer los valores del Acelerometro
Wire.beginTransmission(MPU);
Wire.write(0x3B); //Pedir el registro 0x3B - corresponde al AcX
Wire.endTransmission(false);
Wire.requestFrom(MPU,6,true); //A partir del 0x3B, se piden 6 registros
while(Wire.available()){
rowAx = Wire.read()<<8|Wire.read(); //Cada valor ocupa 2 registros
rowAy = Wire.read()<<8|Wire.read();
rowAz = Wire.read()<<8|Wire.read();
//Convertir valores a g
rowAgx = (float)rowAx/A_C;
rowAgy = (float)rowAy/A_C;
rowAgz = (float)rowAz/A_C;
//Calcular Angulos
angleAx = -1*atan2((rowAgy),sqrt(pow((rowAgx),2) + pow((rowAgz),2)))*RAD_TO_DEG;
angleAy = -1*atan2(-1*(rowAgx),sqrt(pow((rowAgy),2) + pow((rowAgz),2)))*RAD_TO_DEG;
// //Calcular Angulos
// angleAx = -1*atan2((rowAy/A_C),sqrt(pow((rowAx/A_C),2) + pow((rowAz/A_C),2)))*RAD_TO_DEG;
// angleAy = -1*atan2(-1*(rowAx/A_C),sqrt(pow((rowAy/A_C),2) + pow((rowAz/A_C),2)))*RAD_TO_DEG;
//Filtrar Angulos
xAngleAxf = filtroKalmanX.updateEstimate(angleAx);
xAngleAyf = filtroKalmanY.updateEstimate(angleAy);
}

}
void setup_motores(){

for(int i=0; i<N_SIN; i++)// Array
{
pwmSinMotor = sin(2.0 * i / N_SIN * 3.14159265) * 127.0; // Calculo onda senoidal
}
ledcSetup(canalPWM0, frecuenciaPWM, resolucionPWM); //Asigno los parámetros PWM al canal 0 PWM
ledcSetup(canalPWM1, frecuenciaPWM, resolucionPWM); //Asigno los parámetros PWM al canal 1 PWM
ledcSetup(canalPWM2, frecuenciaPWM, resolucionPWM); //Asigno los parámetros PWM al canal 2 PWM
ledcSetup(canalPWM3, frecuenciaPWM, resolucionPWM); //Asigno los parámetros PWM al canal 3 PWM
ledcSetup(canalPWM4, frecuenciaPWM, resolucionPWM); //Asigno los parámetros PWM al canal 4 PWM
ledcSetup(canalPWM5, frecuenciaPWM, resolucionPWM); //Asigno los parámetros PWM al canal 5 PWM
ledcAttachPin(IN1_motor1, canalPWM0); //Asigno el canal 0 PWM al pin
ledcAttachPin(IN2_motor1, canalPWM1); //Asigno el canal 1 PWM al pin
ledcAttachPin(IN3_motor1, canalPWM2); //Asigno el canal 2 PWM al pin
ledcAttachPin(IN1_motor2, canalPWM3); //Asigno el canal 0 PWM al pin
ledcAttachPin(IN2_motor2, canalPWM4); //Asigno el canal 1 PWM al pin
ledcAttachPin(IN3_motor2, canalPWM5); //Asigno el canal 2 PWM al pin
pinMode(EN1_motor1,OUTPUT);
pinMode(EN2_motor1,OUTPUT);
pinMode(EN3_motor1,OUTPUT);
pinMode(EN1_motor2,OUTPUT);
pinMode(EN2_motor2,OUTPUT);
pinMode(EN3_motor2,OUTPUT);
ledcWrite(canalPWM0, 0); // Escribo el valor PWM para la fase A motor 1
ledcWrite(canalPWM1, 0); // Escribo el valor PWM para la fase B motor 1
ledcWrite(canalPWM2, 0); // Escribo el valor PWM para la fase C motor 1
ledcWrite(canalPWM3, 0); // Escribo el valor PWM para la fase A motor 2
ledcWrite(canalPWM4, 0); // Escribo el valor PWM para la fase B motor 2
ledcWrite(canalPWM5, 0); // Escribo el valor PWM para la fase C motor 2
digitalWrite(EN1_motor1, LOW);
digitalWrite(EN2_motor1, LOW);
digitalWrite(EN3_motor1, LOW);
digitalWrite(EN1_motor2, LOW);
digitalWrite(EN2_motor2, LOW);
digitalWrite(EN3_motor2, LOW);

}
void actualizarPWM(int xU1,int xU2, byte maxPWM1, byte maxPWM2){

digitalWrite(EN1_motor1, HIGH);
digitalWrite(EN2_motor1, HIGH);
digitalWrite(EN3_motor1, HIGH);
digitalWrite(EN1_motor2, HIGH);
digitalWrite(EN2_motor2, HIGH);
digitalWrite(EN3_motor2, HIGH);

xPosStep1 = xU1 & 0xff; // PosStep limitado a valores entre 0 y 255
pwm_a_motor1 = pwmSinMotor[(uint8_t)xPosStep1]; // Muestras PWM para la Fase A
pwm_b_motor1 = pwmSinMotor[(uint8_t)(xPosStep1 + 85)]; // Muestras PWM para la Fase B
pwm_c_motor1 = pwmSinMotor[(uint8_t)(xPosStep1 + 170)]; // Muestras PWM para la Fase C

// Factor & potencia
pwm_a_motor1 = maxPWM1 * pwm_a_motor1; // Multiplico por el máximo PWM
pwm_a_motor1 = pwm_a_motor1 >> 8; // Elimino los primero 8 bits
pwm_a_motor1 += 128; // Sumo 128

pwm_b_motor1 = maxPWM1 * pwm_b_motor1; // Multiplico por el máximo PWM
pwm_b_motor1 = pwm_b_motor1 >> 8; // Elimino los primero 8 bits
pwm_b_motor1 += 128; // Sumo 128

pwm_c_motor1 = maxPWM1 * pwm_c_motor1; // Multiplico por el máximo PWM
pwm_c_motor1 = pwm_c_motor1 >> 8; // Elimino los primero 8 bits
pwm_c_motor1 += 128; // Sumo 128

xDuty_a_motor1 = (uint8_t)pwm_a_motor1; // tomo los primero 8 bits
xDuty_b_motor1 = (uint8_t)pwm_b_motor1; // tomo los primero 8 bits
xDuty_c_motor1 = (uint8_t)pwm_c_motor1; // tomo los primero 8 bits


//Motor2
xPosStep2 = xU2 & 0xff; // PosStep limitado a valores entre 0 y 255
pwm_a_motor2 = pwmSinMotor[(uint8_t)xPosStep2]; // Muestras PWM para la Fase A
pwm_b_motor2 = pwmSinMotor[(uint8_t)(xPosStep2 + 85)]; // Muestras PWM para la Fase B
pwm_c_motor2 = pwmSinMotor[(uint8_t)(xPosStep2 + 170)]; // Muestras PWM para la Fase C

// Factor & potencia
pwm_a_motor2 = maxPWM2 * pwm_a_motor2; // Multiplico por el máximo PWM
pwm_a_motor2 = pwm_a_motor2 >> 8; // Elimino los primero 8 bits
pwm_a_motor2 += 128; // Sumo 128

pwm_b_motor2 = maxPWM2 * pwm_b_motor2; // Multiplico por el máximo PWM
pwm_b_motor2 = pwm_b_motor2 >> 8; // Elimino los primero 8 bits
pwm_b_motor2 += 128; // Sumo 128

pwm_c_motor2 = maxPWM2 * pwm_c_motor2; // Multiplico por el máximo PWM
pwm_c_motor2 = pwm_c_motor2 >> 8; // Elimino los primero 8 bits
pwm_c_motor2 += 128; // Sumo 128

xDuty_a_motor2 = (uint8_t)pwm_a_motor2; // tomo los primero 8 bits
xDuty_b_motor2 = (uint8_t)pwm_b_motor2; // tomo los primero 8 bits
xDuty_c_motor2 = (uint8_t)pwm_c_motor2; // tomo los primero 8 bits


ledcWrite(canalPWM3, xDuty_a_motor2); // Escribo el valor PWM para la fase A motor 2
ledcWrite(canalPWM4, xDuty_b_motor2); // Escribo el valor PWM para la fase B motor 2
ledcWrite(canalPWM5, xDuty_c_motor2); // Escribo el valor PWM para la fase C motor 2
ledcWrite(canalPWM0, xDuty_a_motor1); // Escribo el valor PWM para la fase A motor 1
ledcWrite(canalPWM1, xDuty_b_motor1); // Escribo el valor PWM para la fase B motor 1
ledcWrite(canalPWM2, xDuty_c_motor1); // Escribo el valor PWM para la fase C motor 1


}

void setup() {
Serial.begin(115200);
setup_motores();
setup_IMU();
inicializarTimer(tm*1000); //
}

void loop() {
unsigned long t1 = micros();
leerIMU();
unsigned long t2 = micros();
if (xSemaphoreTake(timerSemaphore1, 0) == pdTRUE){

//-- Secuencia MOTORES:

switch (suich1){

case 0:{ // ESPERA
const int tEspera = 2000; // Wait time in ms
xCountSec++ ;
xIndex1 = 0;
xIndex2 = 0;
if ( (xCountSec*tm) >= tEspera){
m = "Conte 2000";
xCountSec = 0;
suich1 = 1;
}

}break;

case 1:{ // Move MOTOR 1 CW

const int tMove1 = 3000; // Tiempo en segundos para un cico elec. del motor
xCountSec++;
xIndex1 = map((tm*xCountSec),tm,tMove1,0,256);
xIndex2 = 0;
if ( (tm*xCountSec) >= tMove1 ){ // ESPERA "tMove1" ms
m = "Conte 3000";
xCountSec = 0;
suich1 = 2;
}

}break;

case 2:{ // Wait

const int tEspera = 2000; // Wait in ms
xCountSec++;
if ( (xCountSec*tm) >= tEspera){
xCountSec = 0;
suich1 = 3;
}

}break;

case 3:{ // Move Motor 1 in CCW

const int tMove1 = 3000; //
xCountSec++;
xIndex1 = map((tm*xCountSec),tm,tMove1,256,0);
xIndex2 = 0;
if ( (tm*xCountSec) >= tMove1){ // ESPERA "tMove1" ms
xCountSec = 0;
xIndex1 = 0;
suich1 = 4;
}

}break;

case 4:{ // ESPERA

const int tEspera = 2000; // Tiempo de espera en ms
xCountSec++;
if ( (tm*xCountSec) >= tEspera){ // ESPERA "tEspera" ms
xCountSec = 0;
suich1 = 5;
}

}break;

case 5:{ // Accionar MOTOR DOS UP

const int tMove1 = 3000; // Tiempo en segundos para un cico elec. del motor
xCountSec++;
xIndex1 = 0;
xIndex2 = map((tm*xCountSec),tm,tMove1,0,256);
if ( (tm*xCountSec) >= tMove1 ){ // ESPERA "tMove1" ms
xCountSec = 0;
suich1 = 6;
}

}break;

case 6:{ // ESPERA

const int tEspera = 2000; // Tiempo de espera en ms
xCountSec++;
if ( (tm*xCountSec) >= tEspera){ // ESPERA "tEspera" ms
xCountSec = 0;
suich1 = 7;
}

}break;

case 7:{ // Accionar MOTOR DOS DOWN

const int tMove1 = 3000; // Tiempo en segundos para un cico elec. del motor
xCountSec++;
xIndex1 = 0;
xIndex2 = map((tm*xCountSec),tm,tMove1,256,0);
if ( (tm*xCountSec) >= tMove1){ // ESPERA "tMove1" ms
xCountSec = 0;
xIndex2 = 0;
suich1 = 8;
}

}break;

case 8:{ // ESPERA

const int tEspera = 2000; // Tiempo de espera en ms
xCountSec++;
if ( (tm*xCountSec) >= tEspera){ // ESPERA "tEspera" ms
xCountSec = 0;
suich1 = 9;
}

}break;

case 9:{ // Accionar MOTOR UNO y DOS UP

const int tMove1 = 3000; // Tiempo en segundos para un cico elec. del motor
xCountSec++;
xIndex1 = map((tm*xCountSec),tm,tMove1,0,256);
xIndex2 = map((tm*xCountSec),tm,tMove1,0,256);
if ( (tm*xCountSec) >= tMove1 ){ // ESPERA "tMove1" ms
xCountSec = 0;
suich1 = 10;
}

}break;

case 10:{ // ESPERA

const int tEspera = 2000; // Tiempo de espera en ms
xCountSec++;
if ( (tm*xCountSec) >= tEspera){ // ESPERA "tEspera" ms
xCountSec = 0;
suich1 = 11;
}

}break;

case 11:{ // Accionar MOTOR UNO y DOS DOWN

const int tMove1 = 3000; // Tiempo en segundos para un cico elec. del motor
xCountSec++;
xIndex1 = map((tm*xCountSec),tm,tMove1,256,0);
xIndex2 = map((tm*xCountSec),tm,tMove1,256,0);
if ( (tm*xCountSec) >= tMove1){ // ESPERA "tMove1" ms
xCountSec = 0;
xIndex1 = 0;
xIndex2 = 0;
suich1 = 12;
}

}break;

case 12:{ // ESPERA

const int tEspera = 5000; // Tiempo de espera en ms
xCountSec++;
if ( (tm*xCountSec) >= tEspera){ // ESPERA "tEspera" ms
xCountSec = 0;
suich1 = 1;
}

}break;

}



Serial.print(xTiempo);
Serial.print("\t");
Serial.println(t2-t1);


actualizarPWM(xIndex1 ,xIndex2, maxPWM1,maxPWM2); // actualizarPWM(u1,u2)
}