#include "TimerOne.h"
#include <PID_v1.h>

int counter=0;

const int IN1 = 11;
const int IN2 = 10;
volatile float pot=0;
const int POT = 0;
const int ENA = 6;

double Setpoint, Input, Output;
double Kp=2, Ki=5, Kd=1;

PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);


void docount()  // counts from the speed sensor
{
  counter++;  // increase +1 the counter value
}

void timerIsr()
{
  Timer1.detachInterrupt();  //stop the timer
  Serial.print("Motor Speed: ");
  int rotation = (counter / 30);  // divide by number of holes in Disc
  Serial.print(rotation,DEC);
  Serial.println(" Rotation per second");
  counter=0;  //  reset counter to zero
  Timer1.attachInterrupt( timerIsr );  //enable the timer
  Serial.print("pot = ");
  Serial.print(pot);
  Serial.print("n");

  //return rotation;
}

void setup()
{
  Serial.begin(9600);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);

  Timer1.initialize(1000000); // set timer for 1sec
  attachInterrupt(0, docount, FALLING);  // increase counter when speed sensor pin goes High
  Timer1.attachInterrupt( timerIsr ); // enable the timer


  myPID.SetMode(AUTOMATIC);
}

void loop()
{
  //Input = timerIsr();
  pot=analogRead(POT)/4.01569;
  //Setpoint = sp;

  //Serial.print(Input);

  analogWrite(ENA, pot);
  digitalWrite(10, HIGH);  // set rotation of motor to Clockwise
  digitalWrite(11, LOW);

  delay (250);
}