IoT Pull-Up Bar

#include <ADCTouch.h>
#include <NewPing.h>
#include <SoftwareSerial.h>
#include <stdlib.h>
#include <SPI.h>
#include <MFRC522.h>
#include <String.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <SerialCommand.h>
#include <EEPROM.h>

#define SERIALCOMMANDDEBUG

#define WiFiSSIDLen 15
#define WiFiPASSLen 15

//Debug levels- to show some info on the screen during development. Default level=DBGINFO
#define DBGNO       0
#define DBGERROR    1
#define DBGPROTOCOL 2
#define DBGINFO     3
#define DBGALL      4
#define DBGVERBOSE  5

byte    dbg=DBGINFO;

//Number of grips on the pullup bar
#define GripTotal    2

#define DATA_PIN                    2 //onnected to DS pin (data) of NXP 74HC595
#define LATCH_PIN                   3 //Connected to latch pin  of NXP 74HC595
#define CLOCK_PIN                   4 //onnected to SH_CP (clock) of NXP 74HC595
#define ECHO_PIN                    5 // Arduino pin conneted to echo pin on the ultrasonic sensor.
#define TRIGGER_PIN                 6 // Arduino pin connected to trigger pin on the ultrasonic sens
#define RX_WIFI_PIN                 7 // Arduino pin connected to RX pin of ESP8266
#define TX_WIFI_PIN                 8 // Arduino pin connected to TX pin of ESP8266
#define RFID_RST_PIN                9 // Connection to RST pin of cardreader
#define RFID_SDA_PIN                 10 // Connection to SS pin of cardreader
/*Some more digital pins are used:
SPI MOSI    D11        
SPI MISO    D12
SPI SCK     D13*/
#define LEFT_NORM_GRIP              A0 // Connection to left grip
#define RIGHT_NORM_GRIP             A1 // Connection to right grip
/*Some more analogue pins are used (if LED screen connected)
 * LCD SDA A4
 * LCD SLC A5
 */

//
#define WIFIERR_OK                  0
#define WIFIERR_NOTCPCONNECTION     1
#define WIFIERR_SENDERR             2
#define WIFIERR_SERVERERR           3
#define WIFIERR_UNKNOWN             4
/* Cardreader connection
 *  cardreader side     Arduino side
          -------------
   SDA  -|            |- Pin D10
   SCK  -|            |- Pin D13
   MOSI -|            |- Pin D11
   MISO -|            |- Pin D12
   IRQ  -|            |-
   GND  -|            |- GND
   RST  -|            |- Pin D9
   3.3V -|            |- 3.3V 
          -------------

   ESP 8266 connection
   ----------------------------
   |                          |
   | D7   GND       --------  |
   | 3.3V  *        |         |
   |  *    *        --------  |
   | 3.3V  D8              |  |
   |                --------  |
   ----------------------------

   Shiftout IC connection (NXP 74HC595)
          ---------------
    2 bit |      U      | 5V
    3 bit |  N   X   P  | 1 bit
    4 bit |             | Data PIN D2
    5 bit |             | GND
    6 bit |             | Latch PIN D3 
    7 bit |             | Clock PIN D4
    8 bit |             | 5V
      GND |      O      |
          ---------------
*/

//Ultrasonic variables
#define MAX_PULLUP_DIST 30    //Maximal distance between user head and sonar to complete pullup
#define MIN_PULLUP_DIST 2     //Minimal distance between user head and sonar to complete pullup
#define MAX_DISTANCE 100      // Maximum distance for ultrasonic sensor

//Capacitive sensor variables
#define CapacityTouchLevel 50     //"Base" level for sensor capacity
#define CapAverageBase 100        //This variable used to average capacitor delay time

//Variable to exclude too fast pullups- it's may be some errors (it's impossible to make a pullup faster then some milliseconds). Variable in milliseconds- 0,5 sec default
#define MIN_PULLUP_TIME 500

//LED numbers
#define SPK_LED         2    //Speaker
#define WIFI_ERR_LED    3    //WiFi disconnected
#define WIFI_OK_LED     5    //WiFi connected
#define TOUCH_LED       6    //Grips touched
#define RFID_LED        7    //RFID card has been readed
#define PULLUP_LED      8    //Blink on each completed pullup


NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

SerialCommand     serialCommand;                    //New object to manage commands between computer and Arduino device

