#include <CapacitiveSensor.h>

#define touchSendPin 14
#define touchReceivePin 15

//Arduino Pin Assignments
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 wiper        = 17;   //Position of fader relative to GND
const int pot          = 16;   //Potentiometer to set position of fader

double faderMax        = 0;   //Value read by fader's maximum position (0-1023)
double faderMin        = 0;   //Value read by fader's minimum position (0-1023)

CapacitiveSensor touchSensor(touchSendPin, touchReceivePin);

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

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

int position = analogRead(wiper);

void setup() {
  Serial.begin(9600);
  pinMode (motorUp, OUTPUT);
  pinMode (motorDown, OUTPUT);
}

void loop()
{
  checkTouch();

  // Setpoint
  int target = analogRead(pot);
  position = analogRead(wiper);

  float kp = 2.0;
  float kd = 0.1;
  float ki = 0.01;
  float u = pidController(target, kp, kd, ki);

  if (!touched) {
    moveMotor(u);
  }

  Serial.print(target);
  Serial.print(", ");
  Serial.println(position);

}

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, LOW);
    analogWrite(motorDown, speed);
  } else if (u > 0) {
    analogWrite(motorDown, LOW);
    analogWrite(motorUp, speed);
  } else {
    analogWrite(motorUp, LOW);
    analogWrite(motorDown, LOW);
  }
}

void checkTouch() {
  touched = touchSensor.capacitiveSensor(30) > 3000;
}