go away spammer

Author Topic: Smooth and controlled line tracking code without using PD  (Read 6685 times)

0 Members and 1 Guest are viewing this topic.

Offline tesseract4dTopic starter

  • Beginner
  • *
  • Posts: 6
  • Helpful? 0
Smooth and controlled line tracking code without using PD
« on: February 09, 2010, 06:02:31 AM »
Hello all,

New to robotics and building my first robot. One of the things its got to do is to track a line.

The robot is tracking a line - pretty accurately - but the issue is that it is swinging too much - sometimes enough to throw it off the line. The issue is less when I reduce the speed but thats a compromise I don't want to make.

Onto the specifics.
1. I'm using microchip's PIC18f452, coding in C (C18 compiler). I've posted the source code at the end of the post in red.
2. At a higher speed the swing is very high. But when I set basespeed = 300 and offb = 30 the effect is less - but there is still swing. Its just that it isn't as pronounced at higher speeds.
3. I tried using just 3 sensors , Algorithm (mid sensor - rotate both wheels at 80% duty, left sensor - rotate right wheel at 40 % duty and switch off left wheel, right sensor- vice versa). The swinging effect was less (surprisingly) but the robot seemed to jerk a lot - so I thought I'd drop it.
4. I'm not planning on using encoders for the moment. (Correct me if I'm wrong here - but with encoders you can judge the speed of the motors, which you can use to implement PD and that can reduce the oscillating effect). So if there's a way I can softcode PD (ie try and predict the velocity and hence implement PD) please let me know.
5. I'm building this robot for a competition - and its got a lot of stuff to do. Its got a long line to track so if the linetracking is slow it might bog down its performance.

So any help is appreciated.

regards.



Code: [Select]
[color=red][font=Verdana]
#include <stdio.h>
#include <P18CXXX.h>
#include <adc.h>
#include <timers.h>

#pragma code page  // This code make sure the program starts from 0x0004, not from 0x0000

int lasttrack =0; // variable used to decide which side the motor should rotate if it escapes the track

int trackline(void);
int findline(void);
void OpenTimer2( unsigned char config );
void OpenPWM1( char period );
void OpenPWM2( char period );
void SetDCPWM1( unsigned int dutycycle );
void SetDCPWM2( unsigned int dutycycle );

void main()
{
    int trackstatus;

OpenPWM1(0xff); // init PWM1
    OpenPWM2(0xff); // init PWM1
    OpenTimer2 ( TIMER_INT_OFF & T2_PS_1_16 & T2_POST_1_8 ); // init Timer2


    TRISB = 0b00011111; // Port,pin direction configuration
    PORTB = 0;
    TRISC = 0;
    PORTC = 0;
    TRISCbits.TRISC7 = 1;           // make sure this pin is input


while (1)
{
    trackstatus = trackline();
}
}

// The middle 5 sensors of the tracker are connected from PORTB0 to PORTB4. PORTB0 is connected to the right most and PORTB4 is connected to the left most
// Right Motor is motor1, controlled by RC0 (m1d1), RC3 (m2d2) and PWM1 (RC1)
// Left Motor is motor2, controlled by RC4 (m2d1), RC5 (m2d2) and PWM2 (RC2)



