... | @@ -55,18 +55,28 @@ The program for this exercise implements a standard PID controller in the follow |
... | @@ -55,18 +55,28 @@ The program for this exercise implements a standard PID controller in the follow |
|
|
|
|
|
```java
|
|
```java
|
|
// Calculate parameters
|
|
// Calculate parameters
|
|
error = calculateError();
|
|
error = calculateError();
|
|
integral = integral + error;
|
|
integral = integral + error;
|
|
derivative = (error - lastError) / dt;
|
|
derivative = (error - lastError) / dt;
|
|
|
|
|
|
// Apply the PID regulation
|
|
// Apply the PID regulation
|
|
controlledValue = (kp * error) + (ki * integral) + (kd * derivative);
|
|
controlledValue = (kp * error) + (ki * integral) + (kd * derivative);
|
|
|
|
controlSignal(controlledValue);
|
|
|
|
|
|
controlSignal(controlledValue);
|
|
lastError = error;
|
|
|
|
Delay.msDelay(dt);
|
|
lastError = error;
|
|
````
|
|
|
|
where the function controlSignal controls the power of the two motors in the following way
|
|
Delay.msDelay(dt);
|
|
```java
|
|
|
|
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.
|
|
|
|
|
|
### Exercise 6, Color Sensor
|
|
### Exercise 6, Color Sensor
|
|
|
|
|
... | | ... | |