Workaround for sensor glitch, separated functions

Greenhouse.ino

@@ -112,7 +112,7 @@ TouchScreen ts = TouchScreen(XP, YP, XM, YM, 333);
 String outtext = "";
 String oldtext = "";
 boolean fastboot = true;
-boolean debug = false;
+boolean debug = true;
 
 //Adafruit_Image img;
 // Plant logo
@@ -845,7 +845,7 @@ void setup(void) {
     delay(250);
   }
   printConsoleText("[  OK  ] Enabling powersaving features...            ");  // 53 characters wide
-  enablePS(3);
+  enablePS(1);
   upd = true;
   
   if (!soil1.begin(0x36)) {
@@ -895,12 +895,41 @@ void loop(void) {
     printConsoleText(txtin.substring(0, txtin.length() - 1));
   }
   
+  
+  // change upd to true if screen update needed
+  if(!debug && upd) {
+    tft.fillScreen(BLACK);
+    tft.fillRect(0, 0, 320, 20, WHITE);
+    tft.setCursor(2, 2);
+    tft.setTextSize(2);
+    tft.setTextColor(BLACK);
+    tft.print("Water Level: ");
+    
+    
+    upd = false;
+  }
+  getMoisture();
+  delay(2000);
+}
+
+ISR (WDT_vect) 
+{
+   wdt_disable();  // disable watchdog
+}  // end of WDT_vect
+
+void getMoisture() {
   if (s1) {
     float t1 = soil1.getTemp();
     int sm1 = map(soil1.touchRead(0), 0, 1023, 0, 100);
     if(sm1 > 6000 && t1 < 1) {
-      s1 = false;
-      printConsoleText("[ INFO ] Soil sensor 1 unplugged.");
+      if (!soil1.begin(0x36)) {
+        s1 = false;
+        printConsoleText("[ INFO ] Soil sensor 1 unplugged.");
+      } else {
+        t1 = soil1.getTemp();
+        sm1 = map(soil1.touchRead(0), 0, 1023, 0, 100);
+        printS1(t1, sm1);
+      }
     } else {
       printS1(t1, sm1);
     }
@@ -918,8 +947,14 @@ void loop(void) {
     float t2 = soil2.getTemp();
     int sm2 = map(soil2.touchRead(0), 0, 1023, 0, 100);
     if(sm2 > 6000 && t2 < 1) {
-      s2 = false;
-      printConsoleText("[ INFO ] Soil sensor 2 unplugged.");
+      if (!soil2.begin(0x37)) {
+        s2 = false;
+        printConsoleText("[ INFO ] Soil sensor 2 unplugged.");
+      } else {
+        t2 = soil2.getTemp();
+        sm2 = map(soil2.touchRead(0), 0, 1023, 0, 100);
+        printS1(t2, sm2);
+      }
     } else {
       printS2(t2, sm2);
     }
@@ -929,7 +964,7 @@ void loop(void) {
       printConsoleText("[ INFO ] Soil sensor 2 connected.");
       float t2 = soil2.getTemp();
       int sm2 = map(soil2.touchRead(0), 0, 1023, 0, 100);
-      printS1(t2, sm2);
+      printS2(t2, sm2);
     }
   }
 
@@ -937,8 +972,14 @@ void loop(void) {
     float t3 = soil3.getTemp();
     int sm3 = map(soil3.touchRead(0), 0, 1023, 0, 100);
     if(sm3 > 6000 && t3 < 1) {
-      s3 = false;
-      printConsoleText("[ INFO ] Soil sensor 3 unplugged.");
+      if (!soil1.begin(0x38)) {
+        s3 = false;
+        printConsoleText("[ INFO ] Soil sensor 3 unplugged.");
+      } else {
+        t3 = soil3.getTemp();
+        sm3 = map(soil3.touchRead(0), 0, 1023, 0, 100);
+        printS3(t3, sm3);
+      }
     } else {
       printS3(t3, sm3);
     }
@@ -956,8 +997,14 @@ void loop(void) {
     float t4 = soil4.getTemp();
     int sm4 = map(soil4.touchRead(0), 0, 1023, 0, 100);
     if(sm4 > 6000 && t4 < 1) {
-      s4 = false;
-      printConsoleText("[ INFO ] Soil sensor 4 unplugged.");
+      if (!soil1.begin(0x39)) {
+        s4 = false;
+        printConsoleText("[ INFO ] Soil sensor 4 unplugged.");
+      } else {
+        t4 = soil4.getTemp();
+        sm4 = map(soil4.touchRead(0), 0, 1023, 0, 100);
+        printS4(t4, sm4);
+      }
     } else {
       printS4(t4, sm4);
     }
@@ -970,25 +1017,8 @@ void loop(void) {
       printS4(t4, sm4);
     }
   }
-  // change upd to true if screen update needed
-  if(!debug && upd) {
-    tft.fillScreen(BLACK);
-    tft.fillRect(0, 0, 320, 20, WHITE);
-    tft.setCursor(2, 2);
-    tft.setTextSize(2);
-    tft.setTextColor(BLACK);
-    tft.print("Water Level: ");
-    
-    
-    upd = false;
-  }
 }
 
-ISR (WDT_vect) 
-{
-   wdt_disable();  // disable watchdog
-}  // end of WDT_vect
-
 void printS1(float t, int sm) {
   String out = "[  OK  ] Soil sensor 1 temperature: ";
   out += t;