Go Down

Topic: why my motors not running in this program. (Read 747 times) previous topic - next topic

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);
}

my board is Arduino Duemilanove w/ atmega168

dxw00d

#2
Mar 11, 2012, 10:30 am Last Edit: Mar 11, 2012, 07:24 pm by dxw00d Reason: 1
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: [Select]
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: [Select]
    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.

AWOL

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.
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

Go Up
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy