#include <AccelStepper.h>
const int stepsPerRevolution = 800;
const int stepPin = 3;
const int dirPin = 2;
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);


// Set the desired range of motion in degrees
const float minAngle = -450.00;
const float maxAngle = +450.00;

// Define the range of steps corresponding to the angle
const float minSteps = -252.00;
const float maxSteps = +248.00;
float currentAngle = 0;
// PID Constants
float kp = 0.5;
float ki = 0.0;
float kd = 0.0;

// PID Variables
float previousError = 0;
float integral = 0;

// Setpoint
float setpoint = 0;

void setup()
{
  Serial.begin(9600);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  setpoint = 200;
  stepper.setMaxSpeed(100.0);
  stepper.setAcceleration(100.0);
}

void loop()
{
  currentAngle = stepper.currentPosition()*360/stepsPerRevolution ;
  

  float error = setpoint - currentAngle;
  integral += error;
  float derivative = error - previousError;

  float output = (kp * error) + (ki * integral) + (kd * derivative);
  Serial.print("Curr:");
  Serial.print(currentAngle);
  Serial.print("\terror:");
  Serial.print(error);
  Serial.print("\tintegral:");
  Serial.print(kp * error);
  Serial.print("\toutput:");
  Serial.println(output);
  currentAngle += output;
  stepper.moveTo(currentAngle);

  while (stepper.distanceToGo() != 0)
  {
    stepper.run();
    delay(1);
  }
   
  previousError = error;
  delay(10); 

}