//Created by Barbu Vulc!
//Based on this: https://techexplorations.com/blog/arduino/building-a-pid-controller-with-arduino-uno/
//Trying to make an Arduino Uno to work as a PID controller.
//Libraries:
#include <PID_v2.h>
#include <Arduino_FreeRTOS.h>

//Define variables:
#define BUTTON_PIN 2
#define LED_PIN 3
double setpoint, input, output;
double Kp = 10, Ki = 2, Kd = 0;
//Create PID object:
PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(9600);
  pinMode(BUTTON_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  myPID.SetMode(AUTOMATIC);
  setpoint = 100;
  xTaskCreate(Task, "PID control", 512, NULL, 1, NULL);
}

static void Task(void* pvParameters) {
  input = analogRead(BUTTON_PIN);
  myPID.Compute();
  analogWrite(LED_PIN, 255);
  Serial.println(setpoint);
  vTaskDelay(100 / portTICK_PERIOD_MS);

  if (digitalRead(BUTTON_PIN) == HIGH) {
    Serial.println(setpoint);
    Serial.print(" ");
    Serial.println(input);
    analogWrite(LED_PIN, output);
    vTaskDelay(500 / portTICK_PERIOD_MS);
  }
}

void loop() {}