const int EncoderPin1 = 7 ;
const int EncoderPin2 = 6;
 int encoderPin2Value = 0 ;
 
volatile int motorPosition = 0 ;
float previousmotorPosition = -1 ;

int MotorDirection ;
int RotaryCLK = 5 ;
int RotaryDT = 4 ;
int RotarySW = 3 ;
int SW_buttonValue = 0;

int RotaryValue ;
int RotaryTime ;
volatile int encoderPosition = 0 ; 
float   encoderPreviousPosition = -1 ;

int RightDirectionPin = 12 ;
int LeftDirectionPin = 11 ;


int CLKNow ;
int CLKPrevious ;
int DTNow ;
int DTPrevious ;

int TargetPosition ; 

float currentTime  = millis();
float previousTime ;
float errorValue ;
float previousError ; 
float error ;

float controlSignal = 0; //u - Also called as process variable (PV)

//PID parameters - tuned by the user
double kp = 1.35 ;
double ki = 0.00005;
double kd = 0.01;

void setup() {

 Serial.begin(115200);

pinMode(EncoderPin1,INPUT);
pinMode(EncoderPin2,INPUT);
attachInterrupt(digitalPinToInterrupt(EncoderPin1),CheckEncoder,RISING);

pinMode(RightDirectionPin,OUTPUT);
pinMode(LeftDirectionPin,OUTPUT);

pinMode( RotaryCLK,INPUT_PULLUP);
pinMode( RotaryDT,INPUT_PULLUP);
pinMode( RotarySW,INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(EncoderPin2),RotaryEncoder,CHANGE);


CLKPrevious = digitalRead(RotaryCLK);
DTPrevious = digitalRead(RotaryDT);

}

void loop() {

calculatePID();

DriveMotor();

CheckRotaryButton();

}
void CheckEncoder(){

encoderPin2Value = digitalRead(EncoderPin2);
if(encoderPin2Value == 1) //cw
{
  motorPosition++ ;
}
else if(encoderPin2Value == 0) //ccw
{
  motorPosition-- ;
}
}
void DriveMotor(){

if(controlSignal < 0){
 MotorDirection = -1 ;
}
else if(controlSignal >0)
{
  MotorDirection = 1 ;
}
else
{
 MotorDirection = 0 ;

}
 if(MotorDirection == -1)
 {
  digitalWrite(RightDirectionPin,LOW);
  digitalWrite(LeftDirectionPin,HIGH);
 }
 else if(MotorDirection == 1){
  digitalWrite(RightDirectionPin,HIGH);
  digitalWrite(LeftDirectionPin,LOW);
 }
 else{
  digitalWrite(RightDirectionPin,LOW);
  digitalWrite(LeftDirectionPin,LOW);
 }
}

void calculatePID() 
{


int DeltaTime = (currentTime - previousTime )/ 1000000 ;
currentTime = previousTime ;

errorValue = motorPosition - TargetPosition ;

double proportionnel = error ;
double integrale =+ error * ki;
double direvative = (error-previousError ); 
previousError = error ;

errorValue = (proportionnel*kp)+(integrale*ki)+(direvative*kd);




}
void RotaryEncoder(){

CLKNow = digitalRead(RotaryCLK);

if(CLKNow != CLKPrevious && CLKNow == 1)
{
  if(RotaryDT != CLKNow){
    RotaryValue = RotaryValue - 515 ;
  }
  else{
     RotaryValue = RotaryValue + 515 ;
  }
 
}
 CLKNow = CLKPrevious ;
}
void CheckRotaryButton() {

SW_buttonValue = digitalRead(RotarySW);

if(SW_buttonValue == 0){
  
  if(millis() - RotaryTime > 1000){
   
    TargetPosition =  RotaryValue ;

   RotaryTime = millis(); 

  }
}
  
}