Lesson4Week6.markdown

@@ -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)
+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