Changes

Page history
No commit message authored by Jesper Kurtzmann Svith's avatar Jesper Kurtzmann Svith 
No commit message
Show whitespace changes
Inline Side-by-side
Lab4.markdown
View page @ 76903b5d
...@@ -14,7 +14,7 @@ ...@@ -14,7 +14,7 @@
> 3+7+3 hours > 3+7+3 hours
> >
>#### Overall goal >#### 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 @@ ...@@ -134,60 +134,60 @@
>After the implementation the code with our PID controller looked like this: >After the implementation the code with our PID controller looked like this:
>´´´ >´´´
>BlackWhiteSensor sensor = new BlackWhiteSensor(SensorPort.S3); >BlackWhiteSensor sensor = new BlackWhiteSensor(SensorPort.S3);
>
sensor.calibrate(); > sensor.calibrate();
>
LCD.clear(); > LCD.clear();
LCD.drawString("Light: ", 0, 2); > LCD.drawString("Light: ", 0, 2);
>
// Variabler til PID > // Variabler til PID
>
// P led > // P led
int Kp = 300; //Proportional controller > int Kp = 300; //Proportional controller
int offset = sensor.getThreshold(); > int offset = sensor.getThreshold();
int Tp = 50; // Target power. Den verdi der bruges nor error er 0 > int Tp = 50; // Target power. Den verdi der bruges nor error er 0
>
// I led > // I led
int Ki = 0; // Integral constant > int Ki = 0; // Integral constant
int integral = 0; > int integral = 0;
>
// D led > // D led
int Kd = 0; // Derivative constant > int Kd = 0; // Derivative constant
int lastError = 0; > int lastError = 0;
int derivative = 0; > int derivative = 0;
>
>
while (! Button.ESCAPE.isDown()) > while (! Button.ESCAPE.isDown())
{ > {
>
LCD.drawInt(sensor.light(),4,10,2); > LCD.drawInt(sensor.light(),4,10,2);
LCD.refresh(); > LCD.refresh();
>
int LightValue = sensor.light(); > int LightValue = sensor.light();
int error = LightValue - offset; > int error = LightValue - offset;
>
integral = integral + error; > integral = integral + error;
>
derivative = error - lastError; > derivative = error - lastError;
>
int Turn = Kp*error + Ki*integral + Kd*derivative; > int Turn = Kp*error + Ki*integral + Kd*derivative;
Turn = Turn/100; > Turn = Turn/100;
>
int powerLeft = Tp + Turn; > int powerLeft = Tp + Turn;
int powerRight = Tp - Turn; > int powerRight = Tp - Turn;
>
Car.forward(powerLeft, powerRight); > Car.forward(powerLeft, powerRight);
>
lastError = error; > lastError = error;
>
Thread.sleep(50); > Thread.sleep(50);
} > }
>
Car.stop(); > Car.stop();
LCD.clear(); > LCD.clear();
LCD.drawString("Program stopped", 0, 0); > LCD.drawString("Program stopped", 0, 0);
LCD.refresh(); > LCD.refresh();
} > }
> >
>´´´ >´´´
> >
... ...
......