diff --git a/MindControlledUAV/src/ImprovedMain.java b/MindControlledUAV/src/ImprovedMain.java index 15487a7..bca1181 100644 --- a/MindControlledUAV/src/ImprovedMain.java +++ b/MindControlledUAV/src/ImprovedMain.java @@ -1,3 +1,8 @@ +import java.awt.Frame; +import java.awt.event.KeyEvent; +import java.awt.event.KeyListener; +import java.awt.event.WindowAdapter; +import java.awt.event.WindowEvent; import java.io.BufferedReader; import java.io.DataOutputStream; import java.io.InputStreamReader; @@ -6,9 +11,8 @@ import java.util.*; import java.nio.*; import org.json.*; - -//Main class for controlling the drone with my super awesome API -class ImprovedMain { +class ImprovedMain 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 @@ -17,12 +21,9 @@ class ImprovedMain { FloatBuffer fb; IntBuffer ib; - public ImprovedMain(String name, String args[]) throws Exception { + public ImprovedMain() throws Exception { super(); 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) { @@ -42,11 +43,15 @@ class ImprovedMain { 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); - }*/ + send_at_cmd("AT*CONFIG=1,\"control:altitude_max\",\"2000\""); //altitude max 3 meters + 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(); } @@ -55,26 +60,27 @@ class ImprovedMain { //run API_Main for the EPOC server socket Thread t = new Thread(new API_Main()); t.start(); - ImprovedMain self = new ImprovedMain("ARDrone MAIN", args); + ImprovedMain self = new ImprovedMain(); double threshold = 0.5; + double gyro_thresh = 10.0; String params; String JSONResponse; BufferedReader inFromUser = new BufferedReader( new InputStreamReader(System.in)); try { - //connect to the EPOC server socket + // 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 + // get user params and use that to control the drone System.out.println("Enter liftoff and land commands (separated by commas): "); params = inFromUser.readLine(); + // only works for the EPOC+ since the EPOC doesn't have gyros 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'); @@ -85,20 +91,21 @@ class ImprovedMain { 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); + //System.out.println(Xgyro_val); //debug + if (Math.abs(Xgyro_val) > gyro_thresh/*!= 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); + //System.out.println(Ygyro_val); //debug + if (Math.abs(Ygyro_val) > gyro_thresh /*!= 0*/) { + self.control("lift", Ygyro_val); + } } } for (String token : tokens) { @@ -157,16 +164,21 @@ class ImprovedMain { 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 + //lets try gyro magnitude to control speed, use 300 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 + //lets try gyro magnitude to control speed, use 300 as the max and floor if any head jerks occur + cal_speed = (float) ((Math.abs(val) > 300) ? 1.0 : (double) Math.round(val / 300.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("forward")) { + action = (val < 0) ? "Go Forward (pitch+)" : "Go Backward (pitch-)"; + 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," + intOfFloat(cal_speed) + ",0,0"; + } else if (command.equals("takeoff")) { action = "Takeoff"; at_cmd = "AT*REF=" + (seq++) + ",290718208"; @@ -177,15 +189,62 @@ class ImprovedMain { } System.out.println("Speed: " + cal_speed); System.out.println("Action: " + action); - //send_at_cmd(at_cmd); DON'T FLY DRONE INSIDE + 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); NO + 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())); } + + @Override + public void keyTyped(KeyEvent e) {} + + @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 + controlManual(keyCode); + } + catch (Exception ex) { + ex.printStackTrace(); + } + } + + //Control AR.Drone via AT commands per key code for important commands only + public void controlManual(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 'S': //Takeoff + action = "Takeoff"; + at_cmd = "AT*REF=" + (seq++) + ",290718208"; + break; + case 'A': //Land + 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); + } + + @Override + public void keyReleased(KeyEvent e) {} }