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 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);
   }
 }
 public boolean moveForwardSimply() {
   drone.getCommandManager().forward(10).doFor(1000);
   wait(500);
   drone.hover();
   wait(1000);
   return true;
 }
 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 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 void spinRight() {
   drone.getCommandManager().spinRight(30);
 }
 public void goLeftManu() {
   drone.getCommandManager().goLeft(20);
 }
 public void goRightManu() {
   drone.getCommandManager().goRight(20);
 }
 public void forwardManu() {
   drone.getCommandManager().forward(20);
 }
 public void backWardManu() {
   drone.getCommandManager().backward(20);
 }
 ////// **These below APIs are for manual handling**/////////
 public void downManu() {
   drone.getCommandManager().down(20);
 }
 public void upManu() {
   drone.getCommandManager().up(20);
 }
 public String readBottom() {
   drone.getCommandManager().setVideoChannel(VideoChannel.VERT);
   drone.getCommandManager().waitFor(100); // to make sure
   return bar.getResult();
 }
 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);
 }