Garage door manager

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Garage Door management
// GRA
// 02.12.2018
// V1.0 
// Connect to MQTT server and subscribe to topic
// Paramtres de programmations : ESP32 DEV module, QIO, 4MB, Dfaut, 80MHz, Disabled, 921600 Bauds
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "WiFi.h"
#include <PubSubClient.h>
//#include "esp_system.h"
#include "Wire.h"
//#include <DHT.h>
#include <pthread.h>
//#include "Adafruit_HTU21DF.h"

#define OPEN_D1 18
#define CLOSE_D1 19
#define OPEN_D2 5
#define CLOSE_D2 17
#define REL_D1 16
#define REL_D2 10
#define BUTTON_PIN 0
#define ROTAT_D11 32
#define ROTAT_D12 33
#define ROTAT_D21 35
#define ROTAT_D22 34
#define US_D1_TRIG 27
#define US_D1_ECHO 26
#define US_D2_TRIG 25
#define US_D2_ECHO 14
#define MASQ_D1 0b1100
#define MASQ_D2 0b0011
#define LEDJ 2
//#define DHTPIN  22
//#define DHTTYPE DHT22     // DHT22 (AM2302)
//#define ENCODERMAX 100    // Valeur max pour 100% ouvert
#define OUVERTURE 0b01010101
#define FERMETURE 0b00110011
#define NBMESENCODER 500000

//DHT dht(DHTPIN, DHTTYPE);
Adafruit_HTU21DF htu = Adafruit_HTU21DF();

// Ponthaux
const char* ssid = "YourSSID";                                   
const char* password = "YourPwd";
const char* server = "YourMQTTserverIP";

const char* topicopenclosed1 = "garage/openclose-d1";
const char* topicopenclosed2 = "garage/openclose-d2";
const char* topicgarage = "garage/commande";
const char* topiccar1 = "garage/car1";
const char* topiccar2 = "garage/car2";

String swVersion = "V1.0";
String hwVersion = "V1.0";
String payload = "";
String sensD1 = "";
String sensD2 = "";
String msgReceived = "";
String statusD1 = "";
String statusD2 = "";

unsigned long sleepTimeS = 5;     //300 = 5mn, 600 = 10mn, 1800 = 30mn, 3600 = 1h

bool readTempHumid = false;

char message_buff[1];   // initialise storage buffer (i haven't tested to this capacity.)
byte magnetSensors = 0x0;       // Etat des capteurs magntiques des portes ouvert/ferm
byte rotaryEncoderD1 = 0;
byte rotaryEncoderD2 = 0;
//byte buttonPin = 0;          // Etat du bouton pour checker son tat
byte dirD1 = 0;              //01010101 = fermeture, 00110011 = ouverture
byte dirD2 = 0;              //01010101 = fermeture, 00110011 = ouverture
//int interruptionD1 = 0;    // Si une interruption dtecte, ++ cette variable
//int interruptionD2 = 0;    // Si une interruption dtecte, ++ cette variable
//int interruption = 0;
long incrementD1 = 0;
//long oldincrementD1 = 0;
long incrementD2 = 0;
//long oldincrementD2 = 0;
long pourcentage = 0;
long lastReconnectAttempt = 0;
long durationD1, distanceD1;      // Mesure D1 US
long durationD2, distanceD2;      // Mesure D2 US
long distance = 0;
long nbMesTempHumid = 15000000;
int car1 = 0;                     // voiture prsente ou non dans le garage
int car2 = 0;

void callback(char* topic, byte* payload, unsigned int length);
void startInterrupt(void);
void stopInterrupt(void);

WiFiClient wifiClient;
PubSubClient client(server, 1883, callback, wifiClient);

///////////////////////////////////////////////////////////////
// Convert Mac adress to string
///////////////////////////////////////////////////////////////
String macToStr(const uint8_t* mac)
{
  String result;
  for (int i = 0; i < 6; ++i) {
    result += String(mac[i], 16);
    if (i < 5)
      result += ':';
  }
  return result;
}

