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