@Override
public void update(PlaneState ps) {
if (ps.isReset()) {
pid_t.reset();
pid_a.reset();
pid_e.reset();
}
speed = ps.getSpeed();
roll = ps.getRoll();
pitch = ps.getPitch();
altitude = ps.getAlt();
}
@Override
public double[] getControls() {
JXInputManager.updateFeatures();
double e = -ax_y.getValue();
if (Math.abs(e) < 0.1) {
e = 0.0;
}
double a = ax_x.getValue();
if (Math.abs(a) < 0.1) {
a = 0;
}
double t = ax_t.getValue();
if (Math.abs(t) < 0.2) {
t = 0.2 * pid_t.update(speed);
} else {
t = 0.5 + t * 0.5;
}
if (t > 0.1 || altitude > 5) {
pid_t.setRef(10 * (1 + t));
t = pid_t.update(speed);
t = (t + 1) / 2;
} else {
t = 0;
}
pid_a.setRef(a * 8);
if (altitude > 5) {
a = pid_a.update(roll * 180 / Math.PI);
} else {
// pid_a.update(roll * 180 / Math.PI);
}
if (altitude > 5) {
pid_h.setRef(25 + e * 10);
double h = pid_h.update(altitude);
pid_e.setRef(-15 * h);
e = pid_e.update(pitch * 180 / Math.PI);
// Compensatre for roll
double r = 4; // r = m*g/2k
e -= r * roll * roll;
}
// t= -1; // Debug
// System.out.println(String.format("roll: %3.2f A: %2.2f", roll, a));
// System.out.println(String.format("speed: %3.2f T: %2.2f", speed, t));
double[] result = {e + a, e - a, t};
// System.out.println(String.format("t: %1.3f",t));
// System.out.println(String.format("e: %1.3f", e));
// System.out.println(String.format("h: %1.3f", h));
return result;
}