| ... | ... | @@ -14,28 +14,89 @@ The goal of this exercise is to test three different sensors and physical robot |
|
|
|
## Plan
|
|
|
|
The plan is to follow the instructions for Lesson 5 [1].
|
|
|
|
|
|
|
|
## Results
|
|
|
|
|
|
|
|
We have chosen to develop a standard software architecture that can be used in each of the three scenarios (light, color and gyro sensor). This architecture is seen in the following diagram
|
|
|
|
## Exercise 1
|
|
|
|
Self-balancing robot with light sensor
|
|
|
|
|
|
|
|
### Setup
|
|
|
|
|
|
|
|
For this exercise we used a LEGO model build according to the description in [2]. Our final model is shown in the following image.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
We know that the surface which the robot is placed on affects the control mechanism and therefore three different materials are tested. These are a blank white table, a wooden table and a carpet floor which is shown in the following image.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
By inspecting the robots control mechanism we concluded that the best of the three surfaces was the blank white surface and therefore the following tuning and analysis of the PID parameters are performed on this surface.
|
|
|
|
We have created a standard PID software architecture which makes the replacement of a sensor easy. The architecture is shown in the following image.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
In order to simplify the implementation of the software related to the three sensors we have made a generic PID controller with abstract methods to calculate the error and control signal. The specific PID controller will extend this class and define logic for these methods making it easier to manage the code. The program uses the PCconnection class to establish a Bluetooth connection between the NXT and the PC which is used to pass parameters from a GUI on the PC to the NXT. The “Segway” class is the main class which controls the flow of the program by first letting the user chose a balancing point which then is defined as an offset making the set point of the PID controller 0. Afterwards the control algorithm is applied while the parameters are shown on the LCD.
|
|
|
|
The architecture consists of a generic PID controller with abstract methods to calculate the error and control signal. The specific PID controller will extend this class and define logic for these methods. The program uses the PCconnection class to establish a Bluetooth connection between the NXT and the PC which is used to pass parameters from a GUI on the PC to the NXT. The GUI is seen in the following image.
|
|
|
|
|
|
|
|
### Exercise 1, Self-balancing robot with light sensor
|
|
|
|
|
|
|
|
|
|
|
|
For this exercise we used a LEGO model build according to the description in [2]. Our final model is shown in the following picture:
|
|
|
|
The “Segway” class is the main class which controls the flow of the program by first letting the user choose a balancing point which then is defined as an offset making the set point of the PID controller 0. Afterwards the control algorithm is applied while the parameters are shown on the LCD. The core control algorithm is located in the GenericPIDController class and is shown in the following.
|
|
|
|
|
|
|
|
|
|
|
|
```java
|
|
|
|
// Calculate parameters
|
|
|
|
error = calculateError();
|
|
|
|
integral = integral + error;
|
|
|
|
derivative = (error - lastError) / dt;
|
|
|
|
|
|
|
|
We know that the surface which the robot is placed on affects the control mechanism and therefore three different materials is tested. These are a blank white table, a wooden table and a carpet floor which is shown in the following picture.
|
|
|
|
// Reset the the integral part if the error crosses zero
|
|
|
|
if(Math.signum(error) != Math.signum(lastError))
|
|
|
|
integral = 0;
|
|
|
|
|
|
|
|
|
|
|
|
// Apply the PID regulation
|
|
|
|
controlledValue = (kp * error) + (ki * integral) + (kd * derivative);
|
|
|
|
controlSignal(controlledValue);
|
|
|
|
|
|
|
|
By inspecting the robots control mechanism we concluded that the best of the three surfaces was the blank white surface and therefore the following tuning and analysis of the PID paramters are performed on this surface.
|
|
|
|
lastError = error;
|
|
|
|
|
|
|
|
The standard software architecture is used along with the “LightPIDController” class.
|
|
|
|
By repeatedly tuning the parameters through the PC GUI we ended up with the best possible setting of:
|
|
|
|
Delay.msDelay(dt);
|
|
|
|
```
|
|
|
|
|
|
|
|
were the functions `calculateError()` and `controlSignal()` is overriden by the specific PID controller, in this case the `LightPIDController`. The functions are shown in the following.
|
|
|
|
|
|
|
|
```java
|
|
|
|
@Override
|
|
|
|
protected int calculateError() {
|
|
|
|
LCD.drawString("Light value: " + lightsensor.readNormalizedValue() + " ",0,8);
|
|
|
|
log.writeSample(lightsensor.readNormalizedValue());
|
|
|
|
return lightsensor.readNormalizedValue() - offset;
|
|
|
|
}
|
|
|
|
```
|
|
|
|
|
|
|
|
```java
|
|
|
|
@Override
|
|
|
|
protected void controlSignal(float controlledValue) {
|
|
|
|
if (controlledValue > max_power)
|
|
|
|
controlledValue = max_power;
|
|
|
|
if (controlledValue < -max_power)
|
|
|
|
controlledValue = -max_power;
|
|
|
|
|
|
|
|
// Power derived from PID value:
|
|
|
|
int power = Math.abs((int)controlledValue);
|
|
|
|
power = default_power + (power * max_power-default_power) / max_power; // NORMALIZE POWER
|
|
|
|
|
|
|
|
if (controlledValue > 0) {
|
|
|
|
rightMotor.controlMotor(power, BasicMotorPort.FORWARD);
|
|
|
|
leftMotor.controlMotor(power, BasicMotorPort.FORWARD);
|
|
|
|
} else {
|
|
|
|
rightMotor.controlMotor(power, BasicMotorPort.BACKWARD);
|
|
|
|
leftMotor.controlMotor(power, BasicMotorPort.BACKWARD);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
```
|
|
|
|
|
|
|
|
|
|
|
|
The entire code can be seen in [2].
|
|
|
|
|
|
|
|
### Results
|
|
|
|
|
|
|
|
By repeatedly tuning the parameters through the PC GUI we ended up with the best possible setting of the control parameters as:
|
|
|
|
|
|
|
|
| Parameter | Value |
|
|
|
|
| ------------- |:------:|
|
| ... | ... | @@ -45,12 +106,13 @@ By repeatedly tuning the parameters through the PC GUI we ended up with the best |
|
|
|
| Offset | 460 |
|
|
|
|
| Min power | 55 |
|
|
|
|
|
|
|
|
A video of the end result is seen in [XXX]. This video shows that the Lego robot is able to perform self-balancing in short intervals of about 1-2 seconds. In order investigate this behavior the data logger is used to collect the light sensor readings during the execution of the program. The end result of this is seen in the following image.
|
|
|
|
With this configuration the robot was able to self-balance in short intervals of approximately 1-2 seconds as seen in the video in the references section.
|
|
|
|
In order to investigate this behavior the data logger is used to collect the light sensor readings during the execution of the program. The end result of this is seen in the following image.
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
This image shows the PID controllers offset as the red line and the output of the light sensor as the the blue graph.
|
|
|
|
|
|
|
|
This plot shows the PID controllers offset as the red line and the output of the light sensor as the blue graph.
|
|
|
|
When the LEGO robot is tilting forward the light sensor will get closer to the surface which yields less light coming in and therefore the output value of the sensor will decrease. Otherwise when the LEGO robot is tilting backwards more light is coming in and the output value will increase. In order for the LEGO robot to keep balance it must constantly try to keep an upright position by applying motor force in the tilting direction (forward or backward). Due to the LEGO robots high center of gravity it is difficult to maintain an upright position resulting in the toggling back and forth between the offset until it is no longer able to adjust for the tilting.
|
|
|
|
|
|
|
|
|
|
|
|
### Exercise 2, Self-balancing robots with color sensor
|
| ... | ... | @@ -67,7 +129,10 @@ This image shows the PID controllers offset as the red line and the output of th |
|
|
|
## References
|
|
|
|
|
|
|
|
[1] http://legolab.cs.au.dk/DigitalControl.dir/NXT/Lesson5.dir/Lesson.html
|
|
|
|
[2] https://gitlab.au.dk/rene2014/lego/tree/master/Lesson5/Programs/SegwayOnTheFlyNXT
|
|
|
|
|
|
|
|
|
|
|
|
### Videos
|
|
|
|
|
|
|
|
Exerceise 1 - https://www.youtube.com/watch?v=kqeq5SVmsWQ&feature=youtu.be
|
|
|
|
|