LiquidCrystal_I2C lcd(0x3F, 16, 2);   // New object for lcd screend
//Screen map
char LCDMap[16][2]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

//New class to manage LEDs, connected to shiftout IC NXP 74HC595
class InformLED {
  public:  InformLED(byte l_pin, byte c_pin, byte d_pin) {
      L_PIN = l_pin;
      C_PIN = c_pin;
      D_PIN = d_pin;
      pinMode(L_PIN, OUTPUT);
      pinMode(C_PIN, OUTPUT);
      pinMode(D_PIN,  OUTPUT);

    }
  public:  void  BlinkLed(byte LED, int mseconds)  {
      BlinkDelay [LED - 1] = mseconds;
      BlinkStartTime [LED - 1] = millis();
      LedOn(LED);
    }
  public:  void  RunLED()  {
      int t = millis();
      for (int i = 0; i <= 7; i++) {
        if  ((BlinkStartTime [i] + BlinkDelay [i] <= t) && Status(i + 1) && (BlinkDelay[i] != -1)) { 
          LedOff(i + 1);
          BlinkStartTime[i] = 0;
          BlinkDelay[i] = -1;
        }
      }
    }
  public: boolean Status() {
      return  LEDStatus;
    }
  public: boolean Status(byte LED)  {
      byte  i = 1;
      i = i << (LED - 1);
      if (LEDStatus & i) {
        return true;
      } else {
        return false;
      }
    }
  public: void LedOn(byte LED) {
      byte  i = 1;
      i = i << (LED - 1);
      LEDStatus = LEDStatus | i;
      digitalWrite(L_PIN, LOW);                     //Turn latch off
      shiftOut(D_PIN, C_PIN, MSBFIRST, LEDStatus);  //Send data to IC
      digitalWrite(L_PIN, HIGH);                    //Turn latch on
    }

  public: void LedOff(byte LED) {
      byte  i = 1;
      i = i << (LED - 1);
      i = i ^ 255;
      LEDStatus = LEDStatus & i;
      digitalWrite(L_PIN, LOW);                      //Turn latch off
      shiftOut(D_PIN, C_PIN, MSBFIRST, LEDStatus);  //Send data to IC
      digitalWrite(L_PIN, HIGH);                    //Turn latch on
    }

  public: void AllLedOff() {
      LEDStatus = 0;
      for (int i = 0; i <= 7; i++) {
        BlinkDelay[i] = -1;
      }
      digitalWrite(L_PIN, LOW);
      shiftOut(D_PIN, C_PIN, MSBFIRST, LEDStatus);  //Turn off all LEDs
      digitalWrite(L_PIN, HIGH);
    }
  private:    byte  LEDStatus = 0;
  private:    int   BlinkDelay [8] = { -1, -1, -1, -1, -1, -1, -1, -1};
  private:    int   BlinkStartTime [8] = {0, 0, 0, 0, 0, 0, 0, 0};
  private:    byte  L_PIN = 0, C_PIN = 0, D_PIN = 0;
};
//  New object to manage LEDs
InformLED led(LATCH_PIN, CLOCK_PIN, DATA_PIN);

long PullupTimeSet[10]; //Time moments when pullups completed
int PullupCounter = 0;  //Number of completed pullups
long PullupSetStart = 0;

byte GripStatus[GripTotal], GripPrevStatus[GripTotal];  //Arrays to store grip status
long GripCapacity[GripTotal], GripOffset[GripTotal];    //Grips capacitance arrays
byte GripTouched = 0, GripPrevTouched = 0; //Number of touched grips

long  StartTime = 0, FinishTime = 0, PrevPullupTime = 0;  //Start, finish and previous pullup time
boolean HeadOnTopStatus = false, HeadOnTopPrevStatus = false; //Execution status (your head is over the ultrasonic sensor)

//key to send data to thingspeak.com
String apiKey = "ABCDEFGHIJKLMNOP";//Insert your own key here

//Variables for WiFi adapter
#define BUFFER_SIZE 80
char buffer[BUFFER_SIZE];
boolean  WiFiState;
String  NetworkSSID = "";
String  NetworkPASS = "";

//Start a serial connection between Arduino and WiFi-adapter
SoftwareSerial ser(RX_WIFI_PIN, TX_WIFI_PIN); // RX, TX

