diff --git a/software/platformio.ini b/software/platformio.ini
index 7337fff..ebc3f77 100644
--- a/software/platformio.ini
+++ b/software/platformio.ini
@@ -20,8 +20,8 @@ lib_deps =
 ;    ESP Async WebServer
 ;    AsyncTCP
     FS
-;    FastLED
-    https://github.com/samguyer/FastLED.git
+    FastLED
+;    https://github.com/samguyer/FastLED.git
 ;    AsyncMqttClient
     https://github.com/marvinroger/async-mqtt-client.git
 lib_ignore = ESPAsyncTCP
diff --git a/software/src/MyMQTT.cpp b/software/src/MyMQTT.cpp
index ae32106..e7c81b2 100644
--- a/software/src/MyMQTT.cpp
+++ b/software/src/MyMQTT.cpp
@@ -4,10 +4,12 @@
 
 #include "hardware.h"
 #include "shelf.h"
+#include "wifi.h"
 
 using namespace std;
 
 extern Shelf* shelf;
+extern Wifi wifi;
 
 MyMQTT::MyMQTT(void)
   : host {mqtt_server},
@@ -33,6 +35,26 @@ void MyMQTT::onMqttConnectWrapper(bool sessionPresent) {
     mqtt_ptr->onMqttConnect(sessionPresent);
 }
 
