#define DW(pin,val) digitalWrite(pin,val)
#define DR(pin) digitalRead(pin)
#define AW(pin,val) analogWrite(pin,val)
#define AR(pin) analogRead(pin)
#define S Serial
#define PR(val) print(val)
#define PRLN(val) println(val)
#define PM(pin,mode) pinMode(pin,mode)

#define LMdir 3 //right motor dir pin
#define LMspd 2 //right motor spd pin
#define RMdir 9 //left motor dir pin
#define RMspd 8 //left motor spd pin

#define Forward 0
#define Backward 1

#define spd 30 //change it
#define kp 6 //change it
#define kd 3 //change it
//kp and kd should be positive i think

int linePins[5]={53,51,49,47,45};
int lineVal;

int lastError=0;

void setup()
{
  S.begin(9600);
  PM(RMdir,1);
  PM(RMspd,1);
  PM(LMdir,1);
  PM(LMspd,1);
  for (int i=0;i<5;i++)
  PM(linePins[i],0);
}

void loop()
{
  int error=readLine();
  int add=kp*error+kd*(error-lastError);
  lastError=error;
  int rightSpd=spd-add;
  int leftSpd=spd+add;
  rightSpd=min(255,max(rightSpd,-255));
  leftSpd=min(255,max(leftSpd,-255));
  motors(sign(rightSpd),rightSpd,sign(leftSpd),leftSpd);
}

//functions tap

int readLine()
{
  lineVal=0;
  int tem;
  for (int i=0;i<5;i++)
  {
    tem=DR(linePins[i]);
    lineVal|=(tem<<(4-i));
  }
  return (lineVal&3)-((lineVal>>3)&3);
}

void motors(int d1,int s1,int d2,int s2)
{
  DW(RMdir,d1);
  AW(RMspd,abs(s1));
  DW(LMdir,!d2);
  AW(LMspd,abs(s2));
}

int sign(int x)
{
  return (x>0? Forward:Backward);
}