//Create an object for cardreader
MFRC522 mfrc522(RFID_SDA_PIN, RFID_RST_PIN);        // Create MFRC522 instance.
void setup()
{
  Serial.begin(9600);
  lcd.init();                            // lcd initialization
  lcd.backlight();                       // Turn on backlight on LCD screen

  lcd.clear();
  lcd.cursor();
  
  DebugPrintln(F("Start. LED self-test"), DBGALL);
  LCDPrintln(&lcd,F("Start"));
  
  if (dbg>=DBGALL) //Blink LEDs if high debug level
  {
      for (int i = 3; i <= 8; i++) {led.BlinkLed(i, 2000);}
        for (int i = 0; i <= 500; i++) {
          led.RunLED();
          delay(100);
        }
    
  }
//Init serialcommands
  serialCommand.addCommand("1", CompDebugLevel );   //Debug levels configuration
  serialCommand.addCommand("2", CompWiFi);          //WiFi management
  serialCommand.addCommand("3", CompWiFiChangeSSID);//Change WiFi SSID in the memory
  serialCommand.addCommand("4", CompWiFiChangePass);//Change WiFi password (key) in the memory
  serialCommand.addCommand("5", CompRfid);          //RFID tokens management- to be developed
  serialCommand.addDefaultHandler(UnrecognizedCommand);
  
  //Load WiFi configuration from ROM
  char ssidchar[WiFiSSIDLen];
  char passchar[WiFiSSIDLen];
            
  EEPROM.get(0, ssidchar);
  EEPROM.get(WiFiSSIDLen,passchar);
  NetworkSSID=ssidchar;
  NetworkPASS=passchar;
  
  //Start connection between Arduino and WiFi-adapter
  ser.begin(9600);

  //Connecting to WiFi network
  DebugPrint(F("Connect WiFi status: "), DBGINFO);
  WiFiState = connectWiFi(&ser, NetworkSSID, NetworkPASS);
  if (WiFiState) {
    DebugPrintln(F("WiFi connected"), DBGINFO);
    LCDPrintln(&lcd,F("WiFi connected"));
    led.LedOn(WIFI_OK_LED);
  } else
  {
    DebugPrintln(F("WiFi connect error"),DBGINFO);
    LCDPrintln(&lcd,F("WiFi connect error"));
    led.LedOn(WIFI_ERR_LED);
  }

  //Config base level of capacitance for each grip
  GripOffset[0] = ADCTouch.read(LEFT_NORM_GRIP, 500);
  GripOffset[1] = ADCTouch.read(RIGHT_NORM_GRIP, 500);

  //Init cardreader
    SPI.begin();               // Init SPI bus
    mfrc522.PCD_Init();        // Init MFRC522 card
}

