public boolean blinkLED(String color) {
try {
System.out.println("Blink LED.");
if (color.equals("RED")) {
drone.getCommandManager().setLedsAnimation(LEDAnimation.BLINK_RED, 3, 3).doFor(3000);
return true;
} else if (color.equals("ORANGE")) {
drone.getCommandManager().setLedsAnimation(LEDAnimation.BLINK_ORANGE, 3, 3).doFor(3000);
return true;
} else if (color.equals("GREEN")) {
drone.getCommandManager().setLedsAnimation(LEDAnimation.BLINK_GREEN, 3, 3).doFor(3000);
return true;
} else if (color.equals("GREEN_RED")) {
drone.getCommandManager().setLedsAnimation(LEDAnimation.BLINK_GREEN_RED, 3, 3).doFor(3000);
return true;
} else {
return false;
}
} catch (Exception e) {
e.printStackTrace();
return false;
}
}
public boolean moveForwardSimply() {
drone.getCommandManager().forward(10).doFor(1000);
wait(500);
drone.hover();
wait(1000);
return true;
}
public boolean downSimply() {
drone.getCommandManager().down(30);
wait(2500);
drone.getCommandManager().hover();
wait(500);
return true; // alwase true because cannot use height info
}
public void setDirect(Direct d, int speed) {
fm.fileWrite(d.toString());
System.out.println(d.toString());
if (d == Direct.FORWARD) {
drone.getCommandManager().forward(speed);
} else if (d == Direct.BACK) {
drone.getCommandManager().backward(speed);
} else if (d == Direct.LEFT) {
drone.getCommandManager().goLeft(speed);
} else if (d == Direct.RIGHT) {
drone.getCommandManager().goRight(speed);
}
}
private void addListener() {
vl = new VideoListener();
gyro = new AttitudeGetter();
bar = new BarcodeListener();
// heg=new HeightGetter();
bg = new BatteryGetter();
pg = new PositionGetter();
drone.getNavDataManager().addAttitudeListener(gyro);
// drone.getNavDataManager().addAltitudeListener(heg);
drone.getNavDataManager().addBatteryListener(bg);
drone.getVideoManager().addImageListener(vl);
drone.getVideoManager().addImageListener(bar);
}
public boolean takeoff(int time) {
try {
System.out.println("Take off the Drone.");
drone.getCommandManager().takeOff().doFor(time);
return true;
} catch (Exception e) {
e.printStackTrace();
return false;
}
}
public boolean stop() {
try {
System.out.println("Stop the Drone controlling.");
drone.stop();
return true;
} catch (Exception e) {
e.printStackTrace();
return false;
}
}
public boolean landing() {
endLog();
try {
System.out.println("Land the Drone.");
drone.getCommandManager().landing();
if (gyro.getPitch() < 20 && gyro.getRoll() < 20) return true;
else return false;
} catch (Exception e) {
e.printStackTrace();
return false;
}
}
public boolean turn(Direct d) {
float yawtarget = 0;
float th = 5;
int timecounta = 0;
if (d == Direct.FORWARD || d == Direct.NORTH) yawtarget = northDirect;
else if (d == Direct.LEFT || d == Direct.WEST) {
yawtarget = northDirect - 90;
if (yawtarget < 180) yawtarget = 360 + yawtarget;
} else if (d == Direct.RIGHT || d == Direct.EAST) {
yawtarget = northDirect + 90;
if (yawtarget > 180) yawtarget = -360 + yawtarget;
} else if (d == Direct.BACK || d == Direct.SOUTH) {
yawtarget = northDirect + 180;
if (yawtarget > 180) yawtarget = -360 + yawtarget;
}
while (timecounta < 50) {
float err = yawtarget - gyro.getYaw();
if (err > 180) err -= 360;
else if (err < -180) err += 360;
if (err < th && err > -th) {
drone.getCommandManager().hover();
wait(1000);
return true;
} else {
if (err > 0) {
err = err > 50 ? 50 : err;
drone.getCommandManager().spinRight((int) err);
System.out.println("Failed with:" + err + "deg");
} else if (err < 0) {
err = err < -50 ? 50 : -err;
drone.getCommandManager().spinLeft((int) err);
System.out.println("Failed with:" + err + "deg");
}
wait(100);
}
timecounta++;
}
return false;
}
public String readBottom() {
drone.getCommandManager().setVideoChannel(VideoChannel.VERT);
drone.getCommandManager().waitFor(100); // to make sure
return bar.getResult();
}
public void spinRight() {
drone.getCommandManager().spinRight(30);
}
public void goRightManu() {
drone.getCommandManager().goRight(20);
}
public void goLeftManu() {
drone.getCommandManager().goLeft(20);
}
public void backWardManu() {
drone.getCommandManager().backward(20);
}
public void forwardManu() {
drone.getCommandManager().forward(20);
}
public void upManu() {
drone.getCommandManager().up(20);
}
////// **These below APIs are for manual handling**/////////
public void downManu() {
drone.getCommandManager().down(20);
}
public BufferedImage takeBottomPicture() {
drone.getCommandManager().setVideoChannel(VideoChannel.VERT);
drone.getCommandManager().waitFor(100); // to make sure
return vl.getIamge();
}
// block the thread and execution of commands are blocked
public void wait(int time) {
drone.getCommandManager().waitFor(time);
}
public void move(int m, int n) throws ARDroneException {
fm.fileWrite(
"============start move" + Integer.toString(m) + "," + Integer.toString(n) + "===========");
long start = System.currentTimeMillis();
long finish = 0;
float th = (float) 0.15;
int timecounta = 0;
float[] targetPosition = {(float) m, (float) n + (float) 0.5};
turn(Direct.NORTH);
while (timecounta < 200) {
float[] currentPosition = pg.getPosition();
if (currentPosition == null) {
drone.hover();
wait(1000);
throw new FailException();
}
float ex = targetPosition[0] - currentPosition[0];
float ey = targetPosition[1] - currentPosition[1];
if (ex < th && ex > -th && ey < th && ey > -th) {
drone.hover();
fm.fileWrite("Success");
System.out.println(
"Success!!target x:" + targetPosition[0] + "target y:" + targetPosition[1]);
wait(1000);
return;
} else {
finish = System.currentTimeMillis();
fm.fileWrite("err of x:" + ex + "err of y:" + ey + "currentTime:" + (finish - start));
System.out.println("err of x:" + ex + "err of y:" + ey);
if (ex > th || ex < -th) {
if (ex > 0) {
setDirect(Direct.RIGHT, calcSpeed(ex));
} else if (ex < 0) {
setDirect(Direct.LEFT, calcSpeed(ex));
}
} else if (ey > th || ey < -th) {
if (ey > 0) {
setDirect(Direct.FORWARD, calcSpeed(ey));
} else if (ey < 0) {
setDirect(Direct.BACK, calcSpeed(ey));
}
}
}
String text = this.readFront();
if (!text.equals(BarcodeListener.NONE)) {
throw new ObstacleFoundException();
}
wait(100);
if (timecounta % 10 == 0) {
hover(500);
wait(1500);
// turn(Direct.NORTH);
fm.fileWrite("stop!!1500ms");
}
timecounta++;
}
drone.getCommandManager().hover();
wait(500);
fm.fileWrite("Failed");
throw new FailException();
}
public void hover(int time) {
drone.getCommandManager().hover().doFor(time);
}