int trackline()
{

    int track, trackin;
    int basespeed=600, offset=0, offb=60;



    while (1)
    {
        trackin = 0b11111111 ^ PORTB; // The inputs from the line sensor are high on white and Low on Black. I'm tracking a black line so the sensors go low when I am on the line.
                                                // The inputs are inverted to a variable 'trackin' so that it makes thinking easier (1- on the line, 0 - off the line)
        track = trackin & 31; //variable 'track' will indicate the status of the line sensors alone - The other 3 pins of the digital input port (PORTB) are to be used for other stuff
                                                // So basically the last 5 bits of the variable 'track' will indicate the status of the line sensors.
                                                // The right most bit corresponds to the outer most sensor on the right, The 5th bit corresponds to the outer most on the left
                                                // and the 3rd bit is the sensor in the centre in the array of line sensors


        switch (track)
{
case 0b00000100:
case 0b00001110: basespeed = 600; offset = 0; break; // Line is in the dead centre of the tracker => being followed properly
case 0b00000001: basespeed = 550; offset = 5; break;
case 0b00000011: basespeed = 550; offset = 4; break;
case 0b00000010: basespeed = 550; offset = 3; break;
case 0b00000110: basespeed = 550; offset = 1; break;
///////////////////////////////////////////////////////////////////////
case 0b00010000: basespeed = 550; offset = -5; break;
case 0b00011000: basespeed = 550; offset = -4; break;
case 0b00001000: basespeed = 550; offset = -3; break;
case 0b00001100: basespeed = 550; offset = -1; break;
default: basespeed = 600; offset = 0;
}


// *********Drive motors according to inferences from variable 'track'**********************

        PORTCbits.RC0 = 1; // Rotate right motor forward
        PORTCbits.RC3 = 0;

        PORTCbits.RC5 = 0;  // Rotate left motor forward
        PORTCbits.RC4 = 1;

        SetDCPWM2(basespeed + offset*offb); // Set duty cycle value for right motor     ;   offb is set earlier in the code, offb=60
        SetDCPWM1(basespeed - offset*offb); // Set duty cycle value for left motor
                                                                // When the argument for SetDCPWM1() is 1023 then it is 100 % duty
//-*********************************************************************************

    }

}[/font][/color]


Offline waltr

  • Supreme Robot
  • *****
  • Posts: 1,944
  • Helpful? 99
Re: Smooth and controlled line tracking code without using PD
« Reply #1 on: February 09, 2010, 07:47:21 AM »
You don't need the motor speed feedback to implement a PD control loop. Apply the PD to the line tracking algorithmic.  Instead of the steps (in the case statement) make these proportional to the time between detecting one edge to the other. Then apply a Derivative term to damp the change.
Here are a couple of links that may help:
http://www.seattlerobotics.org/Encoder/200108/using_a_pid.html
http://www.wa4dsy.net/robot/line/
http://elm-chan.org/works/ltc/report.html

Offline tesseract4dTopic starter

  • Beginner
  • *
  • Posts: 6
  • Helpful? 0
Re: Smooth and controlled line tracking code without using PD
« Reply #2 on: February 09, 2010, 06:01:49 PM »
The issue there I believe is that the effect of discretization might be too significant.

In the sense for while the robot is rotating around 1/5 th of its motion is completed by the time it reaches the next stage of the line sensor. (I'm using 5 line sensors btw). So by the time I reduce the velocity proportional to the previous velocity it might be too late.

Alternatively I'm considering reducing the duty % proportional to the duty % in the previous iteration of the while loop. It is in a way derivative control because I'm reducing the velocity (rate of change of error) proportional to the velocity itself - assuming the the velocity will be proportional to the duty %.

Do you think this might work?

Offline tesseract4dTopic starter

  • Beginner
  • *
  • Posts: 6
  • Helpful? 0
Re: Smooth and controlled line tracking code without using PD
« Reply #3 on: February 10, 2010, 07:48:48 PM »
Any Help?

Offline waltr

  • Supreme Robot
  • *****
  • Posts: 1,944
  • Helpful? 99
Re: Smooth and controlled line tracking code without using PD
« Reply #4 on: February 10, 2010, 08:43:44 PM »
Did you look at the code in the last link I posted, the ELM- Desktop Line Following Robot? Do you watch the two videos? Sounds like you are having the same proble are shown in the first video.
He only has six sensors to detect the line. The code uses a hardware timer (TMR0 in a PIC) and gets the sensor measurements every timer tick at a 9600Hz rate. Then sorts through the sensors measurements to determine how far off of the line center the robot is and how fast it got there (this is the reason for a timer). These are then applied the the PWM duty cycle to speed up or slow down the right or left motor to keep the robot on the line.

I am not fluent with AVR assembler code but did get feel for how the PD control was done by reading his comments.
Your code is in a while() loop. Try thinking of using a fixed time (TMR0) to sample the sensors and adjusting the PWM duty. If you are clever the 5 sensors can be combined into a value to feed into the PD control.

Quote
The swinging effect was less (surprisingly) but the robot seemed to jerk a lot - so I thought I'd drop it.
This is due to your using discrete values for the PWM duty determined by the switch-case.

By using a timer you will have the amount of error in a fixed time interval to apply to the PWM (proportional term). You can also calculate the rate the error is increasing (Derivative term) to also apply to the PWM. Even without the D term you will find the control feed back to be smoother but it may oscillate. Then adjust the Proportional coeffecent and maybe add the Derivative term.

An 18F PIC should do this, even with the code written in C. Question: How fast does the while loop in your code run?

Quote
4. I'm not planning on using encoders for the moment. (Correct me if I'm wrong here - but with encoders you can judge the speed of the motors, which you can use to implement PD and that can reduce the oscillating effect). So if there's a way I can softcode PD (ie try and predict the velocity and hence implement PD) please let me know.