void loop()
{

  long RunTime = millis();
  boolean HeadOnTopStatus = GetCompletedPullup();
  String ATResult;
  String cmd;
  int TCPSendResult;

  //Check if some of the grips touched
  if (ReadGripStatus())
  {
    if (GripTouched > 0 && GripPrevTouched == 0) {
      StartTime = RunTime;
      PrevPullupTime = RunTime;
      led.LedOn(TOUCH_LED);
      led.BlinkLed(SPK_LED, 200);
      DebugPrintln(F("======= Pull-up set started ======="),DBGINFO);
      LCDPrintln(&lcd,F("Pullups started"));
    }

    DebugPrint(String(RunTime),DBGINFO);
    DebugPrint(F("\t"),DBGINFO);
    PrintGripStatus();
    DebugPrintln("",DBGINFO);

  //Check if all grips untouched- the pullup set completed
    if (GripTouched == 0 && GripPrevTouched > 0) {
      FinishTime = RunTime;
      led.LedOff(TOUCH_LED);
      DebugPrint(F("======= Pull-up set FINISHED , excercise time="),DBGINFO);
      DebugPrint(String((FinishTime - StartTime) / 1000), DBGINFO);
      DebugPrint(F(" seconds, "), DBGINFO);
      DebugPrint(String(PullupCounter), DBGINFO);
      DebugPrintln(F(" Pullups completed. ======="), DBGINFO);
      LCDPrintln(&lcd,"Pshups "+String(PullupCounter)+","+String((FinishTime - StartTime) / 1000)+"sec");

      if (WiFiState ) {   //Publish data to thingspeak.com if WiFi connected

        if (PullupCounter > 0) {  //It's make sense to publish more then zero pullups
          DebugPrintln(F("Staring WiFi transfer"), DBGINFO);
          clearSerialBuffer(&ser);
          ATResult = StartTCPConnection(&ser, F("144.212.80.10"), 80, 1000); //Establish TCP connection 
          DebugPrint(F("\tStartTCPConnection result: "), DBGALL);
          DebugPrintln(ATResult, DBGALL);

          // Create a GET command to publish data to http://thingspeak.com
          String getStr = F("GET /update?api_key=");
          getStr += apiKey;
          getStr += F("&field1=");
          getStr += String(PullupCounter);//PullupsFieled;
          getStr += F("&field2=");
          getStr += String((FinishTime - StartTime) / 1000);
          getStr += F("\r\n");
          DebugPrintln(getStr, DBGALL);

          TCPSendResult = SendTCPData(&ser, getStr);
          
          DebugPrint(F("\tResult to server upload status:"), DBGALL);
          DebugPrint(String(TCPSendResult), DBGALL);
          DebugPrint(F("\t"), DBGALL);

          //Case to check different send results
          switch (TCPSendResult)  {
            case WIFIERR_OK:
              DebugPrintln(F("success"), DBGALL);
              LCDPrintln(&lcd,F("Result upload ok"));
              break;
            case WIFIERR_NOTCPCONNECTION:
              DebugPrintln(F("Can not establish tcp connection"), DBGERROR);
              LCDPrintln(&lcd,F("No TCP connect"));
              break;
            case WIFIERR_SENDERR:
              DebugPrintln(F("Error on thingspeak.com protocol level"), DBGERROR);
              LCDPrintln(&lcd,F("Thingspeak error"));
              break;
            case WIFIERR_SERVERERR:
              DebugPrintln(F("Upload data not accepted, may be too fast for next update"), DBGERROR);
              LCDPrintln(&lcd,F("Too fast upload"));
              break;
            default:
              DebugPrintln(F("Upload unknown error"), DBGERROR);
              LCDPrintln(&lcd,F("Unknown error"));
          }
          led.BlinkLed(SPK_LED, 200);       //beep for 0.2 seconds
          led.BlinkLed(WIFI_ERR_LED, 500);  //blink for 0.5 seconds
        }
      } else
      {
        DebugPrintln(F("Could not send results: WiFi not connected"), DBGERROR);
        LCDPrintln(&lcd,F("No WiFi error"));
      }

      PullupCounter = 0;
    }
  }

  if ((HeadOnTopStatus != HeadOnTopPrevStatus) && (GripTouched > 0))
  {
    if (HeadOnTopStatus)
    {
      if (RunTime - PrevPullupTime > MIN_PULLUP_TIME) {
        PullupTimeSet[PullupCounter] = RunTime - PrevPullupTime; //Store one more pullup
        PrevPullupTime = RunTime;
        PullupCounter++;
        led.BlinkLed(SPK_LED, 200);
        led.BlinkLed(PULLUP_LED, 500);

        DebugPrint(String(PullupCounter), DBGINFO);
        DebugPrint(F(" done in "), DBGINFO);
        DebugPrint(String(PullupTimeSet[PullupCounter - 1]), DBGINFO);
        DebugPrintln(F(" mseconds."), DBGINFO);
      } else
      {
        DebugPrint(String(RunTime - PrevPullupTime), DBGINFO);
        DebugPrint(F(" mseconds is too fast for pullup!!!! Minimal time is "), DBGINFO);
        DebugPrintln(String(MIN_PULLUP_TIME), DBGINFO);
      }
    }
  }

  HeadOnTopPrevStatus = HeadOnTopStatus;
  led.RunLED();     //We have to run this function to update LEDs- may be we have to turn off some LEDs or buzzer
  
  ReadRFIDCard();   //We have to read RFID cards

  serialCommand.readSerial();   //We have to check is there some new commands from the desktop computer
}


/*This function reads current capacity of grips, and updates GripStatus and GripPrevStatus arrays
    It returns: TRUE if some grip states are changed and FALSE, if unchanged*/
