//https://www.youtube.com/watch?v=KkJML_tFbCY

#include <math.h>         //loads the more advanced math functions

#define encoder0PinA  2
#define encoder0PinB  4

#define joystick     A0
#define pwm1          9
#define pwm2         10


 //PID constants
double kp = 1;
double ki = 0.0;
double kd = 1;
unsigned long currentTime, previousTime;
double elapsedTime;
double  setPoint;
double error;
double lastError;
double cumError, rateError;
int output;


int motor_control; 

long encoder0Pos=0;
long newposition;
long oldposition = 0;
unsigned long newtime;
unsigned long oldtime = 0;
short t;

long vel;

void setup()
{
  pinMode(encoder0PinA, INPUT);         //encoder0PinA  2
  digitalWrite(encoder0PinA, HIGH);       
  pinMode(encoder0PinB, INPUT);         //encoder0PinB  4
  digitalWrite(encoder0PinB, HIGH);       
  attachInterrupt(0, doEncoder, RISING);  // encoDER ON PIN 2
  Serial.begin (9600);
  
}

void loop()
{
newposition = encoder0Pos;
newtime = millis();
//vel = (newposition-oldposition) /(newtime-oldtime);
t=newtime-oldtime;
vel = ((newposition-oldposition)*25)/t;
//Serial.print ("speed = ");
//Serial.print (vel+255);
oldposition = newposition;
oldtime = newtime;
//Serial.print ("newposition = ");
//Serial.println (newposition);
delay(5);

 motor_control = analogRead(joystick);

// Serial.print ("Analog = ");
// Serial.println (motor_control);

  motor_control >>= 1;
  
//  Serial.print (" ");
  
  
  Serial.print (vel+256);

  
  setPoint = motor_control;
  Serial.print (" ");
  Serial.print (setPoint);
  
  output = computePID(vel);
  
  
  //Serial.print ("Output = ");
  Serial.print (" ");
  Serial.println (output);
   
  if(output > 255){
    digitalWrite(pwm2, 0);
    analogWrite(pwm1, (output- 256));
  }
  else
    if(output < 255){
      digitalWrite(pwm1, 0);
      analogWrite(pwm2, (255 - output));
    }
    else{
      digitalWrite(pwm1, 0);
      digitalWrite(pwm2, 0);
    }

    

}

void doEncoder()
{
  if (digitalRead(encoder0PinA) == digitalRead(encoder0PinB)) {
    encoder0Pos++;
  } else {
    encoder0Pos--;
  }
}


double computePID(double inp){     
        currentTime = millis();                //get current time
        elapsedTime = (double)(currentTime - previousTime);        //compute time elapsed from previous computation
        
        error = setPoint - inp;                                // determine error
        cumError += error * elapsedTime;                // compute integral
        rateError = (error - lastError)/elapsedTime;   // compute derivative
 
        double out = kp*error + ki*cumError + kd*rateError;                //PID output               
 
        lastError = error;                                //remember current error
        previousTime = currentTime;                        //remember current time
 
        return out;                                        //have function return the PID output
}