... | ... | @@ -14,7 +14,7 @@ |
|
|
> 3+7+3 hours
|
|
|
>
|
|
|
>#### Overall goal
|
|
|
>To make the LEGO 9797 car follow a black line and stop at the end of the line. When the car drives into a goal zone - a green rectangular piece of paper at the end of the black line. The idea is that we will try to use the light sensor to detect three different colors.
|
|
|
>To make the LEGO 9797 car follow a black line and stop at the end of the line. When the car drives into a goal zone - a green rectangular piece of paper at the end of the black line. The idea is that we will try to use the light sensor to detect three different colors. Besides that we will attempt to implement a PID controller so that the robot will follow a line more smoothly.
|
|
|
>
|
|
|
>---
|
|
|
>
|
... | ... | @@ -134,60 +134,60 @@ |
|
|
>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();
|
|
|
}
|
|
|
>
|
|
|
> 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();
|
|
|
> }
|
|
|
>
|
|
|
>´´´
|
|
|
>
|
... | ... | |