boolean ReadGripStatus()
{
  int ChangeCounter = 0;
  GripPrevTouched = GripTouched;
  GripTouched = 0;
  //Store previous grips state
  GripCapacity[0] = ADCTouch.read(LEFT_NORM_GRIP, CapAverageBase) - GripOffset[0];
  GripCapacity[1] = ADCTouch.read(RIGHT_NORM_GRIP, CapAverageBase) - GripOffset[1];

  for (int i = 0; i < GripTotal; i++) {
    GripPrevStatus[i] = GripStatus[i];

    if (GripCapacity[i] > CapacityTouchLevel) {
      GripStatus[i] = 1;
    }
    else
    {
      GripStatus[i] = 0;
    }
    GripTouched += GripStatus[i];
    if (GripStatus[i] != GripPrevStatus[i]) ChangeCounter++;
  }

  if (ChangeCounter > 0)
  {
    return true;
  } else
  {
    return false;
  }
}

//Printing grips status- for debuging purpose only
void PrintGripStatus()
{
  for (int i = 0; i < GripTotal; i++)
  {
    DebugPrint(String(GripStatus[i]), DBGINFO);
    DebugPrint("\t", DBGINFO);
  }

  for (int i = 0; i < GripTotal; i++)
  {
    DebugPrint(String(GripCapacity[i]), DBGINFO);
    DebugPrint("\t", DBGINFO);
  }

}

//Check if a pullup completed
boolean  GetCompletedPullup()
{
  unsigned int distance = sonar.ping(); // Send ping, get ping time in microseconds (uS).
  distance /= US_ROUNDTRIP_CM; // Convert ping time to distance in cm
  if (distance > MAX_PULLUP_DIST || distance < MIN_PULLUP_DIST)
  {
    return false;
  } else
  {
    return true;
  }
}

//Here is a Wi-Fi subroutines
//Send AT command to WiFi adapter
String PassATCommand(SoftwareSerial *ESPport, String AT_Command, int wait)
{
  String tmpData;
  unsigned long t1 = 0, t = 0, t2 = 0;
  
  
      DebugPrint(F("PassATCommand started. Command="), DBGALL);
      DebugPrint(AT_Command, DBGALL);
      DebugPrint(F(" delay="), DBGALL);
      DebugPrintln(String(wait), DBGALL);
      
      DebugPrint(F("Memory available="), DBGVERBOSE);
      DebugPrintln(String(freeRam()), DBGVERBOSE);
      
  ESPport->println(AT_Command);

  t1 = millis();
  t = t1;
  t2 = t1 + wait;
  
  
      DebugPrintln(String("t1="+String(t1)+" t="+String(t)+" t2="+String(t2)), DBGVERBOSE);
  
  while (t < t2)
  {
    while (ESPport->available() > 0 )
    {
      char c = ESPport->read();
      tmpData += c;

      if ( tmpData.indexOf(AT_Command) > -1 )
        tmpData = ""; //If we get back a text of AT command- clean string
      else
      {
        tmpData.trim();//delete blank characters and CR
      }
    }
    t = millis();
  }
  
      DebugPrint(F("PassATCommand finished. ATResult="), DBGALL);
      DebugPrintln(tmpData, DBGALL);
  
  return tmpData;
}
///Clean  ESPport
void clearSerialBuffer(SoftwareSerial *ESPport)
{
  while ( ESPport->available() > 0 )
  {
    ESPport->read();
  }
}

//Show available WiFi networks
String  ListWiFiNetworks() {
  String ConnectResult;
  
  ConnectResult=PassATCommand(&ser,"AT+CWLAP",2000);
  DebugPrintln(F("ListWiFiNetwork="), DBGERROR);
  DebugPrintln(ConnectResult, DBGERROR);

  return ConnectResult;
}

//Reset WiFi adapter
String  ResetWiFi(SoftwareSerial *ESPport) {
  String ConnectResult;
  
  ConnectResult=PassATCommand(ESPport,"AT+RST",3000);
  DebugPrintln(F("ResetWiFi="), DBGERROR);
  DebugPrintln(ConnectResult, DBGERROR);

  return ConnectResult;
}

//Check WiFi connection status
String  WiFiStatus(SoftwareSerial *ESPport) {
  String ConnectResult;
  
  ConnectResult = PassATCommand(ESPport, "AT+CWJAP?", 3000);
  DebugPrintln(F("WiFiStatus="), DBGERROR);
  DebugPrintln(ConnectResult, DBGERROR);

  return ConnectResult;
}



