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 test() { // System.out.println("Now Battery:"+bg.getBatteryParsentage()+"%"); System.out.println("Now yaw:" + gyro.getYaw()); }
public void setCurrentDirectAsNorth() { this.northDirect = gyro.getYaw(); }