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).
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
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.
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?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.
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.
/************************************************************************************
* 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.
/////////////////////////// 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);
}
}
//////////////////////////