#include <CapacitiveSensor.h>

//Arduino Pin Assignments
const int touchSendPin = 14;
const int touchReceivePin = 15;
const int motorDown   = 18;   //H-Bridge control to make the motor go down
const int motorUp     = 19;   //H-Bridge control to make the motor go up

//Inputs
const int faderWiper  = 17;   //Position of fader relative to GND
const int controlPot  = 16;   //Potentiometer to set position of fader

CapacitiveSensor touchSensor(touchSendPin, touchReceivePin);

volatile bool touched  = false; //Is the fader currently being touched?

long previousTime = 0;
float ePrevious = 0;
float eIntegral = 0;

int freq = 31372;
uint16_t target = 0;

uint16_t pot_max_value;
uint16_t pot_min_value;

uint16_t position;

unsigned long previousMillis = 0;   // storing last millis
const long interval = 5000;

void setup()
{
  Serial.begin(115200);
  pinMode (motorUp, OUTPUT);
  pinMode (motorDown, OUTPUT);
  analogWriteFrequency(motorUp, freq);
  analogWriteFrequency(motorDown, freq);
  analogWrite(motorUp, 0);
  analogWrite(motorDown, 0);
  calibration();
}

void loop()
{

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    // save current time
    previousMillis = currentMillis;

    // generate target value between 0 and 1023
    target = random(0, 1023);
  }

  checkTouch();
  position = analogRead(faderWiper);


    if (!touched) {
  float kp = 6;
  float ki = 2;
    float kd = 0.08;
  float u = pidController(target, kp, kd, ki);
    moveMotor(u);
  } else {
    stopMotor();
  }
  Serial.print("Target_Position:");
  Serial.print(target);
  Serial.print(", ");
    Serial.print("Fader_Position:");
    Serial.print(position);
    Serial.print(", ");
    Serial.print("Max_Value:");
    Serial.print(pot_max_value);
    Serial.print(", ");
    Serial.print("Min_Value:");
    Serial.println(pot_min_value);

    delay(1);
}

float pidController(int target, float kp, float kd, float ki)
{
  long currentTime = micros();
  float deltaT = ((float)(currentTime - previousTime)) / 1.0e6;

  int e = position - target;
  float eDerivative = (e - ePrevious) / deltaT;
  eIntegral = eIntegral + e * deltaT;

  float u = (kp * e) + (kd * eDerivative) + (ki * eIntegral);

  previousTime = currentTime;
  ePrevious = e;

  return u;
}

void moveMotor(float u)
{
  float speed = fabs(u);
  if (speed > 255) {
    speed = 255;
  }

  if (u < 0) {
    analogWrite(motorUp, speed);
    analogWrite(motorDown, LOW);
  } else if (u > 0) {
    analogWrite(motorDown, speed);
    analogWrite(motorUp, LOW);
  } else {
    stopMotor();
  }
}

void stopMotor()
{
  analogWrite(motorDown, HIGH);
    analogWrite(motorUp, HIGH);
}

void checkTouch()
{
  touched = touchSensor.capacitiveSensor(5) > 9000;
}

void calibration()
{
  stopMotor();
    analogWrite(motorUp, 255);
  analogWrite(motorDown, LOW);
    delay(1000);
    pot_max_value = analogRead(faderWiper);

    analogWrite(motorUp, LOW);
  analogWrite(motorDown, 255);
  delay(1000);
    pot_min_value = analogRead(faderWiper);

    stopMotor();
}