... | ... | @@ -94,6 +94,73 @@ We are going to take inspiration from the pseudo code in: “A PID Controller Fo |
|
|
|
|
|
#### Results
|
|
|
|
|
|
```
|
|
|
public class PIDCarController {
|
|
|
private static LightSensor light = new LightSensor(SensorPort.S3);
|
|
|
|
|
|
public static void main(String[] args) throws Exception {
|
|
|
final int offset = 37;
|
|
|
double Kp = 2.6;
|
|
|
double Ki = 0.12 ;
|
|
|
double Kd = 120 ;
|
|
|
final int tp = 80;
|
|
|
int integral = 0;
|
|
|
int lastError = 0;
|
|
|
int derivative = 0;
|
|
|
int error = 0;
|
|
|
int turn;
|
|
|
|
|
|
LCD.clear();
|
|
|
LCD.drawString("Light: ", 0, 2);
|
|
|
LCD.drawString("Turn: ", 0, 4);
|
|
|
LCD.drawString("PowerA: ", 0, 5);
|
|
|
LCD.drawString("PowerC: ", 0, 6);
|
|
|
|
|
|
while (! Button.ESCAPE.isDown())
|
|
|
{
|
|
|
|
|
|
LCD.drawInt(LightValue,4,10,2);
|
|
|
LCD.drawInt(error,4,10,3);
|
|
|
LCD.refresh();
|
|
|
|
|
|
int LightValue = light.readValue();
|
|
|
error = LightValue - offset;
|
|
|
|
|
|
|
|
|
derivative = error - lastError ;
|
|
|
|
|
|
// To avoid wind up and high integral value, we reset it to zero if the error equals
|
|
|
// zero or if the error changes operational sign
|
|
|
if (Math.signum(error) != Math.signum(lastError) || error == 0 ){
|
|
|
|
|
|
integral= 0;
|
|
|
integral = integral + error; }
|
|
|
|
|
|
// Here we calculate the value for the turn
|
|
|
turn = (int) ((Kp * error) + (Ki * integral) + (Kd * derivative));
|
|
|
|
|
|
// Instantiates the power for the two motors
|
|
|
int PowerA = tp + turn;
|
|
|
int PowerC = tp - turn;
|
|
|
Car.forward(PowerA,PowerC);
|
|
|
|
|
|
LCD.drawInt(turn,4,10,4);
|
|
|
LCD.drawInt(PowerA,4,10,5);
|
|
|
LCD.drawInt(PowerC,4,10,6);
|
|
|
|
|
|
// Saves the current error so it can be the lastError next time
|
|
|
lastError = error;
|
|
|
Thread.sleep(5);
|
|
|
}
|
|
|
Car.stop();
|
|
|
LCD.clear();
|
|
|
LCD.drawString("Program stopped", 0, 0);
|
|
|
LCD.refresh();
|
|
|
}
|
|
|
}
|
|
|
```
|
|
|
##### CodeSnippet1: The implement code in our PID controlled line follower program.
|
|
|
|
|
|
#### Conclusion
|
|
|
|
|
|
|
... | ... | @@ -112,7 +179,9 @@ Lastly, we will check if it Is possible to use the color sensor for both followi |
|
|
### Overall Conclusion
|
|
|
|
|
|
### References
|
|
|
1. Sourcecode for the programs in this lesson can be found in [Lesson4programs.zip.](http://legolab.cs.au.dk/DigitalControl.dir/NXT/Lesson4.dir/Lesson4programs.zip)
|
|
|
1. The sourcecode for the programs, in this lesson, can be found in [Lesson4programs.zip.](http://legolab.cs.au.dk/DigitalControl.dir/NXT/Lesson4.dir/Lesson4programs.zip)
|
|
|
2. [A PID Controller For Lego Mindstorms Robots.](http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html)
|
|
|
3. [Video 1: LineFollowerCal.java[1] test.](http://www.youtube.com/watch?v=lHlDTTMl_9s)
|
|
|
4. [Video 2: Car following line and stops in green zone.](http://www.youtube.com/watch?v=a1tTYuLr3_0) |
|
|
\ No newline at end of file |
|
|
4. [Video 2: Car following line and stops in green zone.](http://www.youtube.com/watch?v=a1tTYuLr3_0)
|
|
|
5. [Video 3: Line Follower with PID controller.] (https://www.youtube.com/watch?v=Qi-XYG67Eiw&feature=youtu.be)
|
|
|
6. [CodeSnippet1: The implement code in our PID controlled line follower program.](?????) |
|
|
\ No newline at end of file |