Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
Made small modifications to ImprovedMain.java
  • Loading branch information
@amd11037
amd11037 committed Aug 28, 2015
1 parent 6d4b13b commit 03d153f
Showing 1 changed file with 85 additions and 26 deletions.
111 changes: 85 additions & 26 deletions MindControlledUAV/src/ImprovedMain.java
View file
@@ -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;
Expand All @@ -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
Expand All @@ -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) {
Expand All @@ -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();
}
Expand All @@ -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');
Expand All @@ -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) {
Expand Down Expand Up @@ -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";
Expand All @@ -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) {}
}

0 comments on commit 03d153f

Please sign in to comment.