From da65f748dd7a1e664a34b305ef2b373220ae2311 Mon Sep 17 00:00:00 2001
From: Laurent Eschenauer <laurent@eschenauer.be>
Date: Tue, 25 Jun 2013 21:50:43 +0200
Subject: [PATCH] Iterated on square example; fixed Infinity issue

---
 examples/square.js | 14 +++++++++-----
 lib/Controller.js  | 19 ++++++++++---------
 2 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/examples/square.js b/examples/square.js
index 8b359cb..e1253b4 100644
--- a/examples/square.js
+++ b/examples/square.js
@@ -68,28 +68,32 @@ async.waterfall([
     },
     function(cb){
         console.log("Going to base position");
-        ctrl.go({x: 0, y: 0}, cb);
+        ctrl.go({x: 0, y:0, z: 1.5}, cb);
     },
     function(cb){
         console.log("Going to 1");
-        ctrl.go({x: 1, y: 0}, cb);
+        ctrl.go({x: 2, y: 0}, cb);
     },
     function(cb) {
         console.log("Going to 2");
-        ctrl.go({x: 1, y: 1}, cb);
+        ctrl.go({x: 2, y: 2}, cb);
     },
     function(cb) {
         console.log("Going to 3");
-        ctrl.go({x: 0, y: 1}, cb);
+        ctrl.go({x: 0, y: 2}, cb);
     },
     function(cb) {
         console.log("Going back to 0");
         ctrl.go({x: 0, y: 0}, cb);
     },
+    function(cb) {
+        console.log("Waiting 1 sec");
+        setTimeout(cb, 1000);
+    },
     function(cb) {
         console.log("Landing...");
         ctrl.disable();
-        client.land();
+        client.land(cb);
     }
 ],  function (err, result) {
     if (err) {
diff --git a/lib/Controller.js b/lib/Controller.js
index 439aed0..aa05e76 100644
--- a/lib/Controller.js
+++ b/lib/Controller.js
@@ -23,9 +23,9 @@ function Controller(client, options) {
     this._tag     = options.tag || {x: 0, y: 0, yaw: 0};
 
     // Configure the four PID required to control the drone
-    this._pid_x   = new PID(0.5, 0, 0.15);
-    this._pid_y   = new PID(0.5, 0, 0.15);
-    this._pid_z   = new PID(0.5, 0, 0.15);
+    this._pid_x   = new PID(0.5, 0, 0.35);
+    this._pid_y   = new PID(0.5, 0, 0.35);
+    this._pid_z   = new PID(0.6, 0, 0.35);
     this._pid_yaw = new PID(1.0, 0, 0.30);
 
     // kalman filter is used for the drone state estimation
@@ -226,12 +226,6 @@ Controller.prototype._control = function(d) {
     var cz   = within(uz, -1, 1);
     var cyaw = within(uyaw, -1, 1);
 
-    // Send commands to drone
-    this._client.front(cx);
-    this._client.right(cy);
-    this._client.up(cz);
-    this._client.clockwise(cyaw);
-
     // Emit the control data for auditing
     this.emit('controlData', {
         state:   this._state,
@@ -241,6 +235,13 @@ Controller.prototype._control = function(d) {
         last_ok: this._last_ok,
         tag:     (d.visionDetect && d.visionDetect.nbDetected > 0) ? 1 : 0
     });
+
+    // Send commands to drone
+    if (Math.abs(cx) > 0.01) this._client.front(cx);
+    if (Math.abs(cy) > 0.01) this._client.right(cy);
+    if (Math.abs(cz) > 0.01) this._client.up(cz);
+    if (Math.abs(cyaw) > 0.01) this._client.clockwise(cyaw);
+
 }
 
 function within(x, min, max) {