From 08ec061659c3aa6a0984dbd2520e24456862890e Mon Sep 17 00:00:00 2001 From: Ashley Date: Thu, 30 Apr 2015 01:49:55 -0400 Subject: [PATCH] Added support for controlling drone via API --- #Main.java# | 162 +++++++++++++++ .DS_Store | Bin 0 -> 6148 bytes Main.java | 1 - Main.java~ | 162 +++++++++++++++ MindControlledUAV/.DS_Store | Bin 0 -> 6148 bytes MindControlledUAV/.classpath | 9 + MindControlledUAV/.gitignore | 1 + MindControlledUAV/.project | 17 ++ .../.settings/org.eclipse.jdt.core.prefs | 12 ++ MindControlledUAV/AffectivEvents.txt | 5 + MindControlledUAV/CognitivEvents.txt | 13 ++ MindControlledUAV/ExpressivEvents.txt | 11 + MindControlledUAV/src/.DS_Store | Bin 0 -> 6148 bytes MindControlledUAV/src/ImprovedMain.java | 191 ++++++++++++++++++ MindControlledUAV/src/Main.java | 162 +++++++++++++++ MindControlledUAV/src/NavData.java | 132 ++++++++++++ MindControlledUAV/src/Reset.java | 62 ++++++ 17 files changed, 939 insertions(+), 1 deletion(-) create mode 100644 #Main.java# create mode 100644 .DS_Store create mode 100755 Main.java~ create mode 100644 MindControlledUAV/.DS_Store create mode 100644 MindControlledUAV/.classpath create mode 100644 MindControlledUAV/.gitignore create mode 100644 MindControlledUAV/.project create mode 100644 MindControlledUAV/.settings/org.eclipse.jdt.core.prefs create mode 100755 MindControlledUAV/AffectivEvents.txt create mode 100755 MindControlledUAV/CognitivEvents.txt create mode 100755 MindControlledUAV/ExpressivEvents.txt create mode 100644 MindControlledUAV/src/.DS_Store create mode 100644 MindControlledUAV/src/ImprovedMain.java create mode 100644 MindControlledUAV/src/Main.java create mode 100644 MindControlledUAV/src/NavData.java create mode 100644 MindControlledUAV/src/Reset.java diff --git a/#Main.java# b/#Main.java# new file mode 100644 index 0000000..0185cc3 --- /dev/null +++ b/#Main.java# @@ -0,0 +1,162 @@ +import java.net.*; +import java.util.*; +import java.awt.*; +import java.awt.event.*; +import java.nio.*; + +//Main class for controlling the drone with the Emotiv EPOC and EmoKey +class Main extends Frame implements KeyListener { + private static final long serialVersionUID = 1L; + InetAddress inet_addr; + DatagramSocket socket; + int seq = 1; //Send AT command with sequence number 1 will reset the counter + float speed = (float)0.8; //UAV movement speed + boolean shift = false; + FloatBuffer fb; + IntBuffer ib; + + public Main(String name, String args[]) throws Exception { + super(name); + String ip = "192.168.1.1"; //IP address for the UAV WiFi connection + if (args.length >= 1) { + ip = args[0]; + } + StringTokenizer st = new StringTokenizer(ip, "."); + byte[] ip_bytes = new byte[4]; + if (st.countTokens() == 4) { + for (int i = 0; i < 4; i++) { + ip_bytes[i] = (byte)Integer.parseInt(st.nextToken()); + } + } + else { + System.out.println("Incorrect IP address format: " + ip); + System.exit(-1); + } + System.out.println("IP: " + ip); + System.out.println("Speed: " + speed); + ByteBuffer bb = ByteBuffer.allocate(4); + fb = bb.asFloatBuffer(); + ib = bb.asIntBuffer(); + inet_addr = InetAddress.getByAddress(ip_bytes); + socket = new DatagramSocket(); + socket.setSoTimeout(3000); + send_at_cmd("AT*CONFIG=1,\"control:altitude_max\",\"2000\""); //altitude max 2 meters + if (args.length == 2) { //Command line mode + send_at_cmd(args[1]); + System.exit(0); + } + //Use keys from EmoKey to control the drone if different events are triggered + addKeyListener(this); + setSize(320, 160); + setVisible(true); + addWindowListener(new WindowAdapter() { + public void windowClosing(WindowEvent e) { + System.exit(0); + } + }); + Thread t = new Thread(new Reset()); + t.start(); + } + + public static void main(String args[]) throws Exception { + new Main("ARDrone MAIN", args); + } + + @Override + public void keyPressed(KeyEvent e) { + //changes key to its associated integer + int keyCode = e.getKeyCode(); + System.out.println("Key: " + keyCode + " (" + KeyEvent.getKeyText(keyCode) + ")"); + try { + //Use specific key code as a command to the drone + control(keyCode); + } + catch (Exception ex) { + ex.printStackTrace(); + } + } + + @Override + public void keyReleased(KeyEvent arg0) {} + + @Override + public void keyTyped(KeyEvent arg0) {} + + public int intOfFloat(float f) { + fb.put(0, f); + return ib.get(0); + } + + //Control AR.Drone via AT commands per key code + public void control(int keyCode) throws Exception { + String at_cmd = ""; + String action = ""; + switch (keyCode) { + //Only max of 4 cognitive commands at a time recommended + //Any changes to letter meaning/action here must occur in EmoKey mapping too + case 'I'://Up (lift) + action = "Go Up (gaz+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0," + intOfFloat(speed) + ",0"; + break; + case 'P': //Forward (push) + action = "Go Forward (pitch+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0," + intOfFloat(-speed) + ",0,0"; + break; + case 'D'://Down (drop) + action = "Go Down (gaz-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0," + intOfFloat(-speed) + ",0"; + break; + case 'U': //Backward (pull) + action = "Go Backward (pitch-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0," + intOfFloat(speed) + ",0,0"; + break; + case 'T'://turn left (counterclockwise) + action = "Rotate Left (yaw-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0,0," + intOfFloat(-speed); + break; + case 'L': //Left (think left) + action = "Go Left (roll-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1," + intOfFloat(-speed) + ",0,0,0"; + break; + case 'C'://turn right (clockwise) + action = "Rotate Right (yaw+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0,0," + intOfFloat(speed); + break; + case 'R': //Right (think right) + action = "Go Right (roll+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1," + intOfFloat(speed) + ",0,0,0"; + break; + case 'N'://Hover (neutral) + //Not necessary but can be used to stabilize drone + action = "Hovering"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0,0,0"; + break; + case 'S'://Takeoff (raise brow) + action = "Takeoff"; + at_cmd = "AT*REF=" + (seq++) + ",290718208"; + break; + case 'A'://Land (clench) + action = "Landing"; + at_cmd = "AT*REF=" + (seq++) + ",290717696"; + break; + case 'Z'://reset + action = "Reset"; + at_cmd = "AT*REF=1,290717952"; + break; + default: + break; + } + System.out.println("Speed: " + speed); + System.out.println("Action: " + action); + send_at_cmd(at_cmd); + } + + public void send_at_cmd(String at_cmd) throws Exception { + System.out.println("AT command: " + at_cmd); + byte[] buffer = (at_cmd + "\r").getBytes(); + DatagramPacket packet = new DatagramPacket(buffer, buffer.length, inet_addr, 5556); + socket.send(packet); + //socket.receive(packet); //AR.Drone does not send back ack message (like "OK") + //System.out.println(new String(packet.getData(),0,packet.getLength())); + } +} \ No newline at end of file diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..f46bdd9b972110019167461cce6efb1be9549ef9 GIT binary patch literal 6148 zcmeHKyH3ME5S$G`%A-k1X(N*wTEoG;9XKfp8!k_(u*fXDbF{<0!*{a3CDm zIH2E$jG>%6*T(77!6F?2$bEJvu&zIgs>z&l=h~Q82r((yq~b>mF**A)AD27V#wLgO z;X{08@e_)q*?Ipg4Tt2$QHBHIK+}OeYfd!(mp`BXo1$oi1L44*azOgUMKR+md2a2z voaWk$ami5VxY~FlVI`$vj@49rz(`= 1) { + ip = args[0]; + } + StringTokenizer st = new StringTokenizer(ip, "."); + byte[] ip_bytes = new byte[4]; + if (st.countTokens() == 4) { + for (int i = 0; i < 4; i++) { + ip_bytes[i] = (byte)Integer.parseInt(st.nextToken()); + } + } + else { + System.out.println("Incorrect IP address format: " + ip); + System.exit(-1); + } + System.out.println("IP: " + ip); + System.out.println("Speed: " + speed); + ByteBuffer bb = ByteBuffer.allocate(4); + fb = bb.asFloatBuffer(); + ib = bb.asIntBuffer(); + inet_addr = InetAddress.getByAddress(ip_bytes); + socket = new DatagramSocket(); + socket.setSoTimeout(3000); + send_at_cmd("AT*CONFIG=1,\"control:altitude_max\",\"2000\""); //altitude max 2 meters + if (args.length == 2) { //Command line mode + send_at_cmd(args[1]); + System.exit(0); + } + //Use keys from EmoKey to control the drone if different events are triggered + addKeyListener(this); + setSize(320, 160); + setVisible(true); + addWindowListener(new WindowAdapter() { + public void windowClosing(WindowEvent e) { + System.exit(0); + } + }); + Thread t = new Thread(new Reset()); + t.start(); + } + + public static void main(String args[]) throws Exception { + new Main("ARDrone MAIN", args); + } + + @Override + public void keyPressed(KeyEvent e) { + //changes key to its associated integer + int keyCode = e.getKeyCode(); + System.out.println("Key: " + keyCode + " (" + KeyEvent.getKeyText(keyCode) + ")"); + try { + //Use specific key code as a command to the drone + control(keyCode); + } + catch (Exception ex) { + ex.printStackTrace(); + } + } + + @Override + public void keyReleased(KeyEvent arg0) {} + + @Override + public void keyTyped(KeyEvent arg0) {} + + public int intOfFloat(float f) { + fb.put(0, f); + return ib.get(0); + } + + //Control AR.Drone via AT commands per key code + public void control(int keyCode) throws Exception { + String at_cmd = ""; + String action = ""; + switch (keyCode) { + //Only max of 4 cognitive commands at a time recommended + //Any changes to letter meaning/action here must occur in EmoKey mapping too + case 'I': //Up (lift) + action = "Go Up (gaz+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0," + intOfFloat(speed) + ",0"; + break; + case 'P': //Forward (push) + action = "Go Forward (pitch+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0," + intOfFloat(-speed) + ",0,0"; + break; + case 'D': //Down (drop) + action = "Go Down (gaz-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0," + intOfFloat(-speed) + ",0"; + break; + case 'U': //Backward (pull) + action = "Go Backward (pitch-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0," + intOfFloat(speed) + ",0,0"; + break; + case 'T': //turn left (counterclockwise) + action = "Rotate Left (yaw-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0,0," + intOfFloat(-speed); + break; + case 'L': //Left (think left) + action = "Go Left (roll-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1," + intOfFloat(-speed) + ",0,0,0"; + break; + case 'C': //turn right (clockwise) + action = "Rotate Right (yaw+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0,0," + intOfFloat(speed); + break; + case 'R': //Right (think right) + action = "Go Right (roll+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1," + intOfFloat(speed) + ",0,0,0"; + break; + case 'N': //Hover (neutral) + //Not necessary but can be used to stabilize drone + action = "Hovering"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0,0,0"; + break; + case 'S': //Takeoff (raise brow) + action = "Takeoff"; + at_cmd = "AT*REF=" + (seq++) + ",290718208"; + break; + case 'A': //Land (clench) + action = "Landing"; + at_cmd = "AT*REF=" + (seq++) + ",290717696"; + break; + case 'Z': //reset + action = "Reset"; + at_cmd = "AT*REF=1,290717952"; + break; + default: + break; + } + System.out.println("Speed: " + speed); + System.out.println("Action: " + action); + send_at_cmd(at_cmd); + } + + public void send_at_cmd(String at_cmd) throws Exception { + System.out.println("AT command: " + at_cmd); + byte[] buffer = (at_cmd + "\r").getBytes(); + DatagramPacket packet = new DatagramPacket(buffer, buffer.length, inet_addr, 5556); + socket.send(packet); + //socket.receive(packet); //AR.Drone does not send back ack message (like "OK") + //System.out.println(new String(packet.getData(),0,packet.getLength())); + } +} \ No newline at end of file diff --git a/MindControlledUAV/.DS_Store b/MindControlledUAV/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..4e6d64a7cb151b60a6bdcf07b03f09cb12733d97 GIT binary patch literal 6148 zcmeHK!EVz)5S>jzYZD2K)I)po4GuY^GLR!y+9IJ|DjCrOMa8ZoV#)GWu|p1NBp=%X z|AE?H=@p3s|59dVhe~PTN~3m1ntijgGi&dYch>^|sy|NG0XzV3PzhTPvH6S8IOz>( zh-V2=(H#09$#sG~n$C)4z%sDt4A9*xCB6Kp_>J5{`p7r$$=OO)&|joqD(5HNmWu}D5)KkNyqU};%kE@9TYzq=aDBX zsZf-Z9?Iu%P@zHFS_Uiw^9(di*Q5J?>)-YNe3I>11}p=2iUHNyj5hnYB)MB}EROD4 tAN3 + + + + + + + + diff --git a/MindControlledUAV/.gitignore b/MindControlledUAV/.gitignore new file mode 100644 index 0000000..ae3c172 --- /dev/null +++ b/MindControlledUAV/.gitignore @@ -0,0 +1 @@ +/bin/ diff --git a/MindControlledUAV/.project b/MindControlledUAV/.project new file mode 100644 index 0000000..b298f1c --- /dev/null +++ b/MindControlledUAV/.project @@ -0,0 +1,17 @@ + + + MindControlledUAV + + + + + + org.eclipse.jdt.core.javabuilder + + + + + + org.eclipse.jdt.core.javanature + + diff --git a/MindControlledUAV/.settings/org.eclipse.jdt.core.prefs b/MindControlledUAV/.settings/org.eclipse.jdt.core.prefs new file mode 100644 index 0000000..a698e59 --- /dev/null +++ b/MindControlledUAV/.settings/org.eclipse.jdt.core.prefs @@ -0,0 +1,12 @@ +eclipse.preferences.version=1 +org.eclipse.jdt.core.compiler.codegen.inlineJsrBytecode=enabled +org.eclipse.jdt.core.compiler.codegen.methodParameters=do not generate +org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.8 +org.eclipse.jdt.core.compiler.codegen.unusedLocal=preserve +org.eclipse.jdt.core.compiler.compliance=1.8 +org.eclipse.jdt.core.compiler.debug.lineNumber=generate +org.eclipse.jdt.core.compiler.debug.localVariable=generate +org.eclipse.jdt.core.compiler.debug.sourceFile=generate +org.eclipse.jdt.core.compiler.problem.assertIdentifier=error +org.eclipse.jdt.core.compiler.problem.enumIdentifier=error +org.eclipse.jdt.core.compiler.source=1.8 diff --git a/MindControlledUAV/AffectivEvents.txt b/MindControlledUAV/AffectivEvents.txt new file mode 100755 index 0000000..437723c --- /dev/null +++ b/MindControlledUAV/AffectivEvents.txt @@ -0,0 +1,5 @@ +Frustration (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_AffectivGetFrustrationScore(eState); } +Meditation (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_AffectivGetMeditationScore(eState); } +EngagementBoredom (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_AffectivGetEngagementBoredomScore(eState); } +ExcitementShortTerm (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_AffectivGetExcitementShortTermScore(eState); } +ExcitementLongTerm (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_AffectivGetExcitementLongTermScore(eState); } \ No newline at end of file diff --git a/MindControlledUAV/CognitivEvents.txt b/MindControlledUAV/CognitivEvents.txt new file mode 100755 index 0000000..7297a6a --- /dev/null +++ b/MindControlledUAV/CognitivEvents.txt @@ -0,0 +1,13 @@ +0x0002 Push +0x0004 Pull +0x0008 Lift +0x0010 Drop +0x0020 Left +0x0040 Right +0x0080 RotateLeft +0x0100 RotateRight +0x0200 RotateClockwise +0x0400 RotateCounterclockwise +0x0800 RotateForwards +0x1000 RotateReverse +0x2000 Disappear \ No newline at end of file diff --git a/MindControlledUAV/ExpressivEvents.txt b/MindControlledUAV/ExpressivEvents.txt new file mode 100755 index 0000000..056758c --- /dev/null +++ b/MindControlledUAV/ExpressivEvents.txt @@ -0,0 +1,11 @@ +Blink (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_ExpressivIsBlink(eState); } +LeftWink (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_ExpressivIsLeftWink(eState); } +RightWink (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_ExpressivIsRightWink(eState); } +LookingLeft (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_ExpressivIsLookingLeft(eState); } +LookingRight (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_ExpressivIsLookingRight(eState); } +LookingDown (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_ExpressivIsLookingDown(eState); } +LookingUp (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_ExpressivIsLookingUp(eState); } +EyesOpen (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_ExpressivIsEyesOpen(eState); } +Clench (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_ExpressivGetClenchExtent(eState); } +Smile (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_ExpressivGetSmileExtent(eState); } +EyebrowRaise (EmoState INSTANCE, Pointer eState) -> { INSTANCE.ES_ExpressivGetEyebrowExtent(eState); } \ No newline at end of file diff --git a/MindControlledUAV/src/.DS_Store b/MindControlledUAV/src/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..efa5aad869f9fbd82c05f85a3ff5b584775908e7 GIT binary patch literal 6148 zcmeHKyG}zf3_XSpfQ62-SQ zBfhMEc{^JyXY5sK&Z)<5CiH+qT-iQ5+7)$M-s?u*J|>vsiMraBBa@9Boi8OBNCuLD zWFQ&%IRvWFs` zO7v8T5<@(l?Iq%BU~}|zNE9CuGk+8>VzYDnV&RbLm^K+m2G$wS+LtoY`~Qr;OmC5I zA!(9= 1) { + ip = args[0]; + } + StringTokenizer st = new StringTokenizer(ip, "."); + byte[] ip_bytes = new byte[4]; + if (st.countTokens() == 4) { + for (int i = 0; i < 4; i++) { + ip_bytes[i] = (byte)Integer.parseInt(st.nextToken()); + } + } + else { + System.out.println("Incorrect IP address format: " + ip); + System.exit(-1); + } + System.out.println("IP: " + ip); + System.out.println("Speed: " + speed); + ByteBuffer bb = ByteBuffer.allocate(4); + fb = bb.asFloatBuffer(); + ib = bb.asIntBuffer(); + inet_addr = InetAddress.getByAddress(ip_bytes); + socket = new DatagramSocket(); + socket.setSoTimeout(3000); + send_at_cmd("AT*CONFIG=1,\"control:altitude_max\",\"4000\""); //altitude max 4 meters? + /*if (args.length == 2) { //Command line mode + send_at_cmd(args[1]); + System.exit(0); + }*/ + Thread t = new Thread(new Reset()); + t.start(); + } + + public static void main(String args[]) throws Exception { + //run API_Main for the EPOC server socket + Thread t = new Thread(new API_Main()); + t.start(); + ImprovedMain self = new ImprovedMain("ARDrone MAIN", args); + double threshold = 0.5; + String params; + String JSONResponse; + BufferedReader inFromUser = new BufferedReader( new InputStreamReader(System.in)); + + try { + //connect to the EPOC server socket + Socket clientSocket = new Socket("localhost", 4444); + DataOutputStream outToServer = new DataOutputStream(clientSocket.getOutputStream()); + + //get user params and use that to control the drone + System.out.println("Enter liftoff and land commands (separated by commas): "); + params = inFromUser.readLine(); + System.out.println("Dare to use the X gyro? (y/n): "); + String useXGyro = inFromUser.readLine(); + System.out.println("Dare to use the Y gyro? (y/n): "); + String useYGyro = inFromUser.readLine(); + + + BufferedReader inFromServer = new BufferedReader(new InputStreamReader(clientSocket.getInputStream())); + if ((useXGyro.equals("y")) || (useYGyro.equals("y"))) outToServer.writeBytes("Gyros, " + params + '\n'); + else outToServer.writeBytes(params + '\n'); + String[] tokens = params.split(", "); + while ((JSONResponse = inFromServer.readLine()) == null) { + System.out.println("waiting..."); + } + while ((JSONResponse = inFromServer.readLine()) != null) { + JSONObject obj = new JSONObject(JSONResponse); + //System.out.println(obj); //debug + + if ((useXGyro.equals("y")) || (useYGyro.equals("y"))) { + JSONArray gyros = obj.getJSONObject("EmoStateData").getJSONArray("Gyros"); + if (useXGyro.equals("y")) { + //use the GyroX data + double Xgyro_val = gyros.getJSONObject(0).getDouble("GyroX"); + System.out.println(Xgyro_val); + if (Xgyro_val != 0) self.control("turn", Xgyro_val); + } + if (useYGyro.equals("y")) { + //use the GyroY data + double Ygyro_val = gyros.getJSONObject(1).getDouble("GyroY"); + System.out.println(Ygyro_val); + if (Ygyro_val != 0) self.control("lift", Ygyro_val); + } + } + for (String token : tokens) { + //for expressiv and affectiv events, which are contained in JSONArrays + if (API_Main.getAffectivMap().containsKey(token) || API_Main.getExpressivMap().containsKey(token)){ + JSONArray array = (API_Main.getAffectivMap().containsKey(token)) ? + obj.getJSONObject("EmoStateData").getJSONArray("Affectiv") : + obj.getJSONObject("EmoStateData").getJSONArray("Expressiv"); + for (int i = 0; i < array.length(); i++) { + double param_val = array.getJSONObject(i).getDouble(token); + if (param_val > threshold && token == tokens[0]) { //take off + self.control("takeoff", 0); + } + else if (param_val > threshold && token == tokens[1]) { //land + self.control("land", 0); + } + } + } + //for cognitiv events, which are contained in a JSONObject + else if (API_Main.getCogntivMap().containsKey(token)) { + String cog_action = obj.getJSONObject("EmoStateData").getString("Cognitiv"); + if (cog_action.equals(token)) { + double param_val = obj.getJSONObject("EmoStateData").getDouble("Cognitiv"); + if (param_val > threshold && token == tokens[0]) { //take off + self.control("takeoff", 0); + } + else if (param_val > threshold && token == tokens[1]) { //land + self.control("land", 0); + } + } + } + } + } + //close all resources + clientSocket.close(); + inFromUser.close(); + inFromServer.close(); + outToServer.close(); + } + catch (SocketException e) { + System.out.println("Could not start EPOC data server socket, aborting."); + e.printStackTrace(); + System.exit(-1); + } + } + + public int intOfFloat(float f) { + fb.put(0, f); + return ib.get(0); + } + + //Control AR.Drone via AT commands + public void control(String command, double val) throws Exception { + String at_cmd = ""; + String action = ""; + float cal_speed = 0.0f; + if (command.equals("turn")) { + action = (val < 0) ? "Rotate Left (yaw-)" : "Rotate Right (yaw+)"; + //lets try gyro magnitude to control speed, use 500 as the max and floor if any head jerks occur + cal_speed = (float) ((Math.abs(val) > 500) ? 1.0 : (double) Math.round(val / 500.0 * 100) / 100); //will be negative for left + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0,0," + intOfFloat(cal_speed);//-speed); + } + else if (command.equals("lift")) { + action = (val < 0) ? "Go Down (gaz-)" : "Go Up (gaz+)"; + //lets try gyro magnitude to control speed, use 500 as the max and floor if any head jerks occur + cal_speed = (float) ((Math.abs(val) > 500) ? 1.0 : (double) Math.round(val / 500.0 * 100) / 100); //will be negative for left + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0," + intOfFloat(cal_speed) + ",0";//(double) Math.round((val / 500.0 * 100) / 100; + } + else if (command.equals("takeoff")) { + action = "Takeoff"; + at_cmd = "AT*REF=" + (seq++) + ",290718208"; + } + else if (command.equals("land")) { + action = "Landing"; + at_cmd = "AT*REF=" + (seq++) + ",290717696"; + } + System.out.println("Speed: " + cal_speed); + System.out.println("Action: " + action); + //send_at_cmd(at_cmd); NO DON'T FLY DRONE INSIDE + } + + public void send_at_cmd(String at_cmd) throws Exception { + System.out.println("AT command: " + at_cmd); + byte[] buffer = (at_cmd + "\r").getBytes(); + DatagramPacket packet = new DatagramPacket(buffer, buffer.length, inet_addr, 5556); + //socket.send(packet); NO + //socket.receive(packet); //AR.Drone does not send back ack message (like "OK") + //System.out.println(new String(packet.getData(),0,packet.getLength())); + } +} \ No newline at end of file diff --git a/MindControlledUAV/src/Main.java b/MindControlledUAV/src/Main.java new file mode 100644 index 0000000..311d52a --- /dev/null +++ b/MindControlledUAV/src/Main.java @@ -0,0 +1,162 @@ +import java.net.*; +import java.util.*; +import java.awt.*; +import java.awt.event.*; +import java.nio.*; + +//Main class for controlling the drone with the Emotiv EPOC and EmoKey +class Main extends Frame implements KeyListener { + private static final long serialVersionUID = 1L; + InetAddress inet_addr; + DatagramSocket socket; + int seq = 1; //Send AT command with sequence number 1 will reset the counter + float speed = (float)0.8; //UAV movement speed + boolean shift = false; + FloatBuffer fb; + IntBuffer ib; + + public Main(String name, String args[]) throws Exception { + super(name); + String ip = "192.168.1.1"; //IP address for the UAV WiFi connection AA + if (args.length >= 1) { + ip = args[0]; + } + StringTokenizer st = new StringTokenizer(ip, "."); + byte[] ip_bytes = new byte[4]; + if (st.countTokens() == 4) { + for (int i = 0; i < 4; i++) { + ip_bytes[i] = (byte)Integer.parseInt(st.nextToken()); + } + } + else { + System.out.println("Incorrect IP address format: " + ip); + System.exit(-1); + } + System.out.println("IP: " + ip); + System.out.println("Speed: " + speed); + ByteBuffer bb = ByteBuffer.allocate(4); + fb = bb.asFloatBuffer(); + ib = bb.asIntBuffer(); + inet_addr = InetAddress.getByAddress(ip_bytes); + socket = new DatagramSocket(); + socket.setSoTimeout(3000); + send_at_cmd("AT*CONFIG=1,\"control:altitude_max\",\"4000\""); //altitude max 2 meters + if (args.length == 2) { //Command line mode + send_at_cmd(args[1]); + System.exit(0); + } + //Use keys from EmoKey to control the drone if different events are triggered + addKeyListener(this); + setSize(320, 160); + setVisible(true); + addWindowListener(new WindowAdapter() { + public void windowClosing(WindowEvent e) { + System.exit(0); + } + }); + Thread t = new Thread(new Reset()); + t.start(); + } + + public static void main(String args[]) throws Exception { + new Main("ARDrone MAIN", args); + } + + @Override + public void keyPressed(KeyEvent e) { + //changes key to its associated integer + int keyCode = e.getKeyCode(); + System.out.println("Key: " + keyCode + " (" + KeyEvent.getKeyText(keyCode) + ")"); + try { + //Use specific key code as a command to the drone + control(keyCode); + } + catch (Exception ex) { + ex.printStackTrace(); + } + } + + @Override + public void keyReleased(KeyEvent arg0) {} + + @Override + public void keyTyped(KeyEvent arg0) {} + + public int intOfFloat(float f) { + fb.put(0, f); + return ib.get(0);//CC + } + + //Control AR.Drone via AT commands per key code + public void control(int keyCode) throws Exception { + String at_cmd = ""; + String action = ""; + switch (keyCode) { + //Only max of 4 cognitive commands at a time recommended + //Any changes to letter meaning/action here must occur in EmoKey mapping too + case 'I': //Up (lift) + action = "Go Up (gaz+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0," + intOfFloat(speed) + ",0"; + break; + case 'P': //Forward (push) + action = "Go Forward (pitch+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0," + intOfFloat(-speed) + ",0,0"; + break; + case 'D': //Down (drop) + action = "Go Down (gaz-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0," + intOfFloat(-speed) + ",0"; + break; + case 'U': //Backward (pull) + action = "Go Backward (pitch-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0," + intOfFloat(speed) + ",0,0"; + break; + case 'T': //turn left (counterclockwise) //SSASAACCCCCAAS + action = "Rotate Left (yaw-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0,0," + intOfFloat(-speed); + break; + case 'L': //Left (think left) + action = "Go Left (roll-)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1," + intOfFloat(-speed) + ",0,0,0"; + break; + case 'C': //turn right (clockwise) + action = "Rotate Right (yaw+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0,0," + intOfFloat(speed); + break; + case 'R': //Right (think right) + action = "Go Right (roll+)"; + at_cmd = "AT*PCMD=" + (seq++) + ",1," + intOfFloat(speed) + ",0,0,0"; + break; + case 'N': //Hover (neutral) + //Not necessary but can be used to stabilize drone + action = "Hovering"; + at_cmd = "AT*PCMD=" + (seq++) + ",1,0,0,0,0"; + break; + case 'S': //Takeoff (raise brow) + action = "Takeoff"; + at_cmd = "AT*REF=" + (seq++) + ",290718208"; + break; + case 'A': //Land (clench) + action = "Landing"; + at_cmd = "AT*REF=" + (seq++) + ",290717696"; + break; + case 'Z': //reset + action = "Reset"; + at_cmd = "AT*REF=1,290717952"; + break; + default: + break; + } + System.out.println("Speed: " + speed); + System.out.println("Action: " + action); + send_at_cmd(at_cmd); + } + + public void send_at_cmd(String at_cmd) throws Exception { + System.out.println("AT command: " + at_cmd); + byte[] buffer = (at_cmd + "\r").getBytes(); + DatagramPacket packet = new DatagramPacket(buffer, buffer.length, inet_addr, 5556); + socket.send(packet); + //socket.receive(packet); //AR.Drone does not send back ack message (like "OK") + //System.out.println(new String(packet.getData(),0,packet.getLength())); + } +} \ No newline at end of file diff --git a/MindControlledUAV/src/NavData.java b/MindControlledUAV/src/NavData.java new file mode 100644 index 0000000..6e94977 --- /dev/null +++ b/MindControlledUAV/src/NavData.java @@ -0,0 +1,132 @@ +/*import java.awt.Frame; +import java.awt.event.KeyEvent; +import java.awt.event.KeyListener; +import java.net.DatagramPacket; +import java.net.DatagramSocket; +import java.net.InetAddress; +import java.net.SocketTimeoutException; + +class NavData extends Thread { + DatagramSocket socket_nav; + InetAddress inet_addr; + ARDrone ardrone; + + public NavData(ARDrone ardrone, InetAddress inet_addr) throws Exception { + this.ardrone = ardrone; + this.inet_addr = inet_addr; + + socket_nav = new DatagramSocket(ARDrone.NAVDATA_PORT); + socket_nav.setSoTimeout(3000); + } + + public void run() { + int cnt = 0; + + try { + byte[] buf_snd = {0x01, 0x00, 0x00, 0x00}; + DatagramPacket packet_snd = new DatagramPacket(buf_snd, buf_snd.length, inet_addr, ARDrone.NAVDATA_PORT); + socket_nav.send(packet_snd); + System.out.println("Sent trigger flag to UDP port " + ARDrone.NAVDATA_PORT); + + ardrone.send_at_cmd("AT*CONFIG=" + ardrone.get_seq() + ",\"general:navdata_demo\",\"TRUE\""); + + byte[] buf_rcv = new byte[10240]; + DatagramPacket packet_rcv = new DatagramPacket(buf_rcv, buf_rcv.length); + + while(true) { + try { + socket_nav.receive(packet_rcv); + + cnt++; + if (cnt >= 5) { + cnt = 0; + System.out.println("NavData Received: " + packet_rcv.getLength() + " bytes"); + //System.out.println(ARDrone.byte2hex(buf_rcv, 0, packet_rcv.getLength())); + System.out.println("Battery: " + ARDrone.get_int(buf_rcv, ARDrone.NAVDATA_BATTERY) + + "%, Altitude: " + ((float)ARDrone.get_int(buf_rcv, ARDrone.NAVDATA_ALTITUDE)/1000) + "m"); + } + } catch(SocketTimeoutException ex3) { + System.out.println("socket_nav.receive(): Timeout"); + } catch(Exception ex1) { + ex1.printStackTrace(); + } + } + } catch(Exception ex2) { + ex2.printStackTrace(); + } + } +} + +class Video extends Thread { + DatagramSocket socket_video; + InetAddress inet_addr; + ARDrone ardrone; + + public Video(ARDrone ardrone, InetAddress inet_addr) throws Exception { + this.ardrone = ardrone; + this.inet_addr = inet_addr; + + socket_video = new DatagramSocket(ARDrone.VIDEO_PORT); + socket_video.setSoTimeout(3000); + } + + public void run() { + try { + byte[] buf_snd = {0x01, 0x00, 0x00, 0x00}; + DatagramPacket packet_snd = new DatagramPacket(buf_snd, buf_snd.length, inet_addr, ARDrone.VIDEO_PORT); + socket_video.send(packet_snd); + System.out.println("Sent trigger flag to UDP port " + ARDrone.VIDEO_PORT); + + ardrone.send_at_cmd("AT*CONFIG=" + ardrone.get_seq() + ",\"general:video_enable\",\"TRUE\""); + + byte[] buf_rcv = new byte[64000]; + DatagramPacket packet_rcv = new DatagramPacket(buf_rcv, buf_rcv.length); + + while(true) { + try { + socket_video.receive(packet_rcv); + System.out.println("Video Received: " + packet_rcv.getLength() + " bytes"); + //System.out.println(ARDrone.byte2hex(buf_rcv, 0, packet_rcv.getLength())); + } catch(SocketTimeoutException ex3) { + System.out.println("socket_video.receive(): Timeout"); + socket_video.send(packet_snd); + } catch(Exception ex1) { + ex1.printStackTrace(); + } + } + } catch(Exception ex2) { + ex2.printStackTrace(); + } + } +} + +class ARDrone extends Frame implements KeyListener { + /** + * + */ + /*private static final long serialVersionUID = 1L; + static final int NAVDATA_PORT = 5554; + static final int VIDEO_PORT = 5555; + static final int AT_PORT = 5556; + + //NavData offset + static final int NAVDATA_STATE = 4; + static final int NAVDATA_BATTERY = 24; + static final int NAVDATA_ALTITUDE = 40; + + @Override + public void keyPressed(KeyEvent arg0) { + // TODO Auto-generated method stub + + } + @Override + public void keyReleased(KeyEvent arg0) { + // TODO Auto-generated method stub + + } + @Override + public void keyTyped(KeyEvent arg0) { + // TODO Auto-generated method stub + + } +}*/ \ No newline at end of file diff --git a/MindControlledUAV/src/Reset.java b/MindControlledUAV/src/Reset.java new file mode 100644 index 0000000..21845af --- /dev/null +++ b/MindControlledUAV/src/Reset.java @@ -0,0 +1,62 @@ +import java.net.*; +import java.util.*; +import java.nio.*; + + +class Reset implements Runnable { + InetAddress inet_addr; + DatagramSocket socket; + FloatBuffer fb; + IntBuffer ib; + + public Reset() throws Exception { + String ip = "192.168.1.1"; + + StringTokenizer st = new StringTokenizer(ip, "."); + + byte[] ip_bytes = new byte[4]; + if (st.countTokens() == 4){ + for (int i = 0; i < 4; i++){ + ip_bytes[i] = (byte)Integer.parseInt(st.nextToken()); + } + } + else { + System.out.println("Incorrect IP address format: " + ip); + System.exit(-1); + } + + System.out.println("IP: " + ip); + + ByteBuffer bb = ByteBuffer.allocate(4); + fb = bb.asFloatBuffer(); + ib = bb.asIntBuffer(); + + inet_addr = InetAddress.getByAddress(ip_bytes); + socket = new DatagramSocket(); + socket.setSoTimeout(3000); + } + + public void send(String at_cmd) throws Exception { + byte[] buffer = (at_cmd + "\r").getBytes(); + DatagramPacket packet = new DatagramPacket(buffer, buffer.length, inet_addr, 5556); + socket.send(packet); + } + + public void run() { + while(true){ + String at_cmd = "AT*COMWDG=1"; + try{ + send(at_cmd); + } + catch(Exception ex){ + ex.printStackTrace(); + } + try{ + Thread.sleep(300); + } + catch(InterruptedException iex){ + iex.printStackTrace(); + } + } + } +} \ No newline at end of file