Added ComplexPartyFinder.

lesson3/ComplexPartyFinder/src/Car.java

+import lejos.nxt.*;
+/**
+ * A locomotion module with methods to drive
+ * a car with two independent motors. The left motor 
+ * should be connected to port C and the right motor
+ * to port B.
+ *  
+ * @author  Ole Caprani
+ * @version 17.4.08
+ */
+public class Car 
+{
+    // Commands for the motors
+    public final static int forward  = 1,
+                            backward = 2,
+                            stop     = 3;
+	                         
+    private static MotorPort leftMotor = MotorPort.C;
+    private static MotorPort rightMotor= MotorPort.B;
+	
+    private Car()
+    {	   
+    } 
+   
+    public static void stop() 
+    {
+	    leftMotor.controlMotor(0,stop);
+	    rightMotor.controlMotor(0,stop);
+    }
+   
+    public static void controlLeft(int leftPower, int mode)
+    {
+	    leftMotor.controlMotor(leftPower, mode);
+    }
+   
+    public static void controlRight(int rightPower, int mode)
+    {
+	    rightMotor.controlMotor(rightPower, mode);
+    }
+}

lesson3/ComplexPartyFinder/src/PartyDriver.java

+import lejos.nxt.Button;
+import lejos.nxt.LCD;
+import lejos.util.Delay;
+
+
+public class PartyDriver {
+
+	public static void main (String[] aArg)	{
+		PartyFinder follower = new PartyFinder();
+
+		LCD.drawString("PartyFinder", 0, 0);
+		LCD.drawString("Diff Avg:    ", 0, 2);
+		LCD.drawString("Diff:    ", 0, 3);
+		LCD.drawString("LeftSound:    ", 0, 4);
+		LCD.drawString("RightSound:    ", 0, 5);
+		
+		while (! Button.ENTER.isDown()) {
+			Delay.msDelay(100);
+		}			
+
+		follower.go();
+
+		while (! Button.ESCAPE.isDown())
+		{		   
+			LCD.drawInt(follower.getLeftSound(), 4,10,4);
+			LCD.drawInt(follower.getRightSound(), 4,10,5);
+
+			Delay.msDelay(30);
+		}
+
+		follower.stop();
+		LCD.clear();
+		LCD.drawString("Program stopped", 0, 0);
+		Delay.msDelay(2000);
+	}
+}

lesson3/ComplexPartyFinder/src/PartyFinder.java

+import java.util.ArrayList;
+
+import lejos.nxt.LCD;
+import lejos.nxt.SensorPort;
+import lejos.util.Delay;
+
+
+public class PartyFinder extends Thread {	
+	private boolean running;
+	private int sampleInterval = 30; // ms default value
+	private final int averageCount = 20;
+	private SensorPort sensorLeft = SensorPort.S2;
+	private SensorPort sensorRight = SensorPort.S3;	
+
+	private int powerLeft, powerRight, soundLeft, soundRight, diff,
+		motorMode = Car.stop, diffAverage, soundLeftAvg, soundRightAvg;
+	
+	private ArrayList<Integer> diffList = new ArrayList<Integer>();
+	private ArrayList<Integer> soundLeftList = new ArrayList<Integer>(); 
+	private ArrayList<Integer> soundRightList = new ArrayList<Integer>(); 
+
+	public PartyFinder() {
+		sensorLeft.setTypeAndMode(SensorPort.TYPE_SOUND_DB,
+				SensorPort.MODE_PCTFULLSCALE);
+		sensorRight.setTypeAndMode(SensorPort.TYPE_SOUND_DB,
+				SensorPort.MODE_PCTFULLSCALE);
+	}
+
+	public void go() {
+		running = true;
+		this.start();
+	}
+
+	public void stop() {
+		running = false;
+	}
+
+	public void run() {
+		// Do initial setup before main loop.
+		int counter = 0;
+		int threshold = 950;
+
+		while ( running ) {		   
+			// Read values from the sensors.
+			// More noise from motors behind the robot.
+			soundLeft = sensorLeft.readRawValue() + 125;
+			// Less noise in front.
+			soundRight = sensorRight.readRawValue();
+			
+			soundLeftList.add(soundLeft);
+			soundRightList.add(soundRight);
+			if (soundLeftList.size() > averageCount) soundLeftList.remove(0);
+			if (soundRightList.size() > averageCount) soundRightList.remove(0);
+			
+			diff = soundLeft - soundRight;
+			
+			diffList.add(diff);
+			if (diffList.size() > averageCount) diffList.remove(0);
+			
+			counter++;
+			if (counter < averageCount) continue;
+						
+			diffAverage = getAverage(diffList);
+			soundLeftAvg = getAverage(soundLeftList);
+			soundRightAvg = getAverage(soundRightList);			
+			powerLeft = 75;
+			powerRight = 75;
+			
+			/*
+			 * * Sensors:
+			 * left = points backwards
+			 * right = points forwards
+			 * 
+			 * * Steps:
+			 * 1. Read values from sensors
+			 * 2. Compare sensor values to determine direction
+			 * 3. Move forwards/backwards towards loud sound
+			 * 4. Adjust angle based on difference between values. 
+			 * When forward:
+			 * highest diff is straight ahead
+			 * nearing zero diff when left of robot
+			 * lower diff when right of robot
+			 * When backwards
+			 * lower diff is straight back (-60) 
+			 * lowest diff is right of robot (0)
+			 * highest diff is left of robot (-150)
+			 * 5. Save average of highest sound readings to
+			 * know if we are getting closer.
+			 * 6. When average doens't get lower we are at the party.
+			 */												
+			
+			if (soundLeftAvg > threshold + 125 && soundRightAvg > threshold) {
+				motorMode = Car.stop;
+			} else if (soundLeftAvg > soundRightAvg) {
+				motorMode = Car.forward;
+			} else if (soundLeftAvg < soundRightAvg) {
+				motorMode = Car.backward;
+			}
+			
+			if (counter % averageCount == 0) {
+				threshold = (motorMode == Car.stop) ? 950 : 800;
+			}							
+				
+			// Adjust angle based on difference between values.
+			if (motorMode == Car.forward) {
+				if (diffAverage < 20) { // move left
+					powerLeft = 60;
+				} else if (diffAverage < 40) { // move right
+					powerRight = 60;
+				} else { // move straight
+					
+				}
+				
+			} else if (motorMode == Car.backward) {
+				if (diffAverage > -20) { // move right
+					powerRight = 60;
+				} else if (diffAverage < 100) { // move left
+					powerLeft = 60;
+				} else { // move straight
+					
+				}
+			}			
+
+			Car.controlLeft(powerLeft, motorMode);
+			Car.controlRight(powerRight, motorMode);
+			
+			LCD.drawInt(diffAverage, 4, 10, 2);
+			LCD.drawInt(diff, 4, 10, 3);
+			
+			Delay.msDelay(sampleInterval);
+		}
+	}		
+	
+	private int getAverage(ArrayList<Integer> values) {
+		float sum = 0;
+		for (Integer i : values) {
+			sum += i;
+		}
+		return (int) (sum / (float)values.size());
+	}
+	
+	public int getLeftSound()
+    {
+        return soundLeftAvg;
+    }
+	
+	public int getRightSound()
+    {
+        return soundRightAvg;
+    }
+}
\ No newline at end of file