added color sensor segway

lesson5/DataLogger.java

+import lejos.nxt.*;
+import java.io.*;
+/**
+ * A simple data logger that can be used to sample a sequence
+ * of integer data values in a flash file. The
+ * file is a text consisting of a sequence of lines with
+ * a time value and a sample data value separated by a comma.
+ * 
+ * When data has been sampled the flash file can be transfered to a
+ * PC by means of the tool nxjbrowse.
+ * 
+ * @author  Ole Caprani
+ * @version 4.02.13
+ */
+public class DataLogger 
+{
+    private File f;
+    private FileOutputStream fos;
+    private int startTime;
+
+    public DataLogger (String fileName)
+    {
+        try
+        {	        
+            f = new File(fileName);
+            if( ! f.exists() )
+            {
+                f.createNewFile();
+            }
+            else
+            {
+                f.delete();
+                f.createNewFile();
+            }
+             
+            fos = new  FileOutputStream(f);
+        }
+        catch(IOException e)
+        {
+            LCD.drawString(e.getMessage(),0,0);
+            System.exit(0);
+        }
+        startTime = (int)System.currentTimeMillis();
+    }
+    
+    public void start()
+    {
+    	startTime = (int)System.currentTimeMillis();
+    }
+	
+    public void writeSample( int time, int sample )
+    {
+		
+        try
+        {
+            Integer timeInt = new Integer(time);
+            String timeString = timeInt.toString();
+            
+            Integer sampleInt = new Integer(sample);
+            String sampleString = sampleInt.toString();
+            
+            for(int i=0; i<timeString.length(); i++)
+            {
+                fos.write((byte) timeString.charAt(i));
+            }
+            
+            // Separate items with comma
+            fos.write((byte)(','));
+            
+            for(int i=0; i<sampleString.length(); i++)
+            {
+                fos.write((byte) sampleString.charAt(i));
+            }
+
+            // New line
+            fos.write((byte)('\r'));
+            fos.write((byte)('\n'));				
+
+        }
+        catch(IOException e)
+        {
+            LCD.drawString(e.getMessage(),0,0);
+            System.exit(0);
+        }
+    }
+    
+    public void writeSample( int sample )
+    {
+    	writeSample((int)System.currentTimeMillis() - startTime, sample);
+    }
+    
+    public void close()
+    {
+        try
+        {
+            fos.close();
+        }
+        catch(IOException e)
+        {
+            LCD.drawString(e.getMessage(),0,0);
+            System.exit(0);
+        }		 
+    }
+}

lesson5/SeqwayColor.java

+import lejos.nxt.*;
+
+public class SeqwayColor {
+    // Global vars:
+    int offset;
+    int prev_error;
+    float int_error;
+	
+    ColorSensor cs; 
+    static DataLogger dl;
+
+    // PID constants
+    final int KP = 45;
+    final int KI = 0;
+    final int KD = 1;
+    final int SCALE = 1;
+	
+	public SeqwayColor() 
+	{
+		cs = new ColorSensor(SensorPort.S3);
+	}
+
+	public void getBalancePos() 
+	{
+		// Wait for user to balance and press orange button
+		while (!Button.ENTER.isDown())
+		{
+			// NXTway must be balanced.
+			offset = cs.getNormalizedLightValue();
+			LCD.clear();
+			LCD.drawInt(offset, 2, 4);
+			LCD.refresh();
+		}
+	}
+	
+	public void pidControl() 
+    {
+        while (!Button.ESCAPE.isDown()) 
+        {
+            int normVal = cs.getNormalizedLightValue();
+
+            // Proportional Error:
+            int error = normVal - offset;
+			
+            // Integral Error:
+            int_error = int_error + error;
+			
+            // Derivative Error:
+            int deriv_error = error - prev_error;
+            prev_error = error;
+			
+            int pid_val = (int)(KP * error + KI * int_error + KD * deriv_error) / SCALE;
+			
+            if (pid_val > 100)
+                pid_val = 100;
+            if (pid_val < -100)
+                pid_val = -100;
+
+            // Power derived from PID value:
+            int power = Math.abs(pid_val);
+            power = 55 + (power * 45) / 100; // NORMALIZE POWER
+
+//            dl.writeSample(error);
+//            dl.writeSample((int)int_error);
+//            dl.writeSample(deriv_error);
+//            dl.writeSample(pid_val);
+
+            if (pid_val > 0) {
+                MotorPort.B.controlMotor(power, BasicMotorPort.FORWARD);
+                MotorPort.C.controlMotor(power, BasicMotorPort.FORWARD);
+            } else {
+                MotorPort.B.controlMotor(power, BasicMotorPort.BACKWARD);
+                MotorPort.C.controlMotor(power, BasicMotorPort.BACKWARD);
+            }
+        }
+    }
+	
+    public void shutDown()
+    {
+        // Shut down light sensor, motors
+        Motor.B.flt();
+        Motor.C.flt();
+        cs.setFloodlight(false);
+//        dl.close();
+    }
+
+	public static void main(String args[]) {
+//		dl = new DataLogger("data.txt");
+//		dl.start();
+		SeqwayColor sw = new SeqwayColor();
+		sw.getBalancePos();
+        sw.pidControl();
+        sw.shutDown();
+	}
+}