... | ... | @@ -34,7 +34,7 @@ else if(sensor.white()) |
|
|
color = white;
|
|
|
else
|
|
|
color = undefined;
|
|
|
````
|
|
|
```
|
|
|
|
|
|
### Exercise 2, Line Follower with Calibration
|
|
|
|
... | ... | @@ -47,9 +47,90 @@ When placed on the right hand side of the black line the car turned once around |
|
|
So in order for this application to work properly, the car had to start of the left hand side of the line.
|
|
|
|
|
|
### Exercise 3, ThreeColorSensor with Calibration
|
|
|
Based on the BlackWhiteSensor class, a ThreeColorSenor class was made, which could calibrate the light sensor to distinguish between white, black and green.
|
|
|
The full source code is found in [3].
|
|
|
The calibration function was extended to include two thresholds as shown in the code:
|
|
|
|
|
|
```java
|
|
|
public void calibrate() {
|
|
|
blackLightValue = read("black");
|
|
|
greenLightValue = read("green");
|
|
|
whiteLightValue = read("white");
|
|
|
|
|
|
// The threshold is calculated as the median between
|
|
|
// the two readings over the two types of surfaces
|
|
|
blackGreenThreshold = (blackLightValue + greenLightValue) / 2;
|
|
|
greenWhiteThreshold = (greenLightValue + whiteLightValue) / 2;
|
|
|
}
|
|
|
```
|
|
|
The two thresholds where then used to determine the color measured by the light sensor. In the following code snippet all functionality concerning green colors are shown.
|
|
|
|
|
|
```java
|
|
|
public boolean green(){
|
|
|
return green(light());
|
|
|
}
|
|
|
|
|
|
private boolean green(int lightValue)
|
|
|
{
|
|
|
return lightValue > blackGreenThreshold &&
|
|
|
lightValue < greenWhiteThreshold;
|
|
|
}
|
|
|
|
|
|
public int light() {
|
|
|
return ls.readValue();
|
|
|
}
|
|
|
|
|
|
public Color getColor()
|
|
|
{
|
|
|
int lightValue = light();
|
|
|
|
|
|
if (black(lightValue))
|
|
|
return Color.BLACK;
|
|
|
else if (green(lightValue))
|
|
|
return Color.GREEN;
|
|
|
else // if (white(lightValue)) //all cases covered, closest match is white.
|
|
|
return Color.WHITE;
|
|
|
}
|
|
|
```
|
|
|
|
|
|
### Exercise 4, Line Follower that stops in a goal zone
|
|
|
Using the ThreeColorSensor class from exercise 3, a simple line follower that stops in a green goal zone was made.
|
|
|
The full source code is in [4].
|
|
|
In the following code snippet the main loop of the application is shown.
|
|
|
The main idea is to use the ThreeColorSensor class, to measure what the current color is, and then decide what to do.
|
|
|
In order for the car not to stop on a transition from black to white (or white to black), a counter was added, to indicate how long time the color is green, or at least for how many samples in a row the color is green.
|
|
|
The car was set to stop only if the green color was sampled 20 times in a row.
|
|
|
This means that the color should be green in $20 * 10 ms = 200 ms$ before the car stops.
|
|
|
|
|
|
A video of the exercise is found in [5].
|
|
|
|
|
|
```java
|
|
|
int greenCount = 0;
|
|
|
...
|
|
|
while (!Button.ESCAPE.isDown())
|
|
|
{
|
|
|
LCD.drawInt(sensor.light(),4,10,2);
|
|
|
LCD.refresh();
|
|
|
|
|
|
if ( sensor.black() ) {
|
|
|
Car.forward(power, 0);
|
|
|
greenCount = 0;
|
|
|
}
|
|
|
else if (sensor.white()) {
|
|
|
Car.forward(0, power);
|
|
|
greenCount = 0;
|
|
|
}
|
|
|
else { // if(sensor.green())
|
|
|
greenCount++;
|
|
|
if(greenCount > stopThreshold) {
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
try{
|
|
|
Thread.sleep(10);
|
|
|
} catch(InterruptedException e){}
|
|
|
}
|
|
|
```
|
|
|
### Exercise 5, PID Line Follower
|
|
|
The program for this exercise implements a standard PID controller in the following way
|
|
|
|
... | ... | @@ -65,7 +146,7 @@ controlSignal(controlledValue); |
|
|
|
|
|
lastError = error;
|
|
|
Delay.msDelay(dt);
|
|
|
````
|
|
|
```
|
|
|
|
|
|
were the function `calculateError()` calculates the error as the difference between the measured light and the given set point. The set point for this application is determined by the light sensor value when it is placed exactly between a white and a black surface. In this case the set point is 48. The function `calculateError()` is seen below
|
|
|
|
... | ... | @@ -74,7 +155,7 @@ protected int calculateError() { |
|
|
LCD.drawString("Light value: " + lightsensor.readValue() + " ",0,6);
|
|
|
return setpoint - lightsensor.readValue();
|
|
|
}
|
|
|
````
|
|
|
```
|
|
|
|
|
|
The function `controlSignal()` controls the power of the two motors in the following way
|
|
|
|
... | ... | @@ -83,13 +164,13 @@ protected void controlSignal(float controlledValue) { |
|
|
leftMotor.controlMotor((int)(Tp + controlledValue),1);
|
|
|
rightMotor.controlMotor((int)(Tp - controlledValue),1);
|
|
|
}
|
|
|
````
|
|
|
```
|
|
|
|
|
|
In order to avoid a bang-bang control mechanism the two motors are applied a standard power `Tp`.
|
|
|
|
|
|
The control algorithm is wrapped in an LeJOS NXT application that is able to communicate with a PC running a LeJOS GUI application letting us choose values for `kp`,`ki`,`kd` and `Tp` remotely. Due to the complexity of the physical world many factors affect the controlling of the Lego car and it is therefore beneficial to modify control parameters on the fly.
|
|
|
|
|
|
By repeatly tuning the paramters we ended up with the optimal setting of:
|
|
|
By repeatedly tuning the parameters we ended up with the optimal setting of:
|
|
|
|
|
|
|
|
|
| Parameter | Value |
|
... | ... | @@ -99,7 +180,7 @@ By repeatly tuning the paramters we ended up with the optimal setting of: |
|
|
| Kd | 0 |
|
|
|
| Tp | 90 |
|
|
|
|
|
|
A video of the end result is seen here: http://youtu.be/Io7r-IV0Q4Q. This video shows the LEGO car following the black oval line. Even though the car do not deviate from the line the motion could be more smooth. Given the timeframe of the exercise we did not have time to tune the setting or the paramters further. The complete code for this exercise is found in [2]
|
|
|
A video of the end result is seen in [6]. This video shows the LEGO car following the black oval line. Even though the car do not deviate from the line the motion could be more smooth. Given the timeframe of the exercise we did not have time to tune the setting or the parameters further. The complete code for this exercise is found in [2]
|
|
|
|
|
|
### Exercise 6, Color Sensor
|
|
|
|
... | ... | @@ -123,8 +204,12 @@ In addition to this, it also seems to be dependent on the curvature of the targe |
|
|
|
|
|
[2] https://gitlab.au.dk/rene2014/lego/tree/master/Lesson4/Programs/exercise5PIDLineFollower
|
|
|
|
|
|
[3] https://gitlab.au.dk/rene2014/lego/raw/master/Lesson4/Programs/GoalLineFollower/src/ThreeColorSensor.java
|
|
|
|
|
|
[4] https://gitlab.au.dk/rene2014/lego/raw/master/Lesson4/Programs/GoalLineFollower/src/GoalLineFollower.java
|
|
|
|
|
|
### Videos
|
|
|
|
|
|
Exercise 4: http://youtu.be/D2NPmQZRgN8
|
|
|
[5] Exercise 4: http://youtu.be/D2NPmQZRgN8
|
|
|
|
|
|
Exercise 5: http://youtu.be/Io7r-IV0Q4Q |
|
|
[6] Exercise 5: http://youtu.be/Io7r-IV0Q4Q |