///////////////////////////////////////////////////////////////
// Connect to wifi
///////////////////////////////////////////////////////////////
void connectwifi()
{
  int cnt = 0;
  int cnt2 = 0;
  Serial.print("Connecting to ");
  Serial.print(ssid);
  WiFi.mode(WIFI_STA);          // 26.05.2018 Mettre en station et non AP pour ne pas tre piratable
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED)
  {
    digitalWrite(LEDJ,HIGH);
    delay(100);
    digitalWrite(LEDJ,LOW);
    Serial.print(".");
    cnt += 1;
    if(cnt>=1000)
    {
      Serial.println("connectwifi() : cnt>=300");
      if(cnt2 == 10)
      {
        ESP.restart();
      }
      delay(1000);
      return;
    }
  }
  
  Serial.println("WiFi connected");  
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
  // Generate client name based on MAC address
  String clientName;
  clientName += "ESP32_";
  uint8_t mac[6];
  WiFi.macAddress(mac);
  clientName += macToStr(mac);

  Serial.print("Connecting to ");
  Serial.print(server);
  Serial.print(" as ");
  Serial.println(clientName);

  if (client.connect((char*) clientName.c_str()))
  {
    Serial.println("Connected to MQTT broker");
    Serial.print("Topic is: ");
    Serial.println(topicgarage);   
    char* topicTest = "test/test-connexion";
    if (client.publish(topicTest, "Starting..."))
    {
      Serial.println("Publish successfully sent");
      client.subscribe(topicgarage);
    }
    else
    {
      Serial.println("Publish failed");
    }
  }
  else
  {
    digitalWrite(LEDJ,HIGH);
    delay(100);
    Serial.println("MQTT connect failed");
    Serial.println("Will reset and try again...");
    ESP.restart();
  }   
}

///////////////////////////////////////////////////////////////
// Callback, quand arrive un message
///////////////////////////////////////////////////////////////

void callback(char* topicCallback, byte* payload, unsigned int length)
{
  // Here is what i have been using to handle subscriptions. I took it as a snippet from elsewhere but i cannot credit author as i dont have reference!
  int i = 0;
  
  Serial.print("Comparaison de topics : ");
  Serial.print(topicgarage);
  Serial.print(" et ");
  Serial.println(topicCallback);
  if (String(topicgarage) == String(topicCallback))
  {
    Serial.println("Message arrived,  topic is : " + String(topicCallback));
   
    // create character buffer with ending null terminator (string)
    for(i=0; i<length; i++)
    {
      message_buff[i] = payload[i];
      Serial.print(message_buff[i]);
      delay(20);
    }
    message_buff[i] = '\0';
   
    String msgString = String(message_buff);
   
    Serial.println("Payload: " + msgString);

    msgReceived = msgString;

    OpenCloseDoors();
  }
  else
  {
    Serial.println("Another topic, not for me");
  }
  //startInterrupt();
}
///////////////////////////////////////////////////////////////
// Reconnect to MQTT client
///////////////////////////////////////////////////////////////
boolean reconnect()
{
  Serial.println("Connection loosen, try to reconnect");
  ESP.restart();
  if (client.connect("ESPClient"))
  {
    Serial.println("reconnect() :  if (client.connect(ESPClient)");
    // Once connected, publish an announcement...
    client.publish(topicopenclosed1,"Booting doors system, please wait...");
    client.publish(topicopenclosed2,"Booting doors system, please wait...");
    // ... and resubscribe
    client.subscribe(topicgarage);
  }
  else
  {
    Serial.println("reconnect() : not connected");
  }
  delay(500);
  return client.connected();
}

