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();
}
