#define step_pin 6
#define dir_pin 5
#define pot_pin A0

double actual_pos = 0; //actual position of the object (the previous location). Initially set to 0
double desired_pos; //the new position where we want to go (the desired location)

//double limit = 0.5;

double kp, kd, ki, dt;
double integral = 0, last_time, previous = 0;

void setup() {
  kp = 5.0;
  ki = 0.1;
  kd = 0.1;

  last_time = millis(); 

  pinMode(step_pin, OUTPUT); //step_pin collects the output from the arduino to decide how much to move
  pinMode(dir_pin, OUTPUT); //dir_pin sets the direction

  //clearing pin vlues: setting them to input LOW
  digitalWrite(step_pin, LOW);
  digitalWrite(dir_pin, LOW);

  delay(1000);
}

void loop() {
  double now = millis(); //returns number of millisecond since program started
  dt = (now - last_time) / 1000.0; //dt is in micro-seconds
  last_time = now; //resets time

  desired_pos = analogRead(pot_pin); //read value from potentiometer
  double error = desired_pos - actual_pos;

  if(abs(error)==0){
    return;
  }
  else{
    double out = pid(error); //PID loop for error reduction

    int steps = map(abs(error), 0, 1023, 0, 200); //calculate steps
    //move stepper motor
    direction(actual_pos, desired_pos);
    for(int i=0; i<=steps; i++){
      digitalWrite(step_pin, HIGH);
      delay(10);
      digitalWrite(step_pin, LOW);
      delay(10);
    } 
  }
  actual_pos = desired_pos;
}

double direction(double current_pos, double next_pos) {
  if(next_pos>current_pos){
    digitalWrite(dir_pin, HIGH);
  }
  else{
    digitalWrite(dir_pin, LOW);
  }
}

double pid(double error) {
  //objective: to reduce error to zero
  double proportional = error; //calculates proportional term 
  integral += error * dt; //calculates integral term
  double derivative = (error - previous) / dt; //calculates derivative term
  
  previous = error; //sets new signal value

  double output = (kp * proportional) + (ki * integral) + (kd * derivative); //calculates output signal
  return output;
}