///////////////////////////////////////////////////////////////
// rotaryInterrupt
///////////////////////////////////////////////////////////////
/*void readRotaryEncoders(byte door)
{
  incrementD1 = 0;
  incrementD2 = 0;
  long j=0;
  for(j=0;j <= NBMESENCODER;j++)
  {
    //delay(10);
    if(door==1)
    {
      //D1
      rotaryEncoderD1 = digitalRead(ROTAT_D11);
      rotaryEncoderD1 = (rotaryEncoderD1 << 1) + digitalRead(ROTAT_D12);
      //Serial.println(rotaryEncoderD1);
      
      switch(rotaryEncoderD1)
      {
        case 0:
        {
          if(rotaryEncoderD1 == 0b00)
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD1 == 0b01) // fermeture
          {
            dirD1 = FERMETURE;
            incrementD1++;
          }
          else if(rotaryEncoderD1 == 0b11) // ouverture
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD1 == 0b10)
          {
            dirD1 = OUVERTURE;
            incrementD1--;
          }
        }
        case 1:
        {
          if(rotaryEncoderD1 == 0b00)
          {
            dirD1 = FERMETURE;
            incrementD1++;
          }
          else if(rotaryEncoderD1 == 0b01) // fermeture
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD1 == 0b11) // ouverture
          {
            dirD1 = OUVERTURE;
            incrementD1--;
          }
          else if(rotaryEncoderD1 == 0b10)
          {
            // pas de mouvement
          }
        }
        case 2:
        {
          if(rotaryEncoderD1 == 0b00)
          {
            dirD1 = OUVERTURE; // fermeture
            incrementD1--;
          }
          else if(rotaryEncoderD1 == 0b01)
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD1 == 0b11)
          {
            dirD1 = FERMETURE; // ouverture
            incrementD1++;
          }
          else if(rotaryEncoderD1 == 0b10)
          {
            // pas de mouvement
          }
        }
        case 3:
        {
          if(rotaryEncoderD1 == 0b00)
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD1 == 0b01) // fermeture
          {
            dirD1 = FERMETURE;
            incrementD1++;
          }
          else if(rotaryEncoderD1 == 0b11) // ouverture
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD1 == 0b10)
          {
            dirD1 = OUVERTURE;
            incrementD1--;
          }
        }
      }
    }
    else if (door == 2)
    {
      // D2   
      rotaryEncoderD2 = digitalRead(ROTAT_D21);
      rotaryEncoderD2 = (rotaryEncoderD2 << 1) + digitalRead(ROTAT_D22);
      //Serial.println(rotaryEncoderD2);
      
      switch(rotaryEncoderD2)
      {
        case 0:
        {
          if(rotaryEncoderD2 == 0b00)
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD2 == 0b01) // fermeture
          {
            dirD2 = FERMETURE;
            incrementD2++;
          }
          else if(rotaryEncoderD2 == 0b11) // ouverture
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD2 == 0b10)
          {
            dirD2 = OUVERTURE;
            incrementD2--;
          }
        }
        case 1:
        {
          if(rotaryEncoderD2 == 0b00)
          {
            dirD2 = FERMETURE;
            incrementD2++;
          }
          else if(rotaryEncoderD2 == 0b01) // fermeture
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD2 == 0b11) // ouverture
          {
            dirD2 = OUVERTURE;
            incrementD2--;
          }
          else if(rotaryEncoderD2 == 0b10)
          {
            // pas de mouvement
          }
        }
        case 2:
        {
          if(rotaryEncoderD2 == 0b00)
          {
            dirD2 = OUVERTURE; // fermeture
            incrementD2--;
          }
          else if(rotaryEncoderD2 == 0b01)
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD2 == 0b11)
          {
            dirD2 = FERMETURE; // ouverture
            incrementD2++;
          }
          else if(rotaryEncoderD2 == 0b10)
          {
            // pas de mouvement
          }
        }
        case 3:
        {
          if(rotaryEncoderD2 == 0b00)
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD2 == 0b01) // fermeture
          {
            dirD2 = FERMETURE;
            incrementD2++;
          }
          else if(rotaryEncoderD2 == 0b11) // ouverture
          {
            // pas de mouvement
          }
          else if(rotaryEncoderD2 == 0b10)
          {
            dirD2 = OUVERTURE;
            incrementD2--;
          }
        }
      }
    }
  }
  Serial.print("incrementD1 = ");
  Serial.println(incrementD1);
  Serial.print("incrementD2 = ");
  Serial.println(incrementD2);  
  if(door == 1)
  {
    if(incrementD1 >= 15000)
    //if(dirD1==OUVERTURE)
    {
      client.publish(topicopenclosed1, "D1 ouverture...");
      Serial.println("D1 ouverture");
    }
    else if(incrementD1 <= -15000)
    //else if(dirD1==FERMETURE)
    {
      client.publish(topicopenclosed1, "D1 fermeture...");
      Serial.println("D1 fermeture");
    }
  }
  
  if(door == 2)
  {
    if(incrementD2 >= 15000)
    //if(dirD2==OUVERTURE)
    {
      client.publish(topicopenclosed2, "D2 ouverture...");
      Serial.println("D2 ouverture");
    }
    else if(incrementD2 <= -15000)
    //else if(dirD2==FERMETURE)
    {
      client.publish(topicopenclosed2, "D2 fermeture...");
      Serial.println("D2 fermeture");
    }
  }
  dirD1 = 0;
  dirD2 = 0;
  //delay(200);
}*/
///////////////////////////////////////////////////////////////
// readTempHumidPressure, lecture de la temprature et humidit avec 280
///////////////////////////////////////////////////////////////
void readTempHumidPressure(void)
{
  double temperature = 0;
  double humidity = 0;

  temperature = htu.readTemperature();
  humidity = htu.readHumidity();
  //temperature = dht.readTemperature();
  //humidity = dht.readHumidity();
  Serial.print("Temperature : ");
  Serial.print(temperature);
  Serial.print(", Humidity : ");
  Serial.println(humidity);
  
  client.publish("temp-humid/garage-humid", String(humidity).c_str());
  client.publish("temp-humid/garage-temp", String(temperature).c_str());
}

