RoboCatz.com

## Playbook: Line Following -- The Exercise (for robots with 1 color sensor)

Definition: The ability for the robot to follow a line or edge between two shades of colors or areas.

The goal of this exercise is to be able to create the necessary functions to perform the following:

```startTask(FollowLine_Edge);   // Just start following the line
stopTask(FollowLine_Edge);    // Just stop following the line
stopAllMotors();```
Line following programs and functions require the use of a while() loop in which the robot will sense the line and make a determination of how to respond to the input from the sensor(s).

Line following programs typically follow the edge of the line--either the right or the left edge. The robot attempts to keep the light sensor directly over the edge. If the sensor is moved to far away from the line, then the robot attempts to compensate for that by moving the robot toward the line. If the light sensor appears directly over the line, then the robot attempts to compensate by moving the robot toward the edge that it was trying to follow.

In essence, the robot makes a series of quick "zig-zag" motions to keep the light sensor over the edge of the line.

### Requirements

This exercise requires you to be in front of a computer with RobotC and a Mindstorms robot. You will write a program according to the instructions below. Try not to just jump to the end of the lesson and copy the whole program. You won't learn much by doing that. Follow the individual steps shown below. Follow them in order--so that you'll know what to do when you have to write your own programs some day.

Go to the Motors and Sensors setup and configure two motors: B and C. Configure two light sensors on ports 2 and 3 and a touch sensor on port 1.

When configuring motor B, just give it the name "B". When configuring motor C, just give it the name "C".

When configuring touch sensor on port 1, just give it the name "touchSensor".
When configuring color sensor on port 2, just give it the name "colorSensor1".
If you want to configure the Ultrasonic sensor on port 4, call it: ultrasonicSensor

### Start RobotC and access a New program

Start RobotC then click on the New File button from the button bar. The new program should appear as:

```task main()
{

}```
```#pragma config(Sensor, S1,     touchSensor,    sensorEV3_Touch)
#pragma config(Sensor, S2,     colorSensor1,   sensorEV3_Color)
#pragma config(Sensor, S4,     ultrasonicSensor, sensorEV3_Ultrasonic)
#pragma config(Motor,  motorB,          B,             tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          C,             tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//```
When you have finished configuring the motors and sensors, you should see the code above added to the top of your program.

Insert a while() loop as shown below.

```task main()
{

while(true) {
displayCenteredBigTextLine(1, "Sensor %d", SensorValue[colorSensor1]);
}

}```
Now run your program.

Did it work? I hope so. If it didn't, try copying the code below.

```#pragma config(Sensor, S1,     touchSensor,    sensorEV3_Touch)
#pragma config(Sensor, S2,     colorSensor1,   sensorEV3_Color)
#pragma config(Sensor, S4,     ultrasonicSensor, sensorEV3_Ultrasonic)
#pragma config(Motor,  motorB,          B,             tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          C,             tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
while(true) {
displayCenteredBigTextLine(1, "Sensor %d", SensorValue[colorSensor1]);
}

}```
Ok. Move the robot over the line. Look at the values displayed.

What is the range of values?
What is the highest number?
What is the lowest number?

Remember, just use the robot on the board. Don't try getting light readings of the ambient room lighting or the sun or a silver shiny juice box.

Now lets try making the robot move. Make the following changes to the main task:

```task main()
{
int diff;
int defaultPower=20; // Lower this value to slow the robot
while(true) {
int k=20;   // Average between light and dark
diff = SensorValue[colorSensor1] - k;
diff/=3;    // Divide by 3 to reduce wobble
displayCenteredBigTextLine(1, "Sensor %d", SensorValue[colorSensor1]);
displayCenteredBigTextLine(4, "diff %d", diff);
motor[B]=defaultPower-diff;
motor[C]=defaultPower+diff;
}
}```
Run it several times. What is k? What does k represent?

k is a constant. It represents the midpoint of the light range. It is the middle value between the low and high reading you received. You may need to change k in your program accordingly. Set it to the midpoint of the values you received.

The variable diff represents the difference between the current sensor reading and k.

If the range of values for the light sensor go from 10 to 30, the midpoint (or k) would be about 20. If we take the current light reading and subtract k we should end up with a number that is sometimes positive and sometimes negative depending on if the sensor is over the line or over the space next to the line.

Adding the difference to k and assigning that to one motor and then subtracting the difference from k and assigning that to the other motor should enable the two motors to work in opposing speeds with the difference between them greatest when the diff variable is either high or low.

Lets move the while() loop into a separate task. Make the changes below to your program.

```task FollowLine_Edge() {
int diff;
int defaultPower=20; // Lower this value to slow the robot
while(true) {
int k=20;   // Average between light and dark
diff = SensorValue[colorSensor1] - k;
diff/=3;    // Divide by 3 to reduce wobble
displayCenteredBigTextLine(1, "Sensor %d", SensorValue[colorSensor1]);
displayCenteredBigTextLine(4, "diff %d", diff);
motor[B]=defaultPower-diff;
motor[C]=defaultPower+diff;
}
}

task main()
{
startTask(FollowLine);
stopTask(FollowLine);
}```
Run your program now.

