Lejos

ParticleFilter/lejos/RemoteControl.java

@@ -0,0 +1,73 @@
+import lejos.nxt.*;
+import java.io.*;
+import lejos.nxt.comm.*;
+import lejos.util.Delay;
+import lejos.robotics.navigation.DifferentialPilot;
+
+public class RemoteControl {
+    public static DataOutputStream dataOut;
+    public static DataInputStream dataIn;
+    public static NXTConnection Link;
+    public static DifferentialPilot pilot;
+    public static LightSensor sensor;
+
+
+    public static int speed = 200, turnSpeed = 125, speedBuffer, speedControl;
+    public static int commandValue,transmitReceived, lastCommand = 0;
+
+    public static void main(String [] args) throws Exception {
+        connect();
+        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.A, Motor.C, false);
+        sensor = new LightSensor(SensorPort.S4) ;
+
+        while(true){
+            if (dataIn.available() > 0){
+                byte x = dataIn.readByte();
+                byte y = dataIn.readByte();
+                if(checkCommand((int)x, (int)y)) {
+                    disconnect();
+                    break;
+                }
+            }
+        }
+    }
+ 
+    public static boolean checkCommand(int cmd, int data) {
+        System.out.println(data);
+        try {
+            switch (data) {
+                case 0x1:
+                    pilot.travel(data);
+                    dataOut.writeChars("OK");
+                    break;
+                case 0x2:
+                    pilot.rotate(data/255.0*360);
+                    dataOut.writeChars("OK");
+                    break;
+                case 0x3:
+                    int v = sensor.readValue();
+                    dataOut.writeChars(String.valueOf(v));
+                    break;
+            }
+            dataOut.flush();
+        } catch (IOException e) {
+        }
+        return false;
+    }
+
+    public static void connect() {  
+        System.out.println("Listening..");
+        Link = Bluetooth.waitForConnection(0, NXTConnection.RAW);
+        dataOut = Link.openDataOutputStream();
+        dataIn = Link.openDataInputStream();
+
+    }
+
+    public static void disconnect() throws java.io.IOException {  
+        System.out.println("Closing...");
+        dataOut.close();
+        dataIn.close();
+        USB.usbReset();
+        System.exit(0);
+    }
+}