///////////////////////////////////////////////////////////////
// readUSsensors
///////////////////////////////////////////////////////////////
int readUSsensors(int cars)
{
  int distD1 = 0;
  int distD2 = 0;
  if(cars == 1)
  {
    //D1
    digitalWrite(US_D1_TRIG, LOW);  
    delayMicroseconds(2); 
    
    digitalWrite(US_D1_TRIG, HIGH);
    delayMicroseconds(10); 
    
    digitalWrite(US_D1_TRIG, LOW);
    durationD1 = pulseIn(US_D1_ECHO, HIGH);
    distD1 = (durationD1/2) / 29.1;
    /*Serial.print("Temps D1 : "); 
    Serial.print(durationD1);
    Serial.print(", Centimeter D1 : ");
    Serial.println(distD1);*/
    return distD1;
  }
  else if(cars == 2)
  {
    //D2
    digitalWrite(US_D2_TRIG, LOW);  
    delayMicroseconds(2); 
    
    digitalWrite(US_D2_TRIG, HIGH);
    delayMicroseconds(10); 
    
    digitalWrite(US_D2_TRIG, LOW);
    durationD2 = pulseIn(US_D2_ECHO, HIGH);
    distD2 = (durationD2/2) / 29.1;
    /*Serial.print("Temps D2 : "); 
    Serial.print(durationD2);
    Serial.print(", Centimeter D2 : ");
    Serial.println(distD2);*/
    return distD2;
  }
}

///////////////////////////////////////////////////////////////
// readMagnetSensors
///////////////////////////////////////////////////////////////
byte readMagnetSensors(void)
{
  //Serial.print("readMagnetSensors : ");
  byte magnetSensor = 0;
  magnetSensor = 0;
  magnetSensor = magnetSensor + digitalRead(OPEN_D1);
  magnetSensor = magnetSensor  << 1;
  magnetSensor = magnetSensor + digitalRead(CLOSE_D1);
  magnetSensor = magnetSensor  << 1;
  magnetSensor = magnetSensor + digitalRead(OPEN_D2);
  magnetSensor = magnetSensor << 1;
  magnetSensor = magnetSensor + digitalRead(CLOSE_D2);
  //Serial.println(magnetSensor);
  return magnetSensor;
}