Did it follow the line?

Probably not.

Do you know why?

The program was told to stop the FollowLine() task as soon as it was started. Let's add in a delay so that we can see the robot perform the task. Make the changes below to the task main():

```task main()
{
startTask(FollowLine_Edge);
wait(5);
stopTask(FollowLine_Edge);
}```
That's better. Now you can see the robot following the line for 5 seconds and then stopping.

Let's make another change (shown below). What do you think the robot will do now? Try it out and see.

```task main()
{
startTask(FollowLine);
wait(5);
stopTask(FollowLine);
wait(5);
}```
What do you think is going to happen?

The robot followed the line for 5 seconds and then just started doing its own thing for 5 seconds. The reason the robot kept going after the first 5 seconds is that the power has been applied to the motors through the task FollowLine_Edge. When you are done with the task FollowLine_Edge, you need to stop the power going to the motors. Try the changes below.

```task main()
{
startTask(FollowLine_Edge);
wait(5);
stopTask(FollowLine_Edge);
stopAllMotors();
wait(5);
}```
Let's put this in a loop just for fun so that we can see the control we have over the line following task.

```task main()
{
int j;
for(j=1; j<=10; j++) {
startTask(FollowLine_Edge);
wait(5);
stopTask(FollowLine_Edge);
stopAllMotors();
wait(5);
}
}```
As you can see, we can start and stop the line following at will. Now let's get the robot to stop after a certain distance.

Add the following BEFORE the task.

```/************************************************************************************
* User Defined helper functions
*************************************************************************************/
typedef struct genericRobotStruct {
int powerLevel;
int wheelDiameterMM;
int gearRatio;
int distanceBetweenWheelsMM;
bool criteriaNotMetYet;
} robotObj; robotObj robot;

#define PI 3.14159

////////////////////////////////////////////////////////////////////////////////////
//
void untilDistance(int moveIN) {
resetMotorEncoder(B);
resetMotorEncoder(C);
float targetDegree;
targetDegree=(moveIN * 25.4)/(robot.wheelDiameterMM*PI)*360;
repeatUntil(abs(getMotorEncoder(B)) > targetDegree) { sleep(5);	}
}```
Before you can utilize the "untilDistance" function, you will need to initialize the wheelDiameterMM property of robot in the main task. See below.

```task main()
{
robot.wheelDiameterMM = 96;
int j;
for(j=1; j<=10; j++) {
startTask(FollowLine_Edge);
untilDistance(5);
stopTask(FollowLine_Edge);
stopAllMotors();
wait(5);
}
}```
By setting the wheelDiameterMM property of robot, you can then have the correct value calculated in the untilDistance function.

How does the formula use the wheel diameter?

Circumference of the wheel = wheel diameter times PI.
Number of millimeters to travel = number of inches * 25.4
Number of rotations of the wheel = distance travelled / circumference of the wheel.
Number of degrees of rotation = number of rotations * 360

Putting all that together yields an expression like the following:

```///////////////////////////
float targetDegree;
targetDegree=(moveIN * 25.4)/(robot.wheelDiameterMM*PI)*360;

// where moveIN is the distance to move in INCHES
///////////////////////////```
Now the complete program should look like:

```#pragma config(Sensor, S1,     touchSensor,    sensorEV3_Touch)
#pragma config(Sensor, S2,     colorSensor1,   sensorEV3_Color)
#pragma config(Sensor, S4,     ultrasonicSensor, sensorEV3_Ultrasonic)
#pragma config(Motor,  motorB,          B,             tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          C,             tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/************************************************************************************
* User Defined helper functions
*************************************************************************************/
typedef struct genericRobotStruct {
int powerLevel;
int wheelDiameterMM;
int gearRatio;
int distanceBetweenWheelsMM;
bool criteriaNotMetYet;
} robotObj; robotObj robot;

#define PI 3.14159

////////////////////////////////////////////////////////////////////////////////////
//
void untilDistance(int moveIN) {
resetMotorEncoder(B);
resetMotorEncoder(C);
float targetDegree;
targetDegree=(moveIN * 25.4)/(robot.wheelDiameterMM*PI)*360;
repeatUntil(abs(getMotorEncoder(B)) > targetDegree) { sleep(5);	}
}

///////////////////

task FollowLine_Edge() {
int diff;
int defaultPower=20; // Lower this value to slow the robot
while(true) {
int k=20;   // Average between light and dark
diff = SensorValue[colorSensor1] - k;
diff/=3;    // Divide by 3 to reduce wobble
displayCenteredBigTextLine(1, "Sensor %d", SensorValue[colorSensor1]);
displayCenteredBigTextLine(4, "diff %d", diff);
motor[B]=defaultPower-diff;
motor[C]=defaultPower+diff;
}
}

task main()
{
robot.wheelDiameterMM = 96;
int j;
for(j=1; j<=10; j++) {
startTask(FollowLine_Edge);
untilDistance(5);
stopTask(FollowLine_Edge);
stopAllMotors();
wait(5);
}
}

//////////////////////////```