Problemas con libreria ProtoThread, Millis y visualizacón en pantalla OLED por I2C

[iotaa]

Buenas, estoy desarrollando un proyecto que trata sobre la medición de corriente AC, voltaje AC y potencia, estos valores se muestran en una pantala OLED y adicional tiene que hacer una lógica de control para activar secuencialmente las salidas de un módulo de 8 reles, para esto utilice la función MILLIS(); , tengo estos dos sistemas trabajando por separado y funcionan bien (el de medición y visualización de las variables, y el del encendido y apagado secuencial del módulo de reles), al momento de integrarlos me da conflicto, no me trabaja la visualización en la pantalla OLED y se para la medición de las variables, por lo cual decidí utilizar una librería llamada PROTOTHREAD, donde yo hago 2 hilos y en teoría los dos programas corren en paralelo en el arduino, cada protothread me funciona bien por separado, pero al crear el programa completo con los dos protothreads no me funciona, solo me funciona el primero que coloque dentro del VOID LOOP (&pt1), el segundo no se ejecuta (&pt2), si cambio de posición, es decir, si coloco primero el &pt2 y de segundo el &pt1, me ejecuta solo el &pt2.
Aquí está el código, agradezco quien me pueda ayudar a resolver el problema.
Saludos

#include <pt.h>


//Global Variables
const byte BUTTON=10; // our button pin
const byte BUTTON2=11; // our button pin
 
unsigned long encendidoMillis; // when button was released
unsigned long apagadoMillis; // when led was turned on
unsigned long Encendido_SALIDA1 = 1000; // wait to turn on LED
unsigned long Encendido_SALIDA2 = 2000; // wait to turn on LED
unsigned long Encendido_SALIDA3 = 3000; // wait to turn on LED
unsigned long Encendido_SALIDA4 = 4000; // wait to turn on LED
unsigned long Encendido_SALIDA5 = 5000; // wait to turn on LED
unsigned long Encendido_SALIDA6 = 6000; // wait to turn on LED
unsigned long Encendido_SALIDA7 = 7000; // wait to turn on LED
unsigned long Encendido_SALIDA8 = 8000; // wait to turn on LED

unsigned long Apagado_SALIDA1 = 1000; // turn off LED after this time
unsigned long Apagado_SALIDA2 = 2000; // turn off LED after this time
unsigned long Apagado_SALIDA3 = 3000; // turn off LED after this time
unsigned long Apagado_SALIDA4 = 4000; // turn off LED after this time
unsigned long Apagado_SALIDA5 = 5000; // turn off LED after this time
unsigned long Apagado_SALIDA6 = 6000; // turn off LED after this time
unsigned long Apagado_SALIDA7 = 7000; // turn off LED after this time
unsigned long Apagado_SALIDA8 = 8000; // turn off LED after this time

bool SecuenciaEncendido = false; // flag for when button is let go
bool SecuenciaApagado = false; // flag for when button is let go

//Control de salidas

int APAGADO = 1;
int ENCENDIDO = 0;

int SALIDA1 = 2; // declaramos la variable SALIDA1 y le asignamos el pin 2
int SALIDA2 = 3; // declaramos la variable SALIDA2 y le asignamos el pin 3
int SALIDA3 = 4; // declaramos la variable SALIDA3 y le asignamos el pin 4
int SALIDA4 = 5; // declaramos la variable SALIDA4 y le asignamos el pin 5
int SALIDA5 = 6; // declaramos la variable SALIDA5 y le asignamos el pin 6
int SALIDA6 = 7; // declaramos la variable SALIDA6 y le asignamos el pin 7
int SALIDA7 = 8; // declaramos la variable SALIDA7 y le asignamos el pin 8
int SALIDA8 = 9; // declaramos la variable SALIDA8 y le asignamos el pin 9



#include "EmonLib.h"

//Librerias para pantallas 
#include <Adafruit_SSD1306.h>
#include <splash.h>
#include <Adafruit_GFX.h>
#include <Adafruit_GrayOLED.h>
#include <Adafruit_SPITFT.h>
#include <Adafruit_SPITFT_Macros.h>
#include <gfxfont.h>
#include <Wire.h>