///////////////////////////////////////////////////////////////
// Interruptions
///////////////////////////////////////////////////////////////
/*void interruptD1(void)
{
  stopInterrupt();
  if(digitalRead(ROTAT_D12) == LOW)
  {
    sensD1 = "fermeture";
  }
  else
  {
    sensD1 = "ouverture";
  }
}

void interruptD2(void)
{
  stopInterrupt();
  if(digitalRead(ROTAT_D22) == LOW)
  {
    sensD2 = "fermeture";
  }
  else
  {
    sensD2 = "ouverture";
  }
}

void startInterrupt(void)
{
  attachInterrupt(ROTAT_D11, interruptD1, FALLING);
  attachInterrupt(ROTAT_D21, interruptD2, FALLING);
}

void stopInterrupt(void)
{
  detachInterrupt(ROTAT_D11);
  detachInterrupt(ROTAT_D21);
}*/
///////////////////////////////////////////////////////////////
// OpenCloseDoors
///////////////////////////////////////////////////////////////
void OpenCloseDoors(void)
{
  int i = 0;
  if(msgReceived == "openDoor1")
  {
    magnetSensors = readMagnetSensors();
    if((magnetSensors & 12) == 8)
    {
      Serial.println("D1 dj ouvert");
    }
    else
    {
      Serial.println("Action sur la porte D1, puis attente...");
      opencloseD1();
      for(i=0;i<=100;i++)
      {
        Serial.println(i);
        magnetSensors = readMagnetSensors();
        if((magnetSensors & 12) == 4)              // Ferm copmplet, relancer l'ouverture
        {
          i = 100;
          Serial.println("D1 ferm, ouverture en cours dans 0.5s...");
          delay(500);
          opencloseD1();
        }
        delay(500);
      }
      msgReceived = "";
    }
  }
  else if(msgReceived == "closeDoor1")
  {
    if((magnetSensors & 12) == 4)
    {
      Serial.println("D1 dj ferm");
    }
    else
    {
      Serial.println("Fermeture de D1...");
      opencloseD1();
      for(i=0;i<=100;i++)
      {
        Serial.println(i);
        magnetSensors = readMagnetSensors();
        if((magnetSensors & 12) == 8)              // Ouvert copmplet, relancer la fermeture
        {
          break;
          Serial.println("D1 ouvert, fermeture en cours dans 0.5s...");
          delay(500);
          opencloseD1();
        }
        delay(500);
      }
      msgReceived = "";
    }
  }
  else if(msgReceived == "openDoor2")
  {
    if((magnetSensors & 3) == 1)
    {
      Serial.println("D2 dj ouvert");
    }
    else
    {
      Serial.println("Ouverture de D2...");
      opencloseD2();
      for(i=0;i<=100;i++)
      {
        Serial.println(i);
        //break;
        magnetSensors = readMagnetSensors();
        if((magnetSensors & 3) == 2)              // Ferm copmplet, relancer l'ouverture
        {
          break;
          Serial.println("D2 ferm, ouverture en cours dans 0.5s...");
          delay(500);
          opencloseD2();
        }
        delay(500);
      }
      msgReceived = "";
    }
  }
  else if(msgReceived == "closeDoor2")
  {
    if((magnetSensors & 3) == 2)
    {
      Serial.println("D2 dj ferm");
    }
    else
    {
      Serial.println("Fermeture de D2...");
      opencloseD2();
      for(i=0;i<=100;i++)
      {
        Serial.println(i);
        magnetSensors = readMagnetSensors();
        if((magnetSensors & 3) == 1)              // Ferm copmplet, relancer l'ouverture
        {
          break;
          Serial.println("D2 ouvert, fermeture en cours dans 0.5s...");
          delay(500);
          opencloseD2();
        }
        delay(500);
      }
      msgReceived = "";
    }
  }
  else
  {
    
  }
}