Feedback from the motor/wheel encoders is to control the speed/position of the motor/wheel to what you want the speed or position to be. This will not help keep the robot following a line. The line sensors is the feedback to control the motors. Hence, a PID (or PD) control loop can be implemented.

« Last Edit: February 10, 2010, 09:13:05 PM by waltr »

Offline tesseract4dTopic starter

  • Beginner
  • *
  • Posts: 6
  • Helpful? 0
Re: Smooth and controlled line tracking code without using PD
« Reply #5 on: February 14, 2010, 07:04:40 AM »
Its been a while - didn't see a reply for some time so stopped following the thread.

Quote
He only has six sensors to detect the line. The code uses a hardware timer (TMR0 in a PIC) and gets the sensor measurements every timer tick at a 9600Hz rate. Then sorts through the sensors measurements to determine how far off of the line center the robot is and how fast it got there (this is the reason for a timer). These are then applied the the PWM duty cycle to speed up or slow down the right or left motor to keep the robot on the line.

&

Quote
By using a timer you will have the amount of error in a fixed time interval to apply to the PWM (proportional term).

Don't you think that when you execute a while loop you are actually taking samples at a constant fixed time interval ?

Offline tesseract4dTopic starter

  • Beginner
  • *
  • Posts: 6
  • Helpful? 0
Re: Smooth and controlled line tracking code without using PD
« Reply #6 on: February 14, 2010, 11:12:19 AM »
I took your advice.

I'm using a timer (TMR3) of my PIC18f452 to take samples at regular intervals.

Observe the following
1. My line follower has 5 sensors - each 1 cm apart - so that's a range of 4 cm; And i've got to follow a line that is 2cm in thickness.
2. I've used a timer that will interrupt every ~ 2 ms. So the sampling occurs every 2ms.

I'm pasting my code here. Just let me know if this is the sort of thing you were expecting (NB: I'm not using all the functions defined in the code). If possible let me know if there are any other pitfalls in the code - like the values used for the P & D terms.

Code: [Select]
#include <stdio.h>
#include <P18CXXX.h>
#include <adc.h>
#include <timers.h>

#pragma code page  // This code make sure the program starts from 0x0004, not from 0x0000


// GLOBAL VARIABLES
int t0_int_count = 0, t1_int_count =0;
int timer1_start =0, timer0_start = 0, timer3_start =61000;
char mode = 'f';
char track=0, trackin = 255, lasttrack=0, lasttrack_count = 0;         // Variables that will maintain the status of the line following sensors and aid in tracking the line;
char trackstatus = 0;
char esctrack =0;                                // variable used to decide which side the motor should rotate if it escapes the track
int offb=0, last_offb=0, pd_basespeed = 10000;      // Variables controlling the derivative term
/////////////////////////////////////////