+void MyMQTT::onMqttDisconnectWrapper(AsyncMqttClientDisconnectReason reason) {
+//  static_cast<MyMQTT*>(parm)->onMqttConnect(NULL);
+    mqtt_ptr->onMqttDisconnect(reason);
+}
+
+void MyMQTT::reconnectTask(void* parameters) {
+    while (!wifi.connected()) {
+      Serial.print(".");
+      vTaskDelay(1000 / portTICK_PERIOD_MS);
+    }
+
+  Serial.println("MQTT reconnecting!");
+  this->connect();
+  vTaskDelete(NULL);
+}
+
+void MyMQTT::cTaskWrapper(void* parameters) {
+  static_cast<MyMQTT*>(parameters)->reconnectTask(NULL);
+}
+
 void MyMQTT::onMqttMessage(char* topic, char* payload, AsyncMqttClientMessageProperties properties, size_t len, size_t index, size_t total) {
   Serial.print("incoming: ");
   Serial.println(topic);
@@ -59,16 +81,30 @@ void MyMQTT::onMqttConnect(bool sessionPresent) {
   subscribe();
 }
 
+void MyMQTT::onMqttDisconnect(AsyncMqttClientDisconnectReason reason) {
+  Serial.println("Disconnected from MQTT.");
+
+    xTaskCreate(
+      &cTaskWrapper,          /* Task function. */
+      "mqttReconnectTask",        /* String with name of task. */
+      1024,            /* Stack size in words. */
+      this,             /* Parameter passed as input of the task */
+      1,                /* Priority of the task. */
+      &mqttTaskHandle);            /* Task handle. */
+}
+
 void MyMQTT::connect(void) {
   mqtt_ptr = this;
   Serial.print("MQTT connecting");
 
   mqttClient.onConnect(&onMqttConnectWrapper);
+  mqttClient.onDisconnect(&onMqttDisconnectWrapper);
   mqttClient.onSubscribe(&onMqttSubscribe);
   mqttClient.onMessage(&onMqttMessageWrapper);
 
   mqttClient.setServer(host, port);
   mqttClient.setCredentials(username, password);
+  mqttClient.setKeepAlive(60);
 //#if ASYNC_TCP_SSL_ENABLED
 //  mqttClient.setSecure(true);
 //#endif
@@ -84,6 +120,10 @@ void MyMQTT::connect(void) {
 
 }
 
+bool MyMQTT::connected(void) {
+  return mqttClient.connected();
+}
+
 void MyMQTT::subscribe(void) {
   uint16_t dispenseIdSub = mqttClient.subscribe("sensor/vendingmachine/dispense", 0);
   mqttClient.publish("sensor/vendingmachine/alive", 0, true, "test");
diff --git a/software/src/MyMQTT.h b/software/src/MyMQTT.h
index c98a2fd..203d722 100644
--- a/software/src/MyMQTT.h
+++ b/software/src/MyMQTT.h
@@ -10,12 +10,15 @@ class MyMQTT {
     MyMQTT(void);
     void connect(void);
     void subscribe(void);
+    bool connected(void);
     void publish(char* topic, char* payload);
     void publish(char* topic, String payload);
     void onMqttConnect(bool sessionPresent);
+    void onMqttDisconnect(AsyncMqttClientDisconnectReason reason);
     void onMqttMessage(char* topic, char* payload, AsyncMqttClientMessageProperties properties, size_t len, size_t index, size_t total);
 //    static void onMqttMessageWrapper(char* topic, char* payload, AsyncMqttClientMessageProperties properties, size_t len, size_t index, size_t total);
     static void onMqttConnectWrapper(bool sessionPresent);
+    static void onMqttDisconnectWrapper(AsyncMqttClientDisconnectReason reason);
   private:
     const char* host;
     const uint32_t port;
@@ -23,6 +26,9 @@ class MyMQTT {
     const char* username;
     const char* password;
     AsyncMqttClient mqttClient;
+    static void cTaskWrapper(void*);
+    void reconnectTask(void*);
+    TaskHandle_t mqttTaskHandle;
 //    static MyMQTT* mqtt_ptr;
 };
 
diff --git a/software/src/hardware.h b/software/src/hardware.h
index 4c95c7c..9d9a0c3 100644
--- a/software/src/hardware.h
+++ b/software/src/hardware.h
@@ -6,7 +6,9 @@
 
 #define LED_BUILTIN 13
 
-static constexpr int PROGMEM SERVO_PINS[] = {23, 22, 1, 3, 21, 19};
+static constexpr int PROGMEM SERVO_PINS[] = {23, 22, 21, 19, 18, 5};
+
+//UART: 1 and 3
 
 static constexpr uint8_t PROGMEM LED_PIN = 4;
 static constexpr uint8_t PROGMEM NUM_LEDS = 21;
@@ -16,7 +18,31 @@ static constexpr uint8_t PROGMEM NUM_LEDS = 21;
 static const char* PROGMEM mqtt_server = "172.16.75.18";
 static constexpr uint32_t PROGMEM mqtt_port = 1883;
 static constexpr bool PROGMEM MQTT_SECURE = true;
-static const char* PROGMEM mqtt_username = "esp100";
+static const char* PROGMEM mqtt_username = "vending";
 static const char* PROGMEM mqtt_password = "password";
 
 #endif /* _HARDWARE_H */
+
+/* ******** pin diagram (esp32-st) ********
+ *             __________
+ *        3v3 |          | GND -- GND
+ *        RST |          |  23 -- Servo0
+ *         36 |          |  22 -- Servo1
+ *         39 |          |  01 (UART_TX)
+ *         34 |          |  03 (UART_RX)
+ *         35 |          |  21 -- Servo2
+ *         32 | ESP32-ST | GND
+ *         33 |          |  19 -- Servo3
+ *         25 | Develop- |  18 -- Servo4
+ *         26 |   ment   |  05 -- Servo5
+ *         27 |  Board   |  17
+ *         14 |          |  16
+ *         12 |          |  04 -- TM1829_Data
+ *        GND |          |  00
+ *         13 |          |  02 (LED1)
+ *         09 |          |  15
+ *         10 |          |  08
+ *         11 |          |  07
+ *  Vin -- 5V |__________|  06
+ * 
+ ****************************************** */
diff --git a/software/src/led.cpp b/software/src/led.cpp
index d44e572..57d3453 100644
--- a/software/src/led.cpp
+++ b/software/src/led.cpp
@@ -1,4 +1,5 @@
-#define FASTLED_ALLOW_INTERRUPTS 0
+//#define FASTLED_ALLOW_INTERRUPTS 0
+//#define FASTLED_INTERRUPT_RETRY_COUNT 1
 #include <FastLED.h>
 
 #include "led.h"
diff --git a/software/src/led.h b/software/src/led.h
index 23d4d4d..56290b5 100644
--- a/software/src/led.h
+++ b/software/src/led.h
@@ -1,7 +1,8 @@
 #ifndef _LED_H
 #define _LED_H
 
-#define FASTLED_ALLOW_INTERRUPTS 0
+//#define FASTLED_ALLOW_INTERRUPTS 0
+//#define FASTLED_INTERRUPT_RETRY_COUNT 1
 #include <FastLED.h>
 
 #include "hardware.h"
diff --git a/software/src/main.cpp b/software/src/main.cpp
index 5b64777..f06cf45 100644
--- a/software/src/main.cpp
+++ b/software/src/main.cpp
@@ -15,6 +15,8 @@
 #include "led.h"
 #include "MyMQTT.h"
 
+#define LED_BUILTIN 2
+
 using namespace std;
 
 volatile SemaphoreHandle_t xPreferencesSemaphore = xSemaphoreCreateMutex();
@@ -43,23 +45,25 @@ void setup()
   led.setup();
   wifi.connect();
   mqtt.connect();
-  delay(1000);
+//  delay(1000);
   shelf = new Shelf(8);
   pusher.setup();
 //  webserver.start();
 }
 
 void loop()
-{/*
+{
+  Serial.print("Free Heap: ");
+  Serial.println(ESP.getFreeHeap());
   // turn the LED on (HIGH is the voltage level)
-  digitalWrite(LED_BUILTIN, HIGH);
+  if (mqtt.connected()) digitalWrite(LED_BUILTIN, HIGH);
   // wait for a second
   delay(1000);
   // turn the LED off by making the voltage LOW
   digitalWrite(LED_BUILTIN, LOW);
    // wait for a second
   delay(1000);
-*/
-delay(5000);
+
+//delay(5000);
 }
 
diff --git a/software/src/pusher.cpp b/software/src/pusher.cpp
index 07c7db9..b2231ee 100644
--- a/software/src/pusher.cpp
+++ b/software/src/pusher.cpp
@@ -14,13 +14,13 @@ Pusher::Pusher(void) {
 
 void Pusher::setup(void) {
   int channel = LEDC_TIMER_0;
+  ledcSetup(channel, 50, TIMER_WIDTH);
   for(int pin : SERVO_PINS) {
     if (xSemaphoreTake(xSemaphore, TIMEOUT) == pdTRUE) {
-      ledcSetup(channel, 50, TIMER_WIDTH);
-/*      ledcAttachPin(pin, channel);
+      ledcAttachPin(pin, channel);
       ledcWrite(channel, MAX_PULSE_WIDTH);
-      delay(200);
-      ledcDetachPin(pin);*/
+      delay(500);
+      ledcDetachPin(pin);
       xSemaphoreGive(xSemaphore);
       Serial.print("configured pin ");
       Serial.println(pin);
@@ -35,8 +35,7 @@ void Pusher::push(int num) {
     if (channel >= LEDC_NUM_CHANNELS) {
       return;
     }
-
-    ledcSetup(channel, 50, TIMER_WIDTH);
+//    ledcSetup(channel, 50, TIMER_WIDTH);
     ledcAttachPin(pin, channel);
     ledcWrite(channel, MIN_PULSE_WIDTH);
     delay(1000);
diff --git a/software/src/shelf.cpp b/software/src/shelf.cpp
index 5850f82..3034f85 100644
--- a/software/src/shelf.cpp
+++ b/software/src/shelf.cpp
@@ -25,7 +25,8 @@ void Shelf::reload(void) {
   }
 }
 
-void Shelf::dispense(int num) {
+void Shelf::dispenseTask(void* parameters) {
+  Serial.println("dispensing");
   item.at(num)->quantity--;
   led.changeAnimation(1, 0);
   pusher.push(num);
@@ -33,4 +34,15 @@ void Shelf::dispense(int num) {
   led.changeAnimation(2, 0);
   delay(7000);
   led.changeAnimation(0, 0);
+
+  vTaskDelete(NULL);
+}
+
+void Shelf::cTaskWrapper(void* parameters) {
+  static_cast<Shelf*>(parameters)->dispenseTask(NULL);
+}
+
+void Shelf::dispense(int num) {
+  this->num = num;
+  xTaskCreate(&cTaskWrapper, "dispenseTask", 1024, this, 0, &dispenseTaskHandle);
 }
diff --git a/software/src/shelf.h b/software/src/shelf.h
index dda7215..3bd5ae5 100644
--- a/software/src/shelf.h
+++ b/software/src/shelf.h
@@ -12,6 +12,10 @@ class Shelf {
     void reload(void);
     void dispense(int);
   private:
+    TaskHandle_t dispenseTaskHandle;
+    void dispenseTask(void*);
+    static void cTaskWrapper(void*);
+    int num;
 };
 
 #endif /* _SHELF_H */
diff --git a/software/src/wifi.cpp b/software/src/wifi.cpp
index b4ca61b..4f04cd9 100644
--- a/software/src/wifi.cpp
+++ b/software/src/wifi.cpp
@@ -14,13 +14,18 @@ void Wifi::reconnectTask(void* parameters) {
     if(wifiMulti.run() != WL_CONNECTED) {
         Serial.println("WiFi not connected!");
     }
-    vTaskDelay(3000 / portTICK_PERIOD_MS);
+    vTaskDelay(1000 / portTICK_PERIOD_MS);
   }
   Serial.println("WiFi disconnecting!");
   WiFi.disconnect();
   vTaskDelete(NULL);
 }
 
+bool Wifi::connected(void) {
+  if(wifiMulti.run() == WL_CONNECTED) return true;
+  else return false;
+}
+
 void Wifi::cTaskWrapper(void* parameters) {
   static_cast<Wifi*>(parameters)->reconnectTask(NULL);
 }
diff --git a/software/src/wifi.h b/software/src/wifi.h
index bf52f03..cd8f54e 100644
--- a/software/src/wifi.h
+++ b/software/src/wifi.h
@@ -9,6 +9,7 @@ class Wifi {
     Wifi();
     void connect();
     void disconnect();
+    bool connected();
   private:
     void reconnectTask(void*);
     static void cTaskWrapper(void*);