#include <Arduino.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Arduino_FreeRTOS.h>
#include <queue.h>


#define EchoPin 5
#define TrigPin 6
#define LED1 7


QueueHandle_t xQueueUltrasonic;


long waktu;
int jarak;


void vTaskUltrasonic(void *pvParameters);
void vTaskLCD(void *pvParameters);


LiquidCrystal_I2C lcd(0x27,16,2);


void setup() {
 pinMode(EchoPin,INPUT);
 pinMode(TrigPin,OUTPUT);
 pinMode(LED1,OUTPUT);
 Serial.begin(9600);


 lcd.init();
 lcd.backlight();
 lcd.setCursor(0,0);
 lcd.print("ultrasonik");
 lcd.setCursor(0,1);
 lcd.print("jarak= ");


 xQueueUltrasonic = xQueueCreate(1, sizeof(int));


 xTaskCreate(vTaskUltrasonic, "Ultrasonic Task", 100, NULL, 1, NULL);
 xTaskCreate(vTaskLCD, "LCD Task", 100, NULL, 1, NULL);


 vTaskStartScheduler();
}


void loop() {
 // Empty. Things are done in the tasks.
}


void vTaskUltrasonic(void *pvParameters){
 for(;;){
    digitalWrite(TrigPin,LOW);
    delayMicroseconds(2);
    digitalWrite(TrigPin,HIGH);
    delayMicroseconds(10);
    digitalWrite(TrigPin,LOW);


    waktu=pulseIn(EchoPin,HIGH);
    jarak=waktu*0.034/2;


    if (jarak <=10){
      digitalWrite(LED1,HIGH);
    }
    else{
      digitalWrite(LED1,LOW);
    }


    xQueueSend(xQueueUltrasonic, &jarak, portMAX_DELAY);
    vTaskDelay(1000);
 }
}


void vTaskLCD(void *pvParameters){
 int jarak;
 for(;;){
    if(xQueueReceive(xQueueUltrasonic, &jarak, portMAX_DELAY) == pdPASS){
      lcd.setCursor (8,1);
      lcd.print(jarak);
    }
 }
}