// FUNCTION PROTOTYPES
//////////////////////////////////////////
void OpenTimer2( unsigned char config );
void OpenPWM1( char period );
void OpenPWM2( char period );
void SetDCPWM1( unsigned int dutycycle );
void SetDCPWM2( unsigned int dutycycle );
void OpenADC( unsigned char config,unsigned char config2 );
void ConvertADC( void );
void WriteTimer0( unsigned int  timer );
void WriteTimer1( unsigned int  timer );
void WriteTimer3( unsigned int  timer );
///////////////////////////////////////////
void timer0init_enc(void); void timer0_isr(void);
void timer1init_enc(void); void timer1_isr(void);
void timer3init_line(void); void timer3_isr(void); void timer3tmnt_line(void);
void modules_initialize(void);
void ports_initialize(void);
void startmotorfwd(int max);
int followline(void);
void findline(void);
void trackline(void);
/////////////////////////////////////////
void high_isr(void);
////////////////////////////////////////




void main(void)
{
    ports_initialize();
    modules_initialize();

    followline();                   // Follow the line


    while (1) {}
}



//--------------------------------------------------------------------
// Misc Functions

void startmotorfwd(int max)                    // to start the motor without any jerk
{
    int i,n;

            PORTCbits.RC0 = 1;    // Start right motor
            PORTCbits.RC3 = 0;
            PORTCbits.RC5 = 0;  // Start left motor
            PORTCbits.RC4 = 1;
   
 for (i=0; i<max; i++)
    {
            SetDCPWM2(i);    // Set duty cycle value for PWM2
            SetDCPWM1(i);    // Set duty cycle value for PWM1

            for (n=0; n<=1500; n++)                // Create a delay so that the motor has enough time to start.
            {
                _asm
                nop
                _endasm
            }
    }
}


int followline(void)                        // will start up and initialize timer3 in order to track the line
{

    trackin = 255 ^ PORTB;  track = 31 & trackin;           // Now track reads the current position of the line follower
   
   // if (track == 0) findline();                             // First find the line if you are not on the track

    timer3init_line();
    trackstatus = 0;
    lasttrack_count = 0;

    PORTCbits.RC0 = 1; PORTCbits.RC3 = 0;                   // Rotate both motors forward
    PORTCbits.RC4 = 1; PORTCbits.RC5 = 0;
    SetDCPWM1(400);                                            // Initial speed to start up motors
    SetDCPWM2(400);

    while (!trackstatus)
    {
        trackin = 255 ^ PORTB;  track = 31 & trackin;           // Now track reads the current position of the line follower
        esctrack = track;

        // Escape conditions: If any of the following conditions are encountered stop following the track;
        if(PORTBbits.RB5 == 0 & track != 0) trackstatus = 'r';
        if(PORTBbits.RB6 == 0 & track != 0) trackstatus = 'l';
        if(track == 0)                      trackstatus = 'n';

    }

    timer3tmnt_line();
}