//Connect to WiFi network
boolean connectWiFi(SoftwareSerial *ESPport, String NetworkSSID, String NetworkPASS)
{
  //Reset WiFi-adapter
  //PassATCommand(ESPport,"AT+RST",3000);
  //ConnectResult=PassATCommand(ESPport,"AT+CWLAP",300);

  String ConnectResult;
  ConnectResult = PassATCommand(ESPport, "AT+CWJAP?", 3000);
  if (strstr(ConnectResult.c_str(), NetworkSSID.c_str()) != NULL) {
        DebugPrintln(F("Wifi already connected"), DBGALL);
        return true;
  }

  //Set device mode. 1- client, 2- access point, 3- client+access point
  ConnectResult = PassATCommand(ESPport, "AT+CWMODE=1", 300);

  String cmd = "AT+CWJAP=\"";
  cmd += NetworkSSID;
  cmd += "\",\"";
  cmd += NetworkPASS;
  cmd += "\"";
  //Connect to WiFi network
  PassATCommand(ESPport, cmd, 6500);

  //We have to get WiFi network name+OK. It's connected if substring found
  if (ConnectResult.indexOf(F("WIFICONNECTEDWIFIGOTIPOK")) > -1) {
    return true;
  } else
  {
    return false;
  }

}
//Start TCP connection
String StartTCPConnection(SoftwareSerial *ESPport, String IP, int Port, int wait)
{
  String ATResult;

      DebugPrint(F("Function StartTCPConnection started. IP="), DBGALL);
      DebugPrintln(IP, DBGALL);
  
  // Create AT command for WiFi adapter
  String cmd = "AT+CIPSTART=\"TCP\",\"";
  cmd += IP;
  cmd += "\",";
  cmd += (String) Port;

  clearSerialBuffer(ESPport);
  //Establish TCP connection
  ATResult = PassATCommand(&ser, cmd, 1000);

      DebugPrint("ATResult=", DBGALL);
      DebugPrintln(ATResult, DBGALL);

  return ATResult;
}

int   SendTCPData(SoftwareSerial *ESPport, String data) {
  String cmd = "";
  String ATResult;

      DebugPrint("Memory available=", DBGVERBOSE);
      DebugPrintln(String(freeRam()), DBGVERBOSE);
  
  cmd += "AT+CIPSEND=";
  cmd += String(data.length());

  ATResult = PassATCommand(ESPport, cmd, 1000);

      DebugPrint(F("Ready to transfer data:"), DBGVERBOSE);
      DebugPrintln(data, DBGVERBOSE);
  
  ATResult = PassATCommand(ESPport, data, 1000);

      DebugPrint(F("Data transfered\nATResult="), DBGALL);
      DebugPrintln(ATResult, DBGALL);

  if (ATResult.indexOf("SENDOK+IPD") > 0) {
  DebugPrintln(F("Data sent"), DBGALL);
    int i = ATResult.indexOf(":");
    int j = ATResult.indexOf("CLOSED");
    String ThingSpeakResultNumber;
    ThingSpeakResultNumber = ATResult.substring(i + 1, j);
    int k = ThingSpeakResultNumber.toInt();
    
    if (k) {
      return WIFIERR_OK;
      
    } else
    {
      return WIFIERR_SERVERERR;
    }
  }

  return WIFIERR_UNKNOWN;
}

