Lesson-3.markdown

@@ -47,14 +47,67 @@ TBD: Add codesnippet of ButtonListener implementation
 ### Exercise 5
 The figure below show four consecutive claps, measured with the nxt sound sensor.
 The length of the four claps vary between 85 and 175 ms with a sound level above 85%.
+
 ![Clap characteristics](https://gitlab.au.dk/rene2014/lego/raw/master/Week5/Measurements/ClapMeasurements.png)
 
 TBD: Code snippet? Limits: 80 and 200ms?
 
-### Exercise 6
-TBD: Code snippet, video, calibration:-30, raw values are inverted compared to percent readings, PID control and gain choice.
+### Exercise 6 - Party Finder Robot
+In this exercise we mount two sound sensors on the Lego car and create a java application that will drive towards loud sound.
+In the application we use a PID controller, to control the steering of the vehicle, so that the sound levels on the two sensors are equal.
+
+In the code snippet below, the main function is shown. 
+The sensor ports are initiated as raw mode, since this will increase the accuracy of the readings.
+The DifferentialPilot is used to steer the vehicle, since is gives a easy handle for turning the vehicle in a smooth way.
+In the inner control loop, the PID controller is implemented.
+Note that the left sensor reading is calibrated, by subtracting 30 from the raw value. 
+This is done, in order for the sound sensors to have equal calibration. 
+Otherwise the vehicle will always turn.
+
+```
+SensorPort leftSound = SensorPort.S1;
+SensorPort rightSound = SensorPort.S4;
+leftSound.setTypeAndMode(SensorPort.TYPE_SOUND_DB, SensorPort.MODE_RAW);
+rightSound.setTypeAndMode(SensorPort.TYPE_SOUND_DB, SensorPort.MODE_RAW);
+DifferentialPilot pilot = new DifferentialPilot(56f, 113f, Motor.B, Motor.C);
+		
+int leftNoise = 0;
+int rightNoise = 0;
+int last_error = 0;
+int integral = 0;
+
+pilot.forward();
+pilot.setTravelSpeed(100);
+
+while(!Button.ESCAPE.isDown())
+{
+	leftNoise = leftSound.readRawValue() - 30; //Calibration :-)
+	rightNoise = rightSound.readRawValue();
+	
+	int error = leftNoise - rightNoise;
+	integral = integral + error * dt;
+	int derivative = (error - last_error) / dt;
+	float turn = Kp * error + Ki * integral + Kd * derivative;
+	last_error = error;
+	
+	pilot.steer((int)turn);	
+	
+	Thread.sleep(dt);
+}
+```
+
+The gains for the vehicle is chosen empirically to:
+```
+static float 	Kp = 5.0f,
+				Ki = 0f,
+				Kd = 0.5f;
+```
+The performance of our Party Finding Robot is shown in the following video.
+
 [![Party Seeking Robot video](http://img.youtube.com/vi/RL0ZnIF1B_o/0.jpg)](http://www.youtube.com/watch?v=RL0ZnIF1B_o)
 
+
+
 ## Conclusion
 
 ## References