// Creando una instancia EnergyMonitor
EnergyMonitor energyMonitor1;
EnergyMonitor energyMonitor2;

// Voltaje de nuestra red eléctrica
float voltajeRed = 120.0;


// Variables para calculo de voltaje

int adc_max=570;        //Reemplazar por valor adc_max entregado por el sketch: volt_ac_cal
int adc_min=450;        //Reemplazar por valor adc_min entregado por el sketch: volt_ac_cal
float volt_multi=120;   //Reemplazar por el "voltaje ac rms" entregado por un multimetro
float volt_multi_p;
float volt_multi_n;

// Definiendo datos apra pantalla Oled
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET 12
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);





// Declare 3 protothreads
static struct pt pt1, pt2;


// First protothread function to blink LED 1 every 1 second
static int protothreadBlinkLED1(struct pt *pt)
{
  static unsigned long lastTimeBlink = 0;
  PT_BEGIN(pt);
  while(1) {
 // get the time at the start of this loop()
 unsigned long currentMillis = millis(); 
 
 // check the button 1
 if (digitalRead(BUTTON) == LOW) {
  // update the time when button was pushed
  encendidoMillis = currentMillis;
  SecuenciaEncendido = true;
 }

 // check the button 2
 if (digitalRead(BUTTON2) == LOW) {
  // update the time when button was pushed
  apagadoMillis = currentMillis;
  SecuenciaApagado = true;
 }
  
 // make sure this code isn't checked until after button has been let go
 if (SecuenciaEncendido) {
   
   //this is typical millis code here:
   if ((unsigned long)(currentMillis - encendidoMillis) >= Encendido_SALIDA1) {
     // okay, enough time has passed since the button was let go.
     digitalWrite(SALIDA1, ENCENDIDO);
     }
      
   if ((unsigned long)(currentMillis - encendidoMillis) >= Encendido_SALIDA2) {
     // okay, enough time has passed since the button was let go.
     digitalWrite(SALIDA2, ENCENDIDO);
     }

   if ((unsigned long)(currentMillis - encendidoMillis) >= Encendido_SALIDA3) {
     // okay, enough time has passed since the button was let go.
     digitalWrite(SALIDA3, ENCENDIDO);
     }
     
   if ((unsigned long)(currentMillis - encendidoMillis) >= Encendido_SALIDA4) {
     // okay, enough time has passed since the button was let go.
     digitalWrite(SALIDA4, ENCENDIDO);
     }
      
   if ((unsigned long)(currentMillis - encendidoMillis) >= Encendido_SALIDA5) {
     // okay, enough time has passed since the button was let go.
     digitalWrite(SALIDA5, ENCENDIDO);
     }

   if ((unsigned long)(currentMillis - encendidoMillis) >= Encendido_SALIDA6) {
     // okay, enough time has passed since the button was let go.
     digitalWrite(SALIDA6, ENCENDIDO);
     }

   if ((unsigned long)(currentMillis - encendidoMillis) >= Encendido_SALIDA7) {
     // okay, enough time has passed since the button was let go.
     digitalWrite(SALIDA7, ENCENDIDO);
     }

   if ((unsigned long)(currentMillis - encendidoMillis) >= Encendido_SALIDA8) {
     // okay, enough time has passed since the button was let go.
     digitalWrite(SALIDA8, ENCENDIDO);
       
     SecuenciaEncendido = false;
   }

   
 }
  
 // see if we are watching for the time to turn off LED



 if (SecuenciaApagado) {
   // okay, led on, check for now long
   if ((unsigned long)(currentMillis - apagadoMillis) >= Apagado_SALIDA1) {
     digitalWrite(SALIDA8, APAGADO);
     }
    
   if ((unsigned long)(currentMillis - apagadoMillis) >= Apagado_SALIDA2) {
     digitalWrite(SALIDA7, APAGADO);
     }

   if ((unsigned long)(currentMillis - apagadoMillis) >= Apagado_SALIDA3) {
     digitalWrite(SALIDA6, APAGADO);
     }

   if ((unsigned long)(currentMillis - apagadoMillis) >= Apagado_SALIDA4) {
     digitalWrite(SALIDA5, APAGADO);     
     }

   if ((unsigned long)(currentMillis - apagadoMillis) >= Apagado_SALIDA5) {
     digitalWrite(SALIDA4, APAGADO);
     }
    
   if ((unsigned long)(currentMillis - apagadoMillis) >= Apagado_SALIDA6) {
     digitalWrite(SALIDA3, APAGADO);
     }

   if ((unsigned long)(currentMillis - apagadoMillis) >= Apagado_SALIDA7) {
     digitalWrite(SALIDA2, APAGADO);
     }

   if ((unsigned long)(currentMillis - apagadoMillis) >= Apagado_SALIDA8) {
     digitalWrite(SALIDA1, APAGADO); 
 
     SecuenciaApagado = false;
   }
 
}
  }
  PT_END(pt);
}
// Second protothread function to blink LED 2 every 0.5 second
static int protothreadBlinkLED2(struct pt *pt)
{
 static unsigned long lastTimeBlink = 0;
  PT_BEGIN(pt);
  while(1) {

  // Obtenemos el valor de la corriente eficaz


  
  // Pasamos el número de muestras que queremos tomar
  double Irms1 = energyMonitor1.calcIrms(1484);
  double Irms2 = energyMonitor2.calcIrms(1484);

  // Calculamos la potencia aparente
  double potencia1 = Irms1 * voltajeRed;
  double potencia2 = Irms2 * voltajeRed;

  
  // Obtenemos los valores de calculo de voltaje 1 ///////////////////////
 
//  float volt_rmsV1=get_voltageV1(); //Voltage eficaz Linea 1 (V-RMS)
 
  float adc_sample;
  float volt_inst=0;
  float Sumatoria=0;
  float volt;
  long tiempo_init=millis();
  int N=0;

//// ****************************************** ////  

//// **********  Calculo de Voltaje en Linea 1  ***********////
  
  while( (millis() - tiempo_init) < 500)//Duración 0.5 segundos(Aprox. 30 ciclos de 60Hz)
  { 
    
    adc_sample = analogRead(A2);    //voltaje del sensor
    volt_inst = map(adc_sample,adc_min,adc_max,volt_multi_n,volt_multi_p);
    Sumatoria = Sumatoria+sq(volt_inst);    //Sumatoria de Cuadrados
    N = N+1;
    delay(1);
  }
  
  
  volt=sqrt((Sumatoria)/N); //ecuación del RMS

///********************************************************///

  // Obtenemos los valores de calculo de voltaje 2 ///////////////////////

 //  float volt_rmsV2=get_voltageV2(); //Voltage eficaz Linea 2 (V-RMS)


  float adc_sample2;
  float volt_inst2=0;
  float Sumatoria2=0;
  float volt2;
  long tiempo_init2=millis();
  int N2=0;

//// ****************************************** ////  

//// **********  Calculo de Voltaje en Linea 2  ***********////
    
  while( (millis() - tiempo_init2) < 500)//Duración 0.5 segundos(Aprox. 30 ciclos de 60Hz)
  { 
    
    adc_sample2 = analogRead(A3);    //voltaje del sensor
    volt_inst2 = map(adc_sample2,adc_min,adc_max,volt_multi_n,volt_multi_p);
    Sumatoria2 = Sumatoria+sq(volt_inst);    //Sumatoria de Cuadrados
    N2 = N2+1;
    delay(1);
  }
  
  
  volt2=sqrt((Sumatoria2)/N2); //ecuación del RMS

///********************************************************///  

  // Mostramos la información por el monitor serie
  Serial.print("Potencia1 = ");
  Serial.print(potencia1);
  Serial.print("    Irms1 = ");
  Serial.println(Irms1);
  Serial.print("Potencia2 = ");
  Serial.print(potencia2);
  Serial.print("    Irms2 = ");
  Serial.println(Irms2);


 // Mostrar valores en pantalla Oled
  oled.clearDisplay();
  oled.setTextColor(WHITE);
  oled.setTextSize(0);
  oled.setCursor(7,2); // (width=128, height=64)
  oled.print("IoT and Automation");
  oled.setTextSize(0);
  oled.setCursor(0, 16); // (width=128, height=64)
  oled.print("I1= ");
  oled.print(Irms1,2);
  oled.setTextSize(0);
  oled.setCursor(66, 16); // (width=128, height=64)
  oled.print("V1= ");
//  oled.print(volt_rmsV1,1);
  oled.print(volt,1);
  oled.setTextSize(0);
  oled.setCursor(0, 25); // (width=128, height=64)
  oled.print("P1= ");
  oled.print(potencia1,1);

  
  oled.setTextSize(0);
  oled.setCursor(0, 31); // (width=128, height=64)
  oled.print("---------------------");
  
  oled.setTextSize(0);
  oled.setCursor(0, 36); // (width=128, height=64)
  oled.print("I2= ");
  oled.print(Irms2,2);
  oled.setTextSize(0);
  oled.setCursor(66, 36); // (width=128, height=64)
  oled.print("V2= ");
//  oled.print(volt_rmsV2,1);
  oled.print(volt2,1);  
  oled.setTextSize(0);
  oled.setCursor(0, 45); // (width=128, height=64)
  oled.print("P2= ");
  oled.print(potencia2,1);
 

  oled.setTextSize(0);
  oled.setCursor(0, 51); // (width=128, height=64)
  oled.print("---------------------");

  oled.setTextSize(0);
  oled.setCursor(0, 57); // (width=128, height=64)
  oled.print("T1= 0.00");
  //oled.print(temp1,2);
  oled.setTextSize(0);
  oled.setCursor(66, 57); // (width=128, height=64)
  oled.print("T2= 0.00");
  //oled.print(temp1,2);

  oled.display(); 
  

 
  }
  PT_END(pt);
}

