next topic »

[on:]

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-(kp*mod_err + ki*integral - 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);
}

[Reply #1 on:]

my board is Arduino Duemilanove w/ atmega168

[Reply #2 on:]

I don't know why, but edit your post and put code tags around the sketch using the '#' button.

Why do you do this?

Code:

void loop ()
{
  while(1)
  {
...
  }
}

What function does an infinite loop inside an infinite loop serve?

You're using ints, so what does d equal when count > 1?

Code:

    d= 1/count;

What is attached to the pins you are reading?

I suggest you add more debugging prints, and see what is happening while the code is running - I would at least want to know what values err was taking.

[Reply #3 on:]

Suggest combining the reads into a single integer and a switch/case.
Have a look at bitWrite if you're not used to binary arithmetic.
But code tags, mmm yeah, code tags.