//Read RFID cards
void ReadRFIDCard() {
  //Check if there are new card available
  if ( ! mfrc522.PICC_IsNewCardPresent()) {
    return;
  }

  DebugPrintln(F("RFID started"), DBGINFO);
  
  // Check if reading acceptable
  if ( ! mfrc522.PICC_ReadCardSerial())    return;

  DebugPrint(F("Card UID in HEX format:"), DBGINFO);    //Dump UID
  for (byte i = 0; i < mfrc522.uid.size; i++) {
    DebugPrint(String(mfrc522.uid.uidByte[i] < 0x10 ? " 0" : " "), DBGINFO);
    DebugPrint(String(mfrc522.uid.uidByte[i], HEX), DBGINFO);
  }

  DebugPrintln("", DBGINFO);
  DebugPrint(F("Card UID:"), DBGINFO);    //Dump UID
  for (byte i = 0; i < mfrc522.uid.size; i++) {
    DebugPrint(String((char)mfrc522.uid.uidByte[i]), DBGINFO);
  }
  byte piccType = mfrc522.PICC_GetType(mfrc522.uid.sak);
  DebugPrint(F("PICC type:"), DBGINFO);
  DebugPrintln(String(mfrc522.PICC_GetTypeName(piccType)), DBGINFO);
  
  if ( piccType != MFRC522::PICC_TYPE_MIFARE_MINI
       &&  piccType != MFRC522::PICC_TYPE_MIFARE_1K
       &&        piccType != MFRC522::PICC_TYPE_MIFARE_4K) {
    DebugPrintln(F("This program only works with MIFARE Classic cards."), DBGERROR);
    return;
  }
  DebugPrintln("", DBGINFO);
  LCDPrintln(&lcd,F("Card read"));
  
  //Turn off CPU inside if RFID card to exclude double reading
  mfrc522.PICC_HaltA(); // Halt PICC
  led.BlinkLed(SPK_LED, 200); //beep for 0.2 seconds
  led.LedOn(RFID_LED);        //blink to show that RFID card accepted
  }


//Print some string starting at (X,Y) positions
void  LCDPrintXY(LiquidCrystal_I2C * lcd, String text, byte x, byte y)
{
  char maxstr=0;
  DebugPrintln(String("x="+String(x)+", y="+String(y)), DBGALL);
  if (text.length()<16) maxstr=text.length(); else maxstr=16;
  for (int i=0;i<maxstr;i++)
  {
   
          DebugPrintln(String("i="+String(i)+", c="+text[i]), DBGALL);
   
    lcd->setCursor(x+i, y);
    lcd->print(text[i]);            // Print text
    LCDMap[x+i][y]=text[i];
  }
      DebugPrintln("", DBGALL);
  
}

//Print a string and scroll up
void  LCDPrintln(LiquidCrystal_I2C * lcd, String text)
{
  char c=0, maxstr=0;
  
  for (int i=0;i<16;i++)
  {
          DebugPrint(String(text[i]), DBGALL); 
    
    if (i>=text.length()) c=32; else c=text[i];
    
    lcd->setCursor(i, 0);
    lcd->print(LCDMap[i][1]);
    LCDMap[i][1]=c;
    lcd->setCursor(i, 1);
    lcd->print(c);
  }
      DebugPrintln("", DBGALL);
}

//Show mem available (RAM)
int freeRam ()
{
  extern int __heap_start, *__brkval;
  int v;
  return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
}

//Similar to Serial.print, but with debug level checking
void  DebugPrint(String SerialText, byte DBGLevel)

{
  if  (dbg>=DBGLevel)
  {
        Serial.print(SerialText);
  }
}

//Similar to Serial.println, but with debug level checking
void  DebugPrintln(String SerialText, byte DBGLevel)
{
  if  (dbg>=DBGLevel)
  {
        Serial.println(SerialText);
  }
}

//////////////////////////////////////////////////////////////////
//Subroutines to chat with computer via serial


//Change debug levels
void CompDebugLevel() {
  char *arg = serialCommand.next();
  boolean CorrectArgument=false;
  
  if (arg != NULL){
    if ( strcmp(arg, "0" ) == 0){
      dbg=DBGNO;
      CorrectArgument=true;
      DebugPrintln(F("Debug level=DBGNO"), DBGINFO);
    }
    if ( strcmp(arg, "1" ) == 0){
      dbg=DBGERROR;
      CorrectArgument=true;
      DebugPrintln(F("Debug level=DBGERROR"), DBGINFO);
    }
    if ( strcmp(arg, "2" ) == 0){
      dbg=DBGPROTOCOL;
      CorrectArgument=true;
      DebugPrintln(F("Debug level=DBGPROTOCOL"), DBGINFO);
    }
    if ( strcmp(arg, "3" ) == 0){
      dbg=DBGINFO;
      CorrectArgument=true;
      DebugPrintln(F("Debug level=DBGINFO"), DBGINFO);
    }
    if ( strcmp(arg, "4" ) == 0){
      dbg=DBGALL;
      CorrectArgument=true;
      DebugPrintln(F("Debug level=DBGALL"), DBGINFO);
    }
    if ( strcmp(arg, "5" ) == 0){
      dbg=DBGVERBOSE;
      CorrectArgument=true;
      DebugPrintln(F("Debug level=DBGVERBOSE"), DBGINFO);
    }
    if(!CorrectArgument) {
    DebugPrintln(F("Incorrect parameters"), DBGERROR);
    }
  } else
  {
    DebugPrint(F("Debug level="), DBGINFO);
    DebugPrintln(String(dbg), DBGPROTOCOL);
  }
}


