// define board connection pins
#define ENCA 2 // YELLOW
#define ENCB 4 // WHITE
#define PWM 5
#define IN2 6
#define IN1 7

// PID parameters
#include "ArduPID.h"
ArduPID myController;

double input;
double output;

// Arbitrary setpoint and gains - adjust these as fit for your project:

double p = 1;
double i = 0;
double d = 0;
double setpoint;

#include <Encoder.h>
//  Change these pin numbers to the pins connected.
//   Best Performance: both pins have interrupt capability
//   Good Performance: only the first pin has interrupt capability
//   Low Performance:  neither pin has interrupt capability
//   avoid using pins with LEDs attached

// Encoder library parameters, pin numbers
Encoder knobLeft(ENCA, ENCB);

void setup() {
  Serial.begin(9600);
  myController.begin(&input, &output, &setpoint, p, i, d);
  myController.setOutputLimits(-255, 255);
  
  myController.start(); 
}
void loop() {

  setpoint = 100; // set new target postion
  long newLeft;  
  newLeft = knobLeft.read(); // read sensor value
  input = newLeft; // Replace with sensor feedback
  myController.compute(); // compute PID correction

  // motor control segment
  int dir = 1;  // define direction
  if(output<0){
    dir = -1;
  }
  float pwr = fabs(output); // normalize power
  // signal the motor
  setMotor(dir,pwr,PWM,IN1,IN2);

  
    Serial.print("Input "); 
    Serial.print(input);
    Serial.print(", Output ");
    Serial.print(output);
    Serial.print(", Power ");
    Serial.print(pwr);
    Serial.print(", Direction ");
    Serial.print(dir);
    Serial.println();
}

 void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
  analogWrite(pwm,pwmVal);
  if(dir == 1){
    digitalWrite(in1,HIGH);
    digitalWrite(in2,LOW);
  }
  else if(dir == -1){
    digitalWrite(in1,LOW);
    digitalWrite(in2,HIGH);
  }
  else{
    digitalWrite(in1,LOW);
    digitalWrite(in2,LOW);
  }  
}