void opencloseD1(void)
{
  digitalWrite(REL_D1,HIGH);
  delay(200);
  digitalWrite(REL_D1,LOW);
  delay(1000);
}
void opencloseD2(void)
{
  digitalWrite(REL_D2,HIGH);
  delay(200);
  digitalWrite(REL_D2,LOW);
  delay(1000);
}

///////////////////////////////////////////////////////////////
// Setup
///////////////////////////////////////////////////////////////
void setup()
{
  pinMode(OPEN_D1, INPUT);
  pinMode(CLOSE_D1, INPUT);
  pinMode(OPEN_D2, INPUT);
  pinMode(CLOSE_D2, INPUT);
  pinMode(REL_D1, OUTPUT);
  pinMode(REL_D2, OUTPUT);
  pinMode(BUTTON_PIN, INPUT);
  pinMode(ROTAT_D11, INPUT);
  pinMode(ROTAT_D12, INPUT);
  pinMode(ROTAT_D21, INPUT);
  pinMode(ROTAT_D22, INPUT);
  pinMode(US_D1_TRIG, OUTPUT);
  pinMode(US_D1_ECHO, INPUT);
  pinMode(US_D2_TRIG, OUTPUT);
  pinMode(US_D2_ECHO, INPUT);
  pinMode(LEDJ,OUTPUT);

  //dht.begin();
  digitalWrite(LEDJ,HIGH);
  
  Serial.begin(115200);
  delay(100);
  Serial.println();
  // Version du soft et HW
  Serial.print("ESP32 - Garage Doors SW:");
  Serial.print(swVersion);
  Serial.print(", HW:");
  Serial.println(hwVersion);

  connectwifi();    // Connect wifi
  
  if (client.connected())   // Connect to broker
  {  
    if (client.publish(topicgarage, (char*) payload.c_str(),true))
    {   
      Serial.println("Publish ok");
      client.subscribe(topicgarage);
    }
    else
    {    
      Serial.println("Publish failed");
    }
  }
  else
  {
    Serial.println("Pas de connexion WIFI");
  }
  delay(100);
  if (!htu.begin())
  {
    Serial.println("Couldn't find sensor!");

  }
}

