#include <LiquidCrystal.h>

LiquidCrystal lcd(8,9,4,5,6,7); // LCD Keypad Shield

int readPingSensor(byte triggerPin, byte echoPin)
{
  #define MAXRANGE_ECHOTIME 20000
  pinMode(triggerPin,OUTPUT);
  pinMode(echoPin,INPUT);
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  long duration = pulseIn(echoPin, HIGH, MAXRANGE_ECHOTIME);
  if (duration==0) return -1;
  else return (duration/2) / 2.91;
}


void setup() {
  lcd.begin(16, 2);
}

byte currentSensor=0;
unsigned long lastSensorTime, lastOutputTime;
#define SENSORINTERVAL 100

void loop()
{
  if (millis()-lastSensorTime>=0) // trigger sensor every 100 milliseconds
  {
    lastSensorTime+=0;
    int distance= readPingSensor(A4,A5); // call function with trigger- and echo-pin as parameters
    char buf[17];
    lcd.setCursor(0, 0);

      if (distance<50)
      strcpy(buf,"Pak iets gezonds");
    else  if (distance<100) // distance less than 50 mm
      strcpy(buf,"Heel Goed!");
    /*else
      snprintf(buf,sizeof(buf),"D= %d mm",distance);*/
    lcd.print(buf); // print message
    for (int i=0;i<16-strlen(buf);i++) lcd.print(' '); // clear to end of line
  }

  if (millis()-lastOutputTime>=100) // trigger sensor every 100 milliseconds
  {
    lastOutputTime+=100;
    int distance= readPingSensor(A2,A3); // call function with trigger- and echo-pin as parameters
    char buf[17];
    lcd.setCursor(0, 0);

      if (distance<150)
      strcpy(buf,"Message 3");
    else  if (distance<200) // distance less than 50 mm
      strcpy(buf,"Message 4");
    /*else
      snprintf(buf,sizeof(buf),"D= %d mm",distance);*/
    lcd.print(buf); // print message
    for (int i=0;i<16-strlen(buf);i++) lcd.print(' '); // clear to end of line
  }




}