int kp = 50;
int ki = 5 ;
int kd = 5;
int integral = 0;
int d = 0;
int output;
int preverr = 0;
int err = 0;
int change;
int count = 0;
int right_speed,left_speed;
int mod_err;
void setup ()
{
Serial.begin(9600);
pinMode(2,INPUT);
pinMode(3,INPUT);
pinMode(4,INPUT);
pinMode(5,INPUT);
pinMode(6,INPUT);
pinMode(10,OUTPUT);
pinMode(11,OUTPUT);
right_speed = 0;
left_speed = 0;
}
void loop ()
{
while(1)
{
analogWrite(10,left_speed);
analogWrite(11,right_speed);
count++;
/*
00100 = 0
00110 = 1 01100 = -1
00010 = 2 01000 = -2
00011 = 3 11000 = -3
00001 = 4 10000 = -4
00000 = 5/-5 depending on preverr */
if((!digitalRead(2))&&(!digitalRead(3))&&(digitalRead(4))&&(!digitalRead(5))&&(!digitalRead(6)))
{
err = 0;
integral = 0;
}
else if((!digitalRead(2))&&(!digitalRead(3))&&(digitalRead(4))&&(digitalRead(5))&&(!digitalRead(6)))
{
err = 1;
}
else if((!digitalRead(2))&&(!digitalRead(3))&&(!digitalRead(4))&&(digitalRead(5))&&(!digitalRead(6)))
{
err = 2;
}
else if((!digitalRead(2))&&(!digitalRead(3))&&(!digitalRead(4))&&(digitalRead(5))&&(digitalRead(6)))
{
err = 3;
}
else if((!digitalRead(2))&&(!digitalRead(3))&&(!digitalRead(4))&&(!digitalRead(5))&&(digitalRead(6)))
{
err = 4;
}
else if((!digitalRead(2))&&(!digitalRead(3))&&(!digitalRead(4))&&(!digitalRead(5))&&(!digitalRead(6)))
{
if (preverr = 0)
{err = 0;}
else if(preverr > 0)
{err = 5;}
else if (preverr < 0)
{err = -5;}
}
else if((!digitalRead(2))&&(digitalRead(3))&&(digitalRead(4))&&(!digitalRead(5))&&(!digitalRead(6)))
{
err = -1;
}
else if((!digitalRead(2))&&(digitalRead(3))&&(!digitalRead(4))&&(!digitalRead(5))&&(!digitalRead(6)))
{
err = -2;
}
else if((digitalRead(2))&&(digitalRead(3))&&(!digitalRead(4))&&(!digitalRead(5))&&(!digitalRead(6)))
{
err = -3;
}
else if((digitalRead(2))&&(!digitalRead(3))&&(!digitalRead(4))&&(!digitalRead(5))&&(!digitalRead(6)))
{
err = -4;
}
change = err - preverr;
if(err > 0)
{
if ( change < 0 )
{
d= 1/count;
count = 0;
}
else if (change > 0)
{
d = -(1/count);
count = 0;
}
}
else if(err < 0)
{
if ( change > 0 )
{
d= 1/count;
count = 0;
}
else if (change < 0)
{
d = -(1/count);
count = 0;
}
}
if (change == 0){d=0;}
if (err>0)mod_err = err;
else mod_err = -err;
integral = integral + mod_err;
output = 255-(kpmod_err + kiintegral - kd*d);
MotorControl(output,err);
preverr = err;
}
}
void MotorControl(int output,int err)
{
int k;
k = output;
if(k<0){k=0;}
else if(k>255){k = 255; }
if ( err = 0)
{
right_speed = 255;
left_speed = 255;
}
else
{
if ( err > 0 )
{
left_speed = 255;
right_speed = k;
}
else if (err < 0)
{
right_speed = 255;
left_speed = k;
}
}Serial.println(k);
}