#include <AutoPID.h>
#include <Stepper.h>

const int stepsPerRevolution = 200;  // change this to fit the number of steps per revolution
// for your motor

const byte stepPin1 = 3;  // Step pin of motor 1
const byte dirPin1 = 2;   // Dir pin of motor 1
Stepper motor1(stepsPerRevolution, stepPin1, dirPin1);

//pins
#define SENSOR_PIN A0
#define LED_PIN 13

#define SENSOR_READ_DELAY 10  //read sensor every ~20ms

//pid settings and gains
#define OUTPUT_MIN 50   // Minimum output speed
#define OUTPUT_MAX 300  // Maximum output speed
#define KP .12
#define KI .0003
#define KD 0

double sensorReading, outputSpeed;
double setPoint = 522;  // Sensor value to keep 

//input/output variables passed by reference, so they are updated automatically
AutoPID myPID(&sensorReading, &setPoint, &outputSpeed, OUTPUT_MIN, OUTPUT_MAX, KP, KI, KD);

unsigned long lastTempUpdate;  //tracks clock time of last temp update

//call repeatedly in loop, only updates after a certain time interval
//returns true if update happened
bool updateSensorReading() {
  if ((millis() - lastTempUpdate) > SENSOR_READ_DELAY) {
    sensorReading = analogRead(SENSOR_PIN);  //get temp reading
    lastTempUpdate = millis();
    return true;
  }
  return false;
}  //void updateSensorReading


void setup() {
  Serial.begin(115200);
  pinMode(SENSOR_PIN, INPUT);  // Set sensor pin as input

  // Initially set the speed of motor as current speed
  motor1.setSpeed(OUTPUT_MIN);

  //if temperature is more than 10 degrees below or above setpoint, OUTPUT will be set to min or max respectively
  myPID.setBangBang(10);
  //set PID update interval to 400ms
  myPID.setTimeStep(400);

}  //void setup


void loop() {
  updateSensorReading(); // Read Sensor
  myPID.run();  //call every loop, updates automatically at certain time interval
  Serial.print(sensorReading); Serial.print(" | "); Serial.println(outputSpeed);
  motor1.setSpeed(outputSpeed);
  // digitalWrite(LED_PIN, myPID.atSetPoint(1));  //light up LED when we're at setpoint +-1 degree
  motor1.step(1);
}  //void loop