//Manage WiFi settings
void  CompWiFi() {
  char *arg = serialCommand.next();
  boolean CorrectArgument=false;
  String ATResult;

  if (arg != NULL)
  {
    //Get WiFi status
          if ( strcmp(arg, "1" ) == 0){
            CorrectArgument=true;
            if (WiFiState) DebugPrintln("1", DBGPROTOCOL);  else DebugPrintln("0", DBGPROTOCOL);
          }

    //Get WiFi current configuration
          if ( strcmp(arg, "2" ) == 0){
            CorrectArgument=true;
            DebugPrintln(NetworkSSID, DBGPROTOCOL);
            DebugPrintln(NetworkPASS, DBGPROTOCOL);
          }
    //Save to EEPROM current WiFi configuration
          if ( strcmp(arg, "3" ) == 0){
            CorrectArgument=true;
            char ssidchar[WiFiSSIDLen];
            char passchar[WiFiPASSLen];
            
            NetworkSSID.toCharArray(ssidchar, WiFiSSIDLen);
            NetworkPASS.toCharArray(passchar, WiFiPASSLen);

            DebugPrintln(F("save to flash"), DBGALL);
            DebugPrintln(ssidchar,DBGALL);
            DebugPrintln(passchar,DBGALL);
            EEPROM.put(0, ssidchar);
            EEPROM.put(WiFiSSIDLen,passchar);
          }
  //Load from EEPROM WiFi configuration
          if ( strcmp(arg, "4" ) == 0){
            CorrectArgument=true;
            char ssidchar[WiFiSSIDLen];
            char passchar[WiFiPASSLen];
            
            EEPROM.get(0, ssidchar);
            EEPROM.get(WiFiSSIDLen,passchar);
            NetworkSSID=ssidchar;
            NetworkPASS=passchar;
            
            DebugPrintln(F("load from flash"), DBGALL);
            DebugPrintln(NetworkSSID, DBGPROTOCOL);
            DebugPrintln(NetworkPASS, DBGPROTOCOL);
          }
   //Reconnect to WiFi
          if ( strcmp(arg, "5" ) == 0){
            CorrectArgument=true;
            
            DebugPrint(F("Connect WiFi status: "), DBGINFO);
            WiFiState = connectWiFi(&ser, NetworkSSID, NetworkPASS);
            if (WiFiState) {
              DebugPrintln(F("WiFi connected"), DBGINFO);
              LCDPrintln(&lcd,F("WiFi connected"));
              led.LedOn(WIFI_OK_LED);
            } else
            {
              DebugPrintln(F("WiFi connect error"),DBGINFO);
              LCDPrintln(&lcd,F("WiFi connect error"));
              led.LedOn(WIFI_ERR_LED);
            }
          }
          
  }
    if(!CorrectArgument) {
    DebugPrintln(F("Incorrect parameters"), DBGERROR);
    }
} 


//Change SSID for WiFi network
void CompWiFiChangeSSID() {
  char *arg = serialCommand.next();

  if (arg != NULL)
  {
    NetworkSSID=arg;
    DebugPrint(F("NetworkSSID="), DBGALL);
    DebugPrintln(NetworkSSID, DBGALL);
  } else {
    DebugPrint(F("Incorrect parameters"), DBGALL);
  }
}

//Change password for WiFi network
void CompWiFiChangePass() {
  char *arg = serialCommand.next();

  if (arg != NULL)
  {
    NetworkPASS=arg;
    DebugPrint(F("NetworkPASS="), DBGALL);
    DebugPrintln(NetworkPASS, DBGALL);
  } else {
    DebugPrint(F("Incorrect parameters"), DBGALL);
  }
}


//Manage RFID cards- to be developed
void CompRfid() {
  
}

//Some other trash from the computer
void UnrecognizedCommand(){  
  
  DebugPrintln(F("Unrecognized command"), DBGERROR);
  DebugPrintln(F("Usage: <debug|wifi|changessid|changepass|flash|rfid>"), DBGERROR);
}