//Motor Channel A pins

const int Dir = 12;

const int Pwm = 3;
const int Brake = 9;

const int EncChAPin = 2;

const int pot= A5;

volatile int counter = 0;

int cpr = 256; //Encoder count per revolution
float lengthPerRev = 1.5; //how long belt travel per motor revolution
int maxPos = 20; //max traval length
int spd = 200; //range 0-255

int Pos;
int ticksToPos;
boolean dir = 0;
int tolerance = 50; //tolerance in ticks
int lastPos = 0;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(Dir, OUTPUT);
pinMode(Pwm, OUTPUT);
pinMode(Brake, OUTPUT);
pinMode(EncChAPin, INPUT);

digitalWrite(Brake, LOW);
attachInterrupt(digitalPinToInterrupt(EncChAPin), count, RISING);

//Set belt to its zero position to start tracking the position

//go to current position to initialize, assume clockwise rotation to go to
current position
Pos = map(analogRead(pot), 0, 1023, 0, maxPos);
Serial.print("Position to go: ");
Serial.println(Pos);
ticksToPos = ((float)Pos/lengthPerRev)*(float)cpr;

dir = 1;
digitalWrite(Dir, dir);
while(!((counter<ticksToPos +tolerance) && (counter>ticksToPos -tolerance))){
analogWrite(Pwm, spd);
}
analogWrite(Pwm, 0);
digitalWrite(Brake, HIGH);
delay(25);
digitalWrite(Brake, LOW);
delay(2000);
Serial.println("Initialized");
}

void loop() {
// put your main code here, to run repeatedly:
Pos = map(analogRead(pot), 0, 1023, 0, maxPos);
ticksToPos = ((float)Pos/lengthPerRev)*(float)cpr;
Serial.print("Position to go: ");
Serial.println(Pos);
 if(abs(lastPos-Pos)>25){
if(counter>ticksToPos )
dir = 0;
else
dir = 1;

digitalWrite(Dir, dir);
while(!((counter<ticksToPos +tolerance) && (counter>ticksToPos -tolerance))){
analogWrite(Pwm, spd);
}

analogWrite(Pwm, 0);
digitalWrite(Brake, HIGH);
delay(25);
digitalWrite(Brake, LOW);
delay(200);
lastPos = Pos;
}

void count(){
if(dir)
counter++;
else
counter--;
}