CCS C Software and Maintenance Offers
FAQFAQ   FAQForum Help   FAQOfficial CCS Support   SearchSearch  RegisterRegister 

ProfileProfile   Log in to check your private messagesLog in to check your private messages   Log inLog in 

CCS does not monitor this forum on a regular basis.

Please do not post bug reports on this forum. Send them to CCS Technical Support
line follower with pd algorithm

 
Post new topic   Reply to topic    CCS Forum Index -> General CCS C Discussion
View previous topic :: View next topic  
Author Message
fbasaris



Joined: 04 Mar 2011
Posts: 8

View user's profile Send private message

line follower with pd algorithm
PostPosted: Mon Mar 28, 2011 6:52 pm     Reply with quote
Hi everyone,

I am working on line follower robot. I have 8 sensors for tracking line use L298 for driving motors. I use 18f452 20 MHz and PD algorithm in my codes.
If I set motor's speed slowly everything is normal but set to fast it is not follow the line and go out the line. I changed Kp and Kd values but I cannot set.

By the way road is white line is black.
...
11111011 error=+3
11110111 error=+1
11100111 error=0
11101111 error=-1
11011111 error=-3
..


Here is my code..anybody can look my pd routine and say where I am doing mistake?
Code:

.
.
.
char sensor;
int Kp =6;
int Kd=10;

volatile int motor_hiz=100;


signed int oransal_hesapla(void)
{
   signed int errorp=0;
   static signed int ultimo_errorp=0;
   char contador_sensor=0;

   if(((sensor & 0x10) != 0) && ((sensor & 0x08) != 0))
   {
      errorp=0;
      return(0);
    }
if((sensor & 0x80) != 0)
   {
      errorp = -7;
      contador_sensor++;
   }


    if((sensor & 0x40) != 0)
   {
      errorp = -5;
      contador_sensor++;
   }

   if((sensor & 0x20) != 0)
   {
      errorp = -3;
      contador_sensor++;
   }

   if((sensor & 0x10) != 0)
   {
      errorp = -1;
      contador_sensor++;
   }

   if((sensor & 0x08) != 0)
   {
      errorp = 1;
      contador_sensor++;
   }

   if((sensor & 0x04) != 0)
   {
      errorp = 3;
      contador_sensor++;
   }

   if((sensor & 0x02) != 0)
   {
      errorp = 5;
      contador_sensor++;
   }
if((sensor & 0x01) != 0)
   {
      errorp = 7;
      contador_sensor++;
   }
 

   if(contador_sensor != 0)
   {
      errorp = errorp / contador_sensor;
      ultimo_errorp = errorp;
      return(Kp * errorp);
   }
   else
   {
      if(ultimo_errorp < 0)
         errorp = -9;
      else
         errorp = 9;

      ultimo_errorp = errorp;
      return(errorp * Kp);
   }     
}

signed int turevsel_hesapla(void)
{
   
   signed int error = 0;
   static signed int error_old = 0;
   static signed int errord=0;
   static signed int errord_old = 0;
   static int16 tic = 1;  // 1
   static int16 tic_old = 1; //


   int diferencia = 0;

   if(((sensor & 0x10) != 0) && ((sensor & 0x08) != 0))
      error=0;

   else if((sensor & 0x08) != 0)
      error = 1;

   else if((sensor & 0x10) != 0)
      error = -1;

   else if((sensor & 0x04) != 0)
      error = 3;

   else if((sensor & 0x20) != 0)
      error = -3;

   else if((sensor & 0x02) != 0)
      error = 5;

   else if((sensor & 0x40) != 0)
      error = -5;
   else if((sensor & 0x01) != 0)
      error = 7;

   else if((sensor & 0x80) != 0)
      error = -7;

   else
      {
         if (error_old < 0)
            error = -9;
         else if(error_old > 0)
            error = 9;
      }

     if (error == error_old)
   {
      tic = tic + 1;
      if(tic > 1000)
         tic = 1000;
      if(tic > tic_old)
         errord = errord_old/tic;
//      if(tic > tic_old)
//         errord = (errord_old*tic_old)/tic;

   }
   else
   {
      tic++;
      diferencia = error - error_old;
      errord = Kd*(diferencia)/tic;
      errord_old = errord;
      tic_old=tic;
      tic=0;
   }

   error_old = error;
   return(errord);
}

#int_TIMER1
void  TIMER1_isr(void)
{
   set_timer1(60535);
   signed int errort=0;
   signed int proportional = oransal_hesapla();
   signed int derivative = turevsel_hesapla();

   errort = proportional+derivative;
 
   if(errort >100)
      errort = 100;
   if (errort < -100)
      errort =-100;
   
   if(errort>0)
   {
       set_pwm1_duty(motor_hiz - errort);     //right MOTOR slow
       set_pwm2_duty(motor_hiz);              //left MOTOR full     
   }
   else if(errort<0)
   {
       set_pwm1_duty(motor_hiz);              //right MOTOR  full
       set_pwm2_duty(motor_hiz + errort);     //left MOTOR slow     
   }

   else
   {
       set_pwm1_duty(motor_hiz);     //right MOTOR full
       set_pwm2_duty(motor_hiz);     //left MOTOR full

   }
   
   }
   
.
.
.   

void main()
{

   setup_adc_ports(ALL_ANALOG);
   setup_adc(ADC_CLOCK_DIV_32);
   setup_psp(PSP_DISABLED);
   setup_spi(SPI_SS_DISABLED);
   setup_wdt(WDT_OFF);
   setup_timer_0(RTCC_INTERNAL);
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);//1 msn
   setup_timer_2(T2_DIV_BY_1,120,1);//8 Khz
   setup_ccp1(CCP_PWM);
   setup_ccp2(CCP_PWM);
   setup_timer_3(T3_DISABLED|T3_DIV_BY_1);
   
   enable_interrupts(INT_TIMER1);
   enable_interrupts(GLOBAL);
   set_timer1(60535);

.
.
.
. 
   
   for(;;)
   {
   sensor=input_d();
   }
     
   
}
temtronic



Joined: 01 Jul 2010
Posts: 9226
Location: Greensville,Ontario

View user's profile Send private message

PostPosted: Tue Mar 29, 2011 5:44 am     Reply with quote
You've got too much code in the ISR !

Interrupts should be small and fast, setting a few bits(flags) then return to main.

At slow speeds you've just got enough time to make it work, but the faster you go, the less time you have inside the ISR to do all of the code before another interrupt comes along.

So rethink the program(actually you don't need ISR).
suggestion

start of mainloop

Read the sensors port
compare to the 'offcourse' patterns( 'Switch' works great here)
do the PID math...
set the PWM data...

end of mainloop
Display posts from previous:   
Post new topic   Reply to topic    CCS Forum Index -> General CCS C Discussion All times are GMT - 6 Hours
Page 1 of 1

 
Jump to:  
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum


Powered by phpBB © 2001, 2005 phpBB Group