void trackline(void)
{
    int rightspeed=0, leftspeed=0;                      // Variables to assign values to the two speeds
    int rbs, lbs, offset = 100;                         // Variables controlling the proportional term


         switch (track)
    {
        case 0b00000100:
        case 0b00001110:    lbs = 4*offset; rbs = 4*offset;  break;            // Line is in the dead centre of the tracker => being followed properly

        case 0b00000001:    lbs = 4*offset; rbs = 0;         break;

        case 0b00000011:     lbs = 4*offset; rbs = offset;    break;

        case 0b00000111:
        case 0b00000010:     lbs = 4*offset; rbs = 2*offset;  break;

        case 0b00000110:    lbs = 4*offset; rbs = 3*offset;  break;
    ///////////////////////////////////////////////////////////////////////

        case 0b00010000:    rbs = 4*offset; lbs = 0;         break;

        case 0b00011000:     rbs = 4*offset; lbs = offset;    break;

        case 0b00011100:
        case 0b00001000:     rbs = 4*offset; lbs = 2*offset;  break;

        case 0b00001100:    rbs = 4*offset; lbs = 3*offset;  break;

        default:            rbs = 100; lbs = 100;
    }


    if (rbs < lbs) offb = 4 - rbs/offset;
    else if (lbs < rbs) offb = -(4 - lbs/offset);

    lasttrack_count ++;

    /* eg:
    If track = 0b00011000 at one point and after 250 interrupts of timer3 track = 0b00001000
    offb = -(3-2) = -1;         lasttrack_count = 250;
    rbs = 400;                  lbs = 200;

    rightspeed = 400 + (-1)*10000/250 = 360;            // See below
    leftspeed = 200 - (-1)*10000/250 = 240;

    Similarly had the same transition occurred at a slower speed such that it took 400 interrupts of timer3
    rightspeed = 400 + (-1)*10000/400 = 380;            // See below
    leftspeed = 200 - (-1)*10000/400 = 220;
    */

    if (last_offb != offb)                                                  // Execute this loop iff the status of the line tracker has changed
    {
        rightspeed = rbs + (last_offb - offb)*pd_basespeed/lasttrack_count;
        leftspeed = lbs - (last_offb - offb)*pd_basespeed/lasttrack_count;
        SetDCPWM1(leftspeed);
        SetDCPWM2(rightspeed);

        lasttrack_count=0;
    }


    lasttrack = track;
    last_offb = offb;
}
// Misc Functions .......
//---------------------------------------------------------------------





//--------------------------------------------------------------------
//  Functions to set up modules for operation

void timer0init_enc(void)                   // initialize timer0 to receive inputs from the encoder in the right wheel
{
    TRISAbits.TRISA4 = 1;                                                    // Ensure that RA4/T0CKI is an input
    //INTCON2bits.TMR0IP = 1;                                                // High interrupt priority - not necessary if
    OpenTimer0( TIMER_INT_ON & T0_16BIT & T0_SOURCE_EXT & T0_PS_1_1 );
    t0_int_count = 0;
}

void timer1init_enc(void)                   // initialize timer1 to receive inputs from the encoder in the left wheel
{
    TRISCbits.TRISC0 = 1;
    //IPR1bits.TMR1IP = 1;
    OpenTimer0( TIMER_INT_ON & T1_16BIT_RW & T1_SOURCE_EXT & T0_PS_1_1 );
    t1_int_count = 0;
}

void timer3init_line(void)
{
    OpenTimer3( TIMER_INT_ON & T3_16BIT_RW & T3_SOURCE_INT & T3_PS_1_1 );
    WriteTimer3(timer3_start);
}

void timer3tmnt_line(void)
{
    CloseTimer3();
}
//  Functions to set up modules for operation.......
//--------------------------------------------------------------------





//------------------------------------------------------------------
//  Startup initialization functions

void modules_initialize(void)
{
    OpenPWM1(0xff);                                                                    // initialize PWM
    OpenPWM2(0xff);                                                                    //
    OpenTimer2 ( TIMER_INT_OFF & T2_PS_1_16 & T2_POST_1_8 );                        //

    OpenADC (ADC_FOSC_32 & ADC_RIGHT_JUST & ADC_5ANA_0REF, ADC_CH0 & ADC_INT_OFF ); // initialize ADC

    RCONbits.IPEN = 0;                // Disable interrupt priority                    // initialize Timer Interrupts
    INTCONbits.GIE = 1;              // enable Global interrupts for the moment
    INTCONbits.PEIE = 1;            // enable Peripheral interrupts for the moment
}


void ports_initialize(void)
{
    TRISB = 255;
    PORTB = 0;

    TRISC = 0;
    PORTC = 0;
    TRISCbits.TRISC7 = 1;           // make sure this pin is an input

    TRISA = 0b00011111;
    PORTA = 0;

    TRISD = 0b11111100;
    PORTD = 0;
}

//  Startup initialization functions....
//-------------------------------------------------------------------





//---------------------------------------------------------------------
//    Functions handling interrupts

#pragma code high_vector=0x08
void interrupt_at_high_vector(void)
{
  _asm
    GOTO high_isr
  _endasm
}
#pragma code /* return to the default code section */


#pragma interrupt high_isr
void high_isr (void)
{
    if (INTCONbits.TMR0IF == 1) timer0_isr();       // timer0 interrupt
    if (PIR1bits.TMR1IF == 1) timer1_isr();         // timer1 interrupt
    if (PIR2bits.TMR3IF == 1) timer3_isr();         // timer3 interrupt
}

//    Functions handling interrupts........
//----------------------------------------------------------------------



//-----------------------------------------------------------------------
//    ISR's

void timer0_isr(void)
{


    WriteTimer0(timer1_start);
    INTCONbits.TMR0IF = 0;                  // Clear flag before RETFIE
}

void timer1_isr(void)
{


    WriteTimer1(timer0_start);
    PIR1bits.TMR1IF = 0;                    // Clear flag before RETFIE
}

void timer3_isr(void)
{
    trackline();
    WriteTimer3(timer3_start);
    PIR2bits.TMR3IF = 0;
}
// ISR's .......
// -----------------------------------------------------------------------



Offline waltr

  • Supreme Robot
  • *****
  • Posts: 1,944
  • Helpful? 99
Re: Smooth and controlled line tracking code without using PD
« Reply #7 on: February 14, 2010, 12:40:52 PM »
A took a quick look through the code and it seems ok. Adjusting the current speed base on the line sensors is better than the way you first did it.

How is it working?

Offline Admin

  • Administrator
  • Supreme Robot
  • *****
  • Posts: 11,703
  • Helpful? 173
    • Society of Robots
Re: Smooth and controlled line tracking code without using PD
« Reply #8 on: February 14, 2010, 10:15:33 PM »
I use fuzzy logic on all of my robots, including my line following robots. Smooth for me, no PID needed. :)

Offline tesseract4dTopic starter

  • Beginner
  • *
  • Posts: 6
  • Helpful? 0
Re: Smooth and controlled line tracking code without using PD
« Reply #9 on: February 15, 2010, 09:20:23 AM »
@ Waltr:
No. Not working properly. Believe I need to calibrate it - but I can't even get moving in the right direction. Any help?

@ Admin:
How do you use fuzzy logic. Is it something I can learn and implement within a couple of days?

Offline waltr

  • Supreme Robot
  • *****
  • Posts: 1,944
  • Helpful? 99
Re: Smooth and controlled line tracking code without using PD
« Reply #10 on: February 15, 2010, 09:57:08 AM »
You probably need to 'tune' the coefficients for the P & D terms. One part of your code I didn't follow is in the switch: case statement. You may wish to double check that the sensors farther from the center have larger weights and in the correct direction (I've gotten that wrong before).

Do you have a debugger for that PIC? PICKit2 like? If not set up the UART and send out the pwm speed, error and correction values to a text file. Then import into Excel and plot them verse time. This can tell you much about how the control loop is acting.
Hanging a serial cable off your bot does work for testing.

Offline Admin

  • Administrator
  • Supreme Robot
  • *****
  • Posts: 11,703
  • Helpful? 173
    • Society of Robots
Re: Smooth and controlled line tracking code without using PD
« Reply #11 on: February 16, 2010, 12:00:04 AM »
@ Admin:
How do you use fuzzy logic. Is it something I can learn and implement within a couple of days?
;D
http://www.societyofrobots.com/programming_fuzzy_logic.shtml

 


Get Your Ad Here