From 1d0b4ce59f984f26376d09a2686a8b5762a4ce86 Mon Sep 17 00:00:00 2001
From: Laurent Eschenauer <laurent@eschenauer.be>
Date: Sat, 22 Jun 2013 21:31:18 +0200
Subject: [PATCH] Refactored controller

---
 lib/Controller.js | 81 +++++++++++++++++++----------------------------
 1 file changed, 33 insertions(+), 48 deletions(-)

diff --git a/lib/Controller.js b/lib/Controller.js
index d506ad5..876d83b 100644
--- a/lib/Controller.js
+++ b/lib/Controller.js
@@ -23,10 +23,10 @@ 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.35);
-    this._pid_y   = new PID(0.5,  0, 0.35);
-    this._pid_z   = new PID(0.6,  0, 0.15);
-    this._pid_yaw = new PID(1.0,  0, 0.5);
+    this._pid_x   = new PID(0.3, 0, 0.1);
+    this._pid_y   = new PID(0.3, 0, 0.1);
+    this._pid_z   = new PID(0.5, 0, 0.1);
+    this._pid_yaw = new PID(1.0, 0, 0.3);
 
     // kalman filter is used for the drone state estimation
     this._ekf     = new EKF(options);
@@ -174,9 +174,6 @@ Controller.prototype._control = function(d) {
     // Do not control if no known state or no goal defines
     if (this._goal == null || this._state ==  null) return;
 
-    // Initialize control commands to zero
-    var ux = uy = uz = uyaw = 0;
-
     // Compute error between current state and goal
     var ex   = (this._goal.x != undefined)   ? this._goal.x   - this._state.x   : 0
       , ey   = (this._goal.y != undefined)   ? this._goal.y   - this._state.y   : 0
@@ -184,31 +181,13 @@ Controller.prototype._control = function(d) {
       , eyaw = (this._goal.yaw != undefined) ? this._goal.yaw - this._state.yaw : 0
       ;
 
-    // Compute control commands
-    if (Math.abs(ex) > EPS_LIN) {
-        ux = this._pid_x.getCommand(ex);
-    }
-    if (Math.abs(ey) > EPS_LIN) {
-        uy = this._pid_y.getCommand(ey);
-    }
-    if (Math.abs(ez) > EPS_LIN) {
-        uz = this._pid_z.getCommand(ez);
-    }
-    if (Math.abs(eyaw) > EPS_ANG) {
-        uyaw = this._pid_yaw.getCommand(eyaw);
-    }
-
-    // If all commands are zero, we have reached our destination;
-    // we go into hover mode and trigger the callback if defined.
-    var now = Date.now();
-    if (ux == 0 && uy == 0 && uz == 0 && uyaw == 0) {
-        // If we have been in this position long enough we consider the target reached
-        if (this._goal.reached) {
-            // do nothing
-        } else if (this._last_ok != 0) {
-            if ((now - this._last_ok) > STABLE_DELAY) {
-                // Set the drone in hover mode
-                this._client.stop();
+    // Check if we are within the target area
+    if ((Math.abs(ex) < EPS_LIN) && (Math.abs(ey) < EPS_LIN) && (Math.abs(ez) < EPS_LIN) && (Math.abs(eyaw) < EPS_ANG)) {
+        // Have we been here before ?
+        if (!this._goal.reached && this._last_ok != 0) {
+            // And for long enough ?
+            if ((Date.now() - this._last_ok) > STABLE_DELAY) {
+                // Mark the goal has reached
                 this._goal.reached = true;
 
                 // We schedule the callback in the near future. This is to make
@@ -222,22 +201,9 @@ Controller.prototype._control = function(d) {
                 this.emit('goalReached', this._state);
             }
         } else {
-            this._last_ok = now;
+            this._last_ok = Date.now();
         }
-    }
-    // Else map controller commands to drone commands
-    else {
-        var yaw  = this._state.yaw;
-        var cx   = within(Math.cos(yaw) * ux - Math.sin(yaw) * uy, -1, 1);
-        var cy   = within(-Math.sin(yaw) * ux + Math.cos(yaw) * uy, -1, 1);
-        var cz   = within(uz, -1, 1);
-        var cyaw = within(uyaw, -1, 1);
-
-        this._client.front(cx);
-        this._client.right(cy);
-        this._client.up(cz);
-        this._client.clockwise(cyaw);
-
+    } else {
         // If we just left the goal, we notify
         if (this._last_ok != 0) {
             // Reset last ok since we are in motion
@@ -247,7 +213,26 @@ Controller.prototype._control = function(d) {
         }
     }
 
-    // Emit the control data for others
+    // Get Raw command from PID
+    var ux = this._pid_x.getCommand(ex);
+    var uy = this._pid_y.getCommand(ey);
+    var uz = this._pid_z.getCommand(ez);
+    var uyaw = this._pid_yaw.getCommand(eyaw);
+
+    // Ceil commands and map them to drone orientation
+    var yaw  = this._state.yaw;
+    var cx   = within(Math.cos(yaw) * ux - Math.sin(yaw) * uy, -1, 1);
+    var cy   = within(-Math.sin(yaw) * ux + Math.cos(yaw) * uy, -1, 1);
+    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,
         goal:    this._goal,