///////////////////////////////////////////////////////////////
// Loop
///////////////////////////////////////////////////////////////
void loop()
{
  digitalWrite(LEDJ,HIGH);
  if (!client.connected())
  {   
    digitalWrite(LEDJ,LOW);
    long now = millis();
    if (now - lastReconnectAttempt > 10000)
    {
      lastReconnectAttempt = now;
      Serial.print("Last Reconnect Attempt : ");
      Serial.println(lastReconnectAttempt);
      
      // Attempt to reconnect
      bool reconnectVar;
      reconnectVar = reconnect();
      if (reconnectVar = true)
      {
        digitalWrite(LEDJ,HIGH);
        Serial.println("Reconnected");
        lastReconnectAttempt = 0;
      }
      else
      {
        Serial.println("Reboot module");
        digitalWrite(LEDJ,LOW);
        ESP.restart();
      }
    }
  }

  client.loop();

  // Lecture des capteurs
  magnetSensors = readMagnetSensors();
  Serial.print("Etat capteurs D2: ");
  Serial.println(magnetSensors & 3);
    Serial.print("Etat capteurs D1: ");
  Serial.println(magnetSensors & 12);
  if((magnetSensors & 12) == 8)              // ouvert completement
  {
    if(statusD1!="D1 opened")
    {
      // Lecture de la temp humid pression
      Serial.println("D1 opened");
      client.publish(topicopenclosed1, "opened");
      readTempHumid = true;
      statusD1 = "D1 opened";
    }
  }
  else if ((magnetSensors & 12) == 4)      // ferm completment
  {
    if(statusD1!="D1 closed")
    {
      // Lecture de la temp humid pression
      Serial.println("D1 closed");
      client.publish(topicopenclosed1, "closed");
      readTempHumid = true;
      statusD1 = "D1 closed";
    }
  }
  else         // sinon, en mouvement, publier le sens en lisant les switch rotatifs
  {
    Serial.println("D1 en mouvement...");
    client.publish(topicopenclosed1, "moving");
    statusD1 = "D1 moving";
    //readRotaryEncoders(1);
  }
  
  //Door 2
  if ((magnetSensors & 3) == 1)         // ouvert completement
  {
    if(statusD2!="D2 opened")
    {
      // Lecture de la temp humid pression
      Serial.println("D2 opened");
      client.publish(topicopenclosed2, "opened");
      statusD2 = "D2 opened";
    }
  }
  else if ((magnetSensors & 3) == 2)      // ferm completment
  {
    //Serial.print("StatusD2 : ");
    //Serial.println(statusD2);
    if(statusD2!="D2 closed")
    {
      // Lecture de la temp humid pression
      Serial.println("D2 closed");
      client.publish(topicopenclosed2, "closed");
      statusD2 = "D2 closed";
    }
  }
  else         // sinon, en mouvement, publier le sens en lisant les switch rotatifs
  {
    //Serial.print("StatusD2 : ");
    //Serial.println(statusD2);
    Serial.println("D2 en mouvement...");
    client.publish(topicopenclosed2, "moving");
    statusD2 = "D2 moving";
    //readRotaryEncoders(2);
  }
  //Serial.print("StatusD2 : ");
  //Serial.println(statusD2);
  /*if(dirD1==OUVERTURE) sensD1="Ouverture";
  else if (dirD1==FERMETURE)sensD1="Fermeture";
  else sensD1="";
  
  if(dirD2==OUVERTURE) sensD2="Ouverture";
  else if (dirD2==FERMETURE)sensD2="Fermeture";
  else sensD2="";
    
  if(sensD1 != "" || sensD2 != "")
  {
    Serial.print("D1 = ");
    Serial.print(sensD1);
    Serial.print(", D2 = ");
    Serial.println(sensD2);

    if(sensD1 != "")
    {
      client.publish(topicopenclosed1, sensD1.c_str());
      Serial.println("Envoi MQTT de l'tat de D1");
    }
    if (sensD2 != "")
    {
      client.publish(topicopenclosed2, sensD2.c_str());
      Serial.println("Envoi MQTT de l'tat de D2");
    }
    sensD1 = "";
    sensD2 = "";
  }*/
  if(nbMesTempHumid >= 15000000)   // 15000000 Lire la temprature et humidit rgulirement (environ 15mn)
  {
    readTempHumid = true;
    nbMesTempHumid = 0;
    Serial.print("Distance sur D1 et D2... ");
    int i = 1;
    for(i=1;i<=2;i++)
    {
      distance = readUSsensors(i);
      Serial.println(distance);
      if(distance <= 5)
      {
        client.publish(topiccar1, "Distance read wrong");
      }
      else if(distance >= 100)               // Mesurer en cm, si plus que 1m, pas de voiture, sinon il y a une voiture
      {
        //Voiture pas l
        if(i==1) client.publish(topiccar1, "free");
        if(i==2) client.publish(topiccar2, "free");
      }
      else
      {
        //Voiture au garage
        if(i==1) client.publish(topiccar1, "occuped");
        if(i==2) client.publish(topiccar2, "occuped");
      }
      
      client.publish(topiccar1,String(distance).c_str());
    }
  }
  else
  {
    nbMesTempHumid++;
  }
  // Affichage de la temprature seulement dans certaines conditions
  if(readTempHumid == true)
  {
    readTempHumidPressure();
    readTempHumid = false;
  }

  delay(5000);

  if (WiFi.status() != WL_CONNECTED)
  {
    delay(100);
    connectwifi();
  }
}