Pages: [1]   Go Down
Author Topic: why my motors not running in this program.  (Read 639 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 6
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Offline Offline
Newbie
*
Karma: 0
Posts: 6
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

my board is Arduino Duemilanove w/ atmega168
Logged

Gosport, UK
Offline Offline
Faraday Member
**
Karma: 21
Posts: 3113
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
« Last Edit: March 11, 2012, 01:24:30 pm by dxw00d » Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 287
Posts: 25682
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

"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.

Pages: [1]   Go Up
Jump to: