View unanswered posts | View active topics It is currently Sat Aug 17, 2019 10:42 am






Reply to topic  [ 10 posts ] 
line tracking using 2 sensors 
Author Message
Rookie

Joined: Fri Mar 25, 2011 11:45 am
Posts: 13
Post line tracking using 2 sensors
Hello guys,
First I will introduce myself; I am Melvin, a 17-year old dutch student. Last year I won at the robocup junior, the Rescue League (green map, easiest). We had won that league with a program, programmed in Robolab, that was pretty easy.
This year we wanted to participate the rescue arena, therefor we switched to RobotC. Robolab was too limited to do program the things that the arena requires.

We've started with programming a line follow program with 2 sensors. But after 2 weeks programming, the program is still not working. I dont know where exactly the fault is because the program looks really logical. Below you'll see our program with some explanation. Could someone please take a look and help us out? Thanks!

Code:
task main ()
{
  while (true){
    int Left = SensorValue(EyeLeft);                  
    int Right = SensorValue (EyeRight);

    //black = 12;
    //white   = 3;


    if (SensorValue(EyeLeft) == Left && SensorValue(EyeRight) == Right){      // both sensors see white, the robot must go straight
      motor[MotorLeft] = 35;      
      motor[MotorRight] = 35;     }

    if (SensorValue(EyeLeft) > Left ){                  // left sensor sees black, so the robot must go to the right         
      motor[MotorLeft] = -20;
      motor[MotorRight] = 50;      }

    if (SensorValue(EyeRight) > Right){                  // right sensor sees black, so the robot must go to the left
      motor[MotorLeft] = 50;
      motor[MotorRight] = -20;     }
              }
}


Sat Mar 26, 2011 11:01 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: line tracking using 2 sensors
What is the purpose of the variables Left and Right? If they are used as the reference levels of the left and right light sensors while "on the line", then you need to move them outside of your while loop because you are changing them in every loop. In other words, you are re-sampling the references in every loop so chances are, your robot would always go straight even if your are off the line. Assuming white line on black floor and the spacing of your light sensors is such that both will see white line when you are on it. Your code should look something like this:
Code:
int leftThreshold;    //should determine the threshold by calibration
int rightThreshold;  //should determine the threshold by calibration

while (true)
{
    if ((SensorValue(EyeLeft) >= leftThreshold) && (SensorValue[EyeRight] >= rightThreshold))
    {
        //
        // Both sensors see the line, so go straight.
        //
        motor[MotorLeft] = 35;
        motor[MotorRight] = 35;
    }
    else if (SensorValue(EyeLeft) >= leftThreshold)
    {
        //
        // Only left sensor sees the line, we must turn left.
        //
        motor[MotorLeft] = 30;
        motor]MotorRight] = 40;
    }
    else if (SensorValue[EyeRight) >= rightThreshold)
    {
        //
        // Only right sensor sees the line, turn right.
        //
        motor[MotorLeft] = 40;
        motor[MotorRight] = 30;
    }
    else
    {
        //
        // We lost the line, what to do? Stop?
        //
        motor[MotorLeft] = 0;
        motor[MotorRight] = 0;
    }
    wait1Msec(10);
}


Sat Mar 26, 2011 3:19 pm
Profile
Rookie

Joined: Fri Mar 25, 2011 11:45 am
Posts: 13
Post Re: line tracking using 2 sensors
Thanks for the quick response!
Yes I see it know, I made a mistake in my program. The left and right int had indeed actually must be placed outside the while loop.
Is there actually a difference between 'if' and 'else if' ?
I have read on the internet about tracking a line using a pid control, is this a better way than your program?
Thanks in advance!


Sun Mar 27, 2011 10:22 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: line tracking using 2 sensors
Yes, I have library code that used PID control to follow line. PID control allows you to adjust the correction according to how far you are "off course". The further you are off course, the stronger the correction. However, in order to use PID control, you must have the ability to detect how far you are off course. With your arrangement of two sensors, you only have "on the line", "off to the left" and "off to the right". You cannot tell how far you are "off". So PID can't help you. If you have more light sensors, you can tell how far you are off course by determining which light sensor the line is detected at. Imagine if you have an array of 7 light sensors, you are on the line if the center sensor detected the line. You can determine how far off by detecting which sensor sees the line and how far that sensor is from the center sensor. Having said that, it is possible to do PID with two NXT light sensors because they are analog. My line following library is capable of making two physical analog light sensors into 5 virtual digital light sensors. This will give you enough sensors to tell how far you are off course and so you can use PID control. For two light sensors, imagine if you can interpolate/extrapolate the analog values of the two sensors to tell if the line is: at the center between the two sensors, at the center of the left sensor, at the center of the right sensor, off to the left of the left sensor and off to the right of the right sensor. There you have 5 positions. So in essense, you are putting a virtual light sensor in between the two physical light sensor as well as putting a virtual light sensor on either side of the physical light sensors. You can accomplish this because when the line is in between the light sensors, each light sensor will get about half the light reflection, so the analog value will be lower than when the line is right underneath a single sensor. Therefore, you can translate the physical light sensor values as the following:
Code:
Left sensor reads lower brightness: line is to the left of the left sensor
Left sensor reads full brightness: line is right under the left sensor
Both left and right sensors read lower brightness: line is in between the two sensors
Right sensor reads full brightness: line is right under the right sensor
Right sensor reads lower brightness: line is to the right of the right sensor


Sun Mar 27, 2011 2:23 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: line tracking using 2 sensors
Regarding else if, the following code are equivalent. So it is just a more readable way to write the code so that if you have many conditions to test, your code won't indent too deep to the right.
Code:
if (condition 1)
{
    //do something
}
else if (condition 2)
{
    // do something else
}

The above code is logically equivalent to the following code:

if (condition 1)
{
    //do something
}
else
{
    if (condition 2)
    {
        //do something else
    }
}


Sun Mar 27, 2011 2:28 pm
Profile
Rookie

Joined: Fri Mar 25, 2011 11:45 am
Posts: 13
Post Re: line tracking using 2 sensors
Yes I see what you mean and I think, its a better way to track the line. But how do you program such a complex code using this PID? Is it possible for you to send me a sample of your PID program, or a different one similar to it? I just want to see the basic formatting, so I can learn and figure out how to do my own PID program. I know this is a huge favor, but I would be very grateful!
Thanks again


Sun Mar 27, 2011 4:25 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: line tracking using 2 sensors
Yes, that's relatively complex code but if you break it down to smaller modules, it should not be too difficult. First, I made a module that will take an analog sensor and calibrated threshold values and return whether the analog sensor value was above or below the thresholds (i.e mapping the sensor value to level 0, 1 or 2 depending on if it crosses the low or high thresholds). Then, you need a routine that will read the two "sensor levels" and produce a value indicating how far you are off course. Then you take this value and feed it to a generic PID control module that will take an input value and apply the PID algorithm to calculate the PID control output value. PID could be quite complex but for most of the scenarios, you can probably get away with just the P (Proportional) and drop I (Integral) and D (Differential) in PID. Here is an example on how it's done (Disclaimer: I just wrote it off the top of my head, so it is not tested).
Code:
int leftLowThreshold = ???;    //determined by calibration
int leftHighThreshold = ???;   //determined by calibration
int rightLowThreshold = ???;   //determined by calibration
int rightHighThreshold = ???;  //determined by calibration
int Kp;                        //determined by experimentation

int SensorLevel(tSensor sensorId, int lowThreshold, int highThreshold)
{
    int value = SensorValue(sensorId);
    int level;

    if (value < lowThreshold)
    {
        level = 0;
    }
    else if (value < highThreshold)
    {
        level = 1;
    }
    else
    {
        level = 2;
    }

    return level;
}

