... | ... | @@ -119,15 +119,79 @@ |
|
|
>
|
|
|
>---
|
|
|
>
|
|
|
>## Exercise 5: PID line follower
|
|
|
>
|
|
|
>#### Task
|
|
|
>It is possible to make a line follower with just one light sensor that follows the line more smoothly and drive faster if a PID regulator is used?
|
|
|
>
|
|
|
>#### Plan
|
|
|
>
|
|
|
>## Exercise 5: PID Line Follower
|
|
|
>
|
|
|
>####Task
|
|
|
>
|
|
|
>Implement the PID controller as described in [ref].
|
|
|
>
|
|
|
>####Plan
|
|
|
>
|
|
|
>We intend to follow the guide step by step and implement the code into the LineFollowerCal.java code exampel from the tutorial plan. After the implementation we will try adjusting the PID constants to achieve a smooth ride along the line.
|
|
|
>
|
|
|
>####Results
|
|
|
>
|
|
|
>After the implementation the code with our PID controller looked like this:
|
|
|
>´´´
|
|
|
>BlackWhiteSensor sensor = new BlackWhiteSensor(SensorPort.S3);
|
|
|
|
|
|
sensor.calibrate();
|
|
|
|
|
|
LCD.clear();
|
|
|
LCD.drawString("Light: ", 0, 2);
|
|
|
|
|
|
// Variabler til PID
|
|
|
|
|
|
// P led
|
|
|
int Kp = 300; // Proportional controller
|
|
|
int offset = sensor.getThreshold();
|
|
|
int Tp = 50; // Target power. Den verdi der bruges nor error er 0
|
|
|
|
|
|
// I led
|
|
|
int Ki = 0; // Integral constant
|
|
|
int integral = 0;
|
|
|
|
|
|
// D led
|
|
|
int Kd = 0; // Derivative constant
|
|
|
int lastError = 0;
|
|
|
int derivative = 0;
|
|
|
|
|
|
|
|
|
while (! Button.ESCAPE.isDown())
|
|
|
{
|
|
|
|
|
|
LCD.drawInt(sensor.light(),4,10,2);
|
|
|
LCD.refresh();
|
|
|
|
|
|
int LightValue = sensor.light();
|
|
|
int error = LightValue - offset;
|
|
|
|
|
|
integral = integral + error;
|
|
|
|
|
|
derivative = error - lastError;
|
|
|
|
|
|
int Turn = Kp*error + Ki*integral + Kd*derivative;
|
|
|
Turn = Turn/100;
|
|
|
|
|
|
int powerLeft = Tp + Turn;
|
|
|
int powerRight = Tp - Turn;
|
|
|
|
|
|
Car.forward(powerLeft, powerRight);
|
|
|
|
|
|
lastError = error;
|
|
|
|
|
|
Thread.sleep(50);
|
|
|
}
|
|
|
|
|
|
Car.stop();
|
|
|
LCD.clear();
|
|
|
LCD.drawString("Program stopped", 0, 0);
|
|
|
LCD.refresh();
|
|
|
}
|
|
|
|
|
|
>´´´
|
|
|
>
|
|
|
>#### Results
|
|
|
>We started adjusting the numbers for the PID constants but we did not have time to find the desired values before it was time to hand in. Therefore we have not yet fully completed a satisfying PID controlled line follower robot.
|
|
|
>
|
|
|
>---
|
|
|
>
|
... | ... | @@ -169,5 +233,5 @@ |
|
|
> Code:
|
|
|
> * Full code from our programs can be found here: https://drive.google.com/?tab=wo&authuser=0#folders/0B2Ko0NOiIJolaDg2SllGZ2p0TGM
|
|
|
>
|
|
|
>
|
|
|
> |
|
|
\ No newline at end of file |
|
|
> Links:
|
|
|
> [?] http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html |
|
|
\ No newline at end of file |