// In setup, set all LEDs as OUTPUT, push button as INPUT, and
// init all protothreads
void setup() {

  PT_INIT(&pt1);
  PT_INIT(&pt2);
 
 
 pinMode(BUTTON, INPUT_PULLUP);
 pinMode(BUTTON2, INPUT_PULLUP);
// pinMode(LED, OUTPUT);
// digitalWrite(LED, LOW);

//Configuracion de los pines
  pinMode(SALIDA1, OUTPUT);
  pinMode(SALIDA2, OUTPUT);
  pinMode(SALIDA3, OUTPUT);
  pinMode(SALIDA4, OUTPUT);
  pinMode(SALIDA5, OUTPUT);
  pinMode(SALIDA6, OUTPUT);
  pinMode(SALIDA7, OUTPUT);
  pinMode(SALIDA8, OUTPUT);

  

// Inicializando las salidas
digitalWrite(SALIDA1, APAGADO);
digitalWrite(SALIDA2, APAGADO);
digitalWrite(SALIDA3, APAGADO);
digitalWrite(SALIDA4, APAGADO);
digitalWrite(SALIDA5, APAGADO);
digitalWrite(SALIDA6, APAGADO);
digitalWrite(SALIDA7, APAGADO);
digitalWrite(SALIDA8, APAGADO);



  Serial.begin(115200);

  // Iniciamos la clase indicando
  // Número de pin: donde tenemos conectado el SCT-013
  // Valor de calibración: valor obtenido de la calibración teórica
  energyMonitor1.current(0, 75.1);
  energyMonitor2.current(1, 75.1);

 // Inicializando la pantalla Oled
  Wire.begin();
  oled.begin(SSD1306_SWITCHCAPVCC, 0x3C);  //0x3C
  

//  Inicializacion para calculo de voltaje
  volt_multi_p = volt_multi * 1.4142;   //Voltaje pico= Voltaje RMS * 1.4142 (Corriente Monofasica)
  volt_multi_n = volt_multi_p * -1; 



}
// In the loop we just need to call the protothreads one by one
void loop() {
  protothreadBlinkLED1(&pt1);
  protothreadBlinkLED2(&pt2);

 
}