/**
 * leftLevel rightLevel Value Comment
 *     0          0       99  Lost the line
 *     0          1        3  Far right
 *     0          2        2  Right
 *     1          0       -3  Far left
 *     1          1        0  Center
 *     1          2        1  Near right
 *     2          0       -2  Left
 *     2          1       -1  Near Left
 *     2          2        0  Not probable but will call it center anyway
 */
int GetPIDInput()
{
    int leftLevel = SensorLevel(EyeLeft, leftLowThreshold, leftHighThreshold);
    int rightLevel = SensorLevel(EyeRight, rightLowThreshold, rightHighThreshold);
    int index = leftLevel*3 + rightLevel;
    int inputValue;

    switch (index)
    {
        case 0:
            //(0, 0): Lost the line.
            inputValue = 99;
            break;
        case 1:
            //(0, 1): Far right.
            inputValue = 3;
            break;
        case 2:
            //(0, 2): Right.
            inputValue = 2;
            break;
        case 3:
            //(1, 0): Far left.
            inputValue = -3;
            break;
        case 4:
            //(1, 1): Center
            inputValue = 0;
            break;
        case 5:
            //(1, 2): Near right
            inputValue = 1;
            break;
        case 6:
            //(2, 0): Left
            inputValue = -2;
            break;
        case 7:
            //(2, 1): Near left
            inputValue = -1;
            break;
        case 8:
            //(2, 2): Center
            inputValue = 0;
            break;
    }

    return inputValue;
}         

int CalcPIDOutput(int inputValue, int Kp, int limit)
{
    int outputValue = inputValue*Kp;

    //
    // Limit the output value to within the range of -limit to limit.
    //
    if (outputValue > limit)
    {
        outputValue = limit;
    }
    else if (outputValue < -limit)
    {
        outputValue = -limit;
    }

    return outputValue;
}

task main()
{
    int input;

    while (true)
    {
        input = GetPIDInput();
        if (input == 99)
        {
            // We lost the line, let's stop.
            motor[MotorLeft] = 0;
            motor[MotorRight] = 0;
        }
        else
        {
            output = CalcPIDOutput(input, Kp, 10);
            motor[MotorLeft] = 50 + output;
            motor[MotorRight] = 50 - output;
        }
        wait1Msec(10);
    }
}


Sun Mar 27, 2011 7:05 pm
Profile
Moderator
Moderator
User avatar

Joined: Tue Sep 14, 2010 9:19 pm
Posts: 496
Post Re: line tracking using 2 sensors
I think that MHTS answered your question, but I really think that you should take look at this article : http://www.inpharmix.com/jps/PID_Contro ... obots.html

_________________
sudo rm -rf /


Sun Mar 27, 2011 7:46 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: line tracking using 2 sensors
BTW, just realize I didn't properly answer your question on the difference between if and else if. In the following code, even if the first condition is met and you set both motors to 35, you will still check the second condition that if the sensor value of EyeLeft is greater than Left even though you already know the left eye value is the same as left. So it is a redundant check. "else if" allows you to skip all other checks because the first condition is already met.
Code:
    if (SensorValue(EyeLeft) == Left && SensorValue(EyeRight) == Right){      // both sensors see white, the robot must go straight
      motor[MotorLeft] = 35;     
      motor[MotorRight] = 35;     }

    if (SensorValue(EyeLeft) > Left ){                  // left sensor sees black, so the robot must go to the right         
      motor[MotorLeft] = -20;
      motor[MotorRight] = 50;      }


Sun Mar 27, 2011 8:02 pm
Profile
Rookie

Joined: Fri Mar 25, 2011 11:45 am
Posts: 13
Post Re: line tracking using 2 sensors
okay thank you very much! That certainly will make me get started. Incidentally, that website is very useful too, therefore thanks!


Mon Mar 28, 2011 12:03 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 10 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


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 post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.