public void yawAdapt(float leaderYawInit, float myYawInit) {
float leaderYaw = leader.getNavDataHandler().getNavDataDemo().getYaw() - leaderYawInit;
float myYaw = this.drone.getNavDataHandler().getNavDataDemo().getYaw() - myYawInit;
// System.out.println("leader :"+leaderYaw);
// System.out.println("suiveur :"+myYaw);
if (Math.abs(leaderYaw - myYaw) > 10) {
if (leaderYaw * myYaw > 0) {
this.drone.setSpeedYaw(
(leaderYaw - myYaw)
/ Math.abs(leaderYaw - myYaw)
* Math.min(Math.abs(leaderYaw - myYaw) / 30, 1));
} else {
if (myYaw < 0) {
if (leaderYaw - myYaw < 360 - leaderYaw + myYaw) {
this.drone.setSpeedYaw(Math.min(1, (leaderYaw - myYaw) / 30));
} else {
this.drone.setSpeedYaw(-Math.min(1, Math.abs(360 - leaderYaw + myYaw) / 30));
}
} else {
if (-leaderYaw + myYaw < 360 + leaderYaw - myYaw) {
this.drone.setSpeedYaw(-Math.min(1, (-leaderYaw + myYaw) / 30));
} else {
this.drone.setSpeedYaw(Math.min(1, Math.abs(360 + leaderYaw - myYaw) / 30));
}
}
}
} else {
this.drone.setSpeedYaw(0);
}
// System.out.println("consigne yaw :"+this.drone.speedYaw);
}
public void altAdapt() {
float leaderAlt = leader.getNavDataHandler().getNavDataDemo().getAltitude();
float myAlt = this.drone.getNavDataHandler().getNavDataDemo().getAltitude();
if (Math.abs(leaderAlt - myAlt) > 6) {
this.drone.setSpeedUD(Math.min((leaderAlt - myAlt) / 150, 1));
// System.out.println((leaderAlt-myAlt)/150);
// this.drone.getController().move();
} else {
this.drone.setSpeedUD(0);
// this.drone.getController().move();
}
}
public void run() {
dronesRegistry.addDrone(drone);
if (drone.getId() != 1) {
leader = dronesRegistry.getLeader();
// A Priori : theta = roulis
float leaderYawInit = leader.getNavDataHandler().getNavDataDemo().getYaw();
float myYawInit = this.drone.getNavDataHandler().getNavDataDemo().getYaw();
while (!kill) {
this.rollAdapt();
this.pitchAdapt();
// this.yawAdapt(leaderYawInit, myYawInit);
// this.altAdapt();
this.drone.getController().move();
// System.out.println("boucle");
try {
Thread.sleep(100); // kills CPU otherwise! sure?
} catch (InterruptedException e) {
e.printStackTrace();
}
}
}
}
public void rollAdapt() {
float leaderRoll = leader.getNavDataHandler().getNavDataDemo().getRoll();
this.drone.setSpeedLR(leaderRoll / 45);
// System.out.println("consigne "+this.drone.speedLR);
}
public void pitchAdapt() {
float leaderPitch = leader.getNavDataHandler().getNavDataDemo().getPitch();
this.drone.setSpeedBF(leaderPitch / 45);
// System.out.println("consigne "+this.drone.speedBF);
}
/** @param args */
public static void main(String[] args) {
ARDroneEntity myDrone = new ARDroneEntity();
boolean stopScenario;
boolean lowBattery;
myDrone.setSpeed(40);
myDrone.setName("drone");
myDrone.setInetAdress("192.168.1.1");
myDrone.setId(1);
myDrone.connect();
Control myController = myDrone.getController();
try {
Thread.sleep(1000);
} catch (Exception e) {
}
stopScenario = myDrone.testEmergency();
if (!stopScenario) {
lowBattery = myDrone.testBatterie();
if (!lowBattery) {
/*
* Scenario begins here
*
* Use the method with time arguments in millisec
* An optionnal second argument is the movement speed in % (default value is 40%)
*
* The combined move needs 4 arguments: time in millisec, speed in % [forward(-) or backward(+)],
* speed in % [down(-) or up (+)], speed in % [spin left(-) or right(+)]
* In the following example the drone will go forward at 40%, go up at 30% and spin right at 50% during 2 seconds
* myDrone.combinedMoves(2000, -40, 30, 50);
*
*/
myDrone.trim();
myDrone.takeoff();
myDrone.hover(1500);
/*
myDrone.goForward(800, 30);
myDrone.goBackward(800, 30);
myDrone.hover(1000);
myDrone.goForward(1500, 10);
myDrone.goBackward(2000, 10);
myDrone.hover(1000);
myDrone.goLeft(600);
myDrone.goRight(800);
myDrone.up(800);
myDrone.down(600);
myDrone.spinLeft(1000);
myDrone.spinRight(1200);
myDrone.combinedMoves(2000, -40, 30, 50);
myDrone.hover(1500);
*/
myDrone.landing();
/*
* The scenario ends here
*/
} else {
for (int i = 0; i < 5; i++) {
System.out.println("\n LOW BATTERY");
}
}
}
myDrone.Disconnect();
}
