diff --git a/bin/cfkinect b/bin/cfkinect
new file mode 100755
index 0000000000000000000000000000000000000000..673645857dd3f639825f2f78cb84a829f5caef49
--- /dev/null
+++ b/bin/cfkinect
@@ -0,0 +1,85 @@
+#!/usr/bin/env python2
+
+from __future__ import absolute_import
+
+APP_NAME = "cfkinect"
+
+def init_config_path(isInSource):
+    import sys
+    import os
+    import os.path as _path
+    from os.path import expanduser
+
+    if isInSource:
+        configPath = _path.join(sys.path[0], "..", "conf")
+    else:
+        prefix = expanduser("~")
+
+        if sys.platform == "linux2":
+            if _path.exists(_path.join(prefix, ".local")):
+                configPath = _path.join(prefix, ".local", APP_NAME)
+            else:
+                configPath = _path.join(prefix, "."+APP_NAME)
+        elif sys.platform == "win32":
+            configPath = _path.join(os.environ['APPDATA'], APP_NAME)
+        elif sys.platform == "darwin":
+            from AppKit import NSSearchPathForDirectoriesInDomains
+            # FIXME: Copy-pasted from StackOverflow, not tested!
+            # http://developer.apple.com/DOCUMENTATION/Cocoa/Reference/Foundation/Miscellaneous/Foundation_Functions/Reference/reference.html#//apple_ref/c/func/NSSearchPathForDirectoriesInDomains
+            # NSApplicationSupportDirectory = 14
+            # NSUserDomainMask = 1
+            # True for expanding the tilde into a fully qualified path
+            configPath = path.join(NSSearchPathForDirectoriesInDomains(14, 1, True)[0], APPNAME)
+        else:
+            #Unknown OS, I hope this is good enough
+            configPath = _path.join(prefix, "."+APP_NAME)
+
+    if not _path.exists(configPath): os.makedirs(configPath)
+
+    return configPath
+
+def init_paths():
+    """Make the app work in the source tree.
+       This puts the root module folder in path[0] and the config folder in
+       path[1]"""
+    import sys
+    import os.path as _path
+    
+    sys.path = ["",""] + sys.path
+    inSource = False
+    
+    if hasattr(sys, "frozen"):
+        if sys.frozen in ('dll', 'console_exe', 'windows_exe'):
+            sys.path[0] = _path.normpath(_path.dirname(_path.realpath(sys.executable)))
+        elif frozen in ('macosx_app',):
+            # FIXME: Copy-pasted from StackOverflow, not tested!
+            # py2app:
+            # Notes on how to find stuff on MAC, by an expert (Bob Ippolito):
+            # http://mail.python.org/pipermail/pythonmac-sig/2004-November/012121.html
+            approot = os.environ['RESOURCEPATH']
+    else:
+        prefix = _path.normpath(_path.join(_path.dirname(_path.realpath(__file__)), '..'))
+
+        src_lib = _path.join(prefix, 'lib')
+        share_lib = prefix
+        inSource = False
+        for location in [src_lib, share_lib] + sys.path:
+            main_ui = _path.join(location, 'cfclient', 'ui','main.ui')
+            if _path.exists(main_ui):
+                sys.path[0] = location
+                if location == src_lib:
+                    inSource = True
+                break
+
+    if sys.path[0] == "":
+        raise Exception("Cannot find cfclient install folder!")
+
+    sys.path[1] = init_config_path(inSource)
+    print "Info: Using config path: ", sys.path[1]
+    print "Info: sys.path=", sys.path
+
+
+if __name__ == '__main__':
+    init_paths()
+    import cfkinect
+    cfkinect.main()
diff --git a/lib/cfkinect.py b/lib/cfkinect.py
new file mode 100644
index 0000000000000000000000000000000000000000..4c401fe251cb72f808acf63bbdc3db6d64863e37
--- /dev/null
+++ b/lib/cfkinect.py
@@ -0,0 +1,156 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __                           
+#  +------+      / __ )(_) /_______________ _____  ___ 
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2013 Bitcraze AB
+#
+#  Crazyflie Nano Quadcopter Client
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#  
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+
+"""
+Kinect controller
+"""
+
+import sys
+import os
+import logging
+import signal
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+import cfclient.utils
+from kinect.kinect import Kinect
+from kinect.pid import PID, PID_RP
+
+from cfclient.utils.config import Config
+
+# Roll/pitch limit
+CAP = 15.0
+# Thrust limit
+TH_CAP = 55000
+
+class KinectPilot():
+    """Crazyflie Kinect Pilot"""
+
+    def __init__(self):
+        """Initialize the headless client and libraries"""
+        cflib.crtp.init_drivers()
+
+        self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache",
+                             rw_cache=sys.path[1]+"/cache")
+
+        self.kinect = Kinect()
+
+        # Create PID controllers for piloting the Crazyflie
+        # The Kinect camera is in the back of the Crazyflie so
+        # pitch = depth, roll = x, thrust = y
+        self.r_pid = PID_RP(P=0.04, D=0.25, set_point=0.0)
+        self.p_pid = PID_RP(P=0.20, D=0.25, set_point=0.0)
+        self.t_pid = PID(P=15.0, D=1000.0, I=40.0, set_point=0.0)
+
+        signal.signal(signal.SIGINT, signal.SIG_DFL) 
+
+    def connect_crazyflie(self, link_uri):
+        """Connect to a Crazyflie on the given link uri"""
+        self._cf.connectionFailed.add_callback(self._connection_failed)
+        self._cf.open_link(link_uri)
+
+    def _connection_failed(self, link, message):
+        """Callback for a failed Crazyflie connection"""
+        print "Connection failed on {}: {}".format(link, message)
+        sys.exit(-1)
+
+    def _p2t(self, percentage):
+        """Convert a percentage to raw thrust"""
+        return int(65000 * (percentage / 100.0))
+
+    def control(self, dry=False):
+        """Control loop for Kinect control"""
+        safety = 10
+        while True:
+            (x,y,depth) = self.kinect.find_position()
+            if x and y and depth:
+                safety = 10
+                roll = self.r_pid.update(320-x)
+                pitch = self.p_pid.update(170-depth)
+                thrust = self.t_pid.update(120-y)
+                roll_sp = -roll
+                pitch_sp = -pitch
+                thrust_sp = thrust+40000
+                if (roll_sp > CAP):
+                    roll_sp = CAP
+                if (roll_sp < -CAP):
+                    roll_sp = -CAP             
+
+                if (pitch_sp > CAP):
+                    pitch_sp = CAP
+                if (pitch_sp < -CAP):
+                    pitch_sp = -CAP             
+
+                if (thrust_sp > TH_CAP):
+                    thrust_sp = TH_CAP
+                if (thrust_sp < 0):
+                    thrust_sp = 0
+
+                print self.t_pid.error
+
+                texts = ["R=%.2f,P=%.2f,T=%.2f" % (roll_sp, pitch_sp, thrust_sp),
+                         "TH: P=%.2f" % self.t_pid.P_value,
+                         "TH: D=%.2f" % self.t_pid.D_value,
+                         "TH: I=%.2f" % self.t_pid.I_value,
+                         "TH: e=%.2f" % self.t_pid.error]
+                self.kinect.show(texts)
+                if not dry:
+                    self._cf.commander.send_setpoint(roll_sp, pitch_sp, 0, thrust_sp)
+            else:
+                safety = safety - 1
+            if safety < 0 and not dry:
+                self._cf.commander.send_setpoint(0, 0, 0, 0)
+                break
+
+def main():
+    """Main Crazyflie Kinect application"""
+    import argparse
+
+    parser = argparse.ArgumentParser(prog="cfkinect")
+    parser.add_argument("-u", "--uri", action="store", dest="uri", type=str,
+                        default="radio://0/10/250K",
+                        help="URI to use for connection to the Crazyradio"
+                             " dongle, defaults to radio://0/10/250K")
+    parser.add_argument("-y", "--dry", dest="dry", action="store_true",
+                        help="Do not send commands to Crazyflie")
+    parser.add_argument("-d", "--debug", action="store_true", dest="debug",
+                        help="Enable debug output")
+    (args, unused) = parser.parse_known_args()
+
+    if args.debug:
+        logging.basicConfig(level=logging.DEBUG)
+    else:
+        logging.basicConfig(level=logging.INFO)
+
+    kinect_pilot = KinectPilot()
+
+    if not args.dry:
+        kinect_pilot.connect_crazyflie(link_uri=args.uri)
+
+    kinect_pilot.control(args.dry)
+
diff --git a/lib/kinect/__init__.py b/lib/kinect/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e30fbc0704efd2a82e75b06d09ea81bf0752a73f
--- /dev/null
+++ b/lib/kinect/__init__.py
@@ -0,0 +1,27 @@
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2013 Bitcraze AB
+#
+#  Crazyflie Nano Quadcopter Client
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+
+#  You should have received a copy of the GNU General Public License along with
+#  this program; if not, write to the Free Software Foundation, Inc., 51
+#  Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+
+
diff --git a/lib/kinect/frame_convert.py b/lib/kinect/frame_convert.py
new file mode 100644
index 0000000000000000000000000000000000000000..9a674130c0261ea436f82d1798fbedc9955f1db8
--- /dev/null
+++ b/lib/kinect/frame_convert.py
@@ -0,0 +1,60 @@
+import numpy as np
+
+
+def pretty_depth(depth):
+    """Converts depth into a 'nicer' format for display
+
+    This is abstracted to allow for experimentation with normalization
+
+    Args:
+        depth: A numpy array with 2 bytes per pixel
+
+    Returns:
+        A numpy array that has been processed whos datatype is unspecified
+    """
+    np.clip(depth, 0, 2**10 - 1, depth)
+    depth >>= 2
+    depth = depth.astype(np.uint8)
+    return depth
+
+
+def pretty_depth_cv(depth):
+    """Converts depth into a 'nicer' format for display
+
+    This is abstracted to allow for experimentation with normalization
+
+    Args:
+        depth: A numpy array with 2 bytes per pixel
+
+    Returns:
+        An opencv image who's datatype is unspecified
+    """
+    import cv
+    depth = pretty_depth(depth)
+    image = cv.CreateImageHeader((depth.shape[1], depth.shape[0]),
+                                 cv.IPL_DEPTH_8U,
+                                 1)
+    cv.SetData(image, depth.tostring(),
+               depth.dtype.itemsize * depth.shape[1])
+    return image
+
+
+def video_cv(video):
+    """Converts video into a BGR format for opencv
+
+    This is abstracted out to allow for experimentation
+
+    Args:
+        video: A numpy array with 1 byte per pixel, 3 channels RGB
+
+    Returns:
+        An opencv image who's datatype is 1 byte, 3 channel BGR
+    """
+    import cv
+    video = video[:, :, ::-1]  # RGB -> BGR
+    image = cv.CreateImageHeader((video.shape[1], video.shape[0]),
+                                 cv.IPL_DEPTH_8U,
+                                 3)
+    cv.SetData(image, video.tostring(),
+               video.dtype.itemsize * 3 * video.shape[1])
+    return image
diff --git a/lib/kinect/kinect.py b/lib/kinect/kinect.py
new file mode 100644
index 0000000000000000000000000000000000000000..4a9a268f9c55adeacfd0543b9c778446456b213f
--- /dev/null
+++ b/lib/kinect/kinect.py
@@ -0,0 +1,188 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __                           
+#  +------+      / __ )(_) /_______________ _____  ___ 
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2013 Bitcraze AB
+#
+#  Crazyflie Nano Quadcopter Client
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#  
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+
+"""
+Kinect/OpenCV interface
+"""
+
+from freenect import sync_get_depth as get_depth, sync_get_video as get_video
+from frame_convert import video_cv, pretty_depth_cv
+import cv  
+import numpy as np
+import time
+
+IMG_WIDTH = 640
+IMG_HEIGHT = 480
+
+class Kinect:
+
+    def __init__(self):
+        self.text = []
+        self.writer = cv.CreateVideoWriter("out.avi", cv.CV_FOURCC('F', 'L', 'V', '1'), 25, (IMG_WIDTH, IMG_HEIGHT), True)
+        self.lasttime = time.time()
+        return
+
+    def _get_pos_spatial(self, th_img):
+        moments = cv.Moments(cv.GetMat(th_img))
+        mom10 = cv.GetSpatialMoment(moments, 1, 0)
+        mom01 = cv.GetSpatialMoment(moments, 0, 1)
+        area = cv.GetCentralMoment(moments, 0, 0)
+
+        if area > 10:
+            pos = [int(mom10/area), int(mom01/area)]
+        else:
+            pos = None
+
+        return pos
+
+    def _get_pos_countour(self, th_img):
+        storage = cv.CreateMemStorage(0)
+        contour = None
+        ci = cv.CreateImage(cv.GetSize(th_img), 8, 1)
+        ci = cv.CloneImage(th_img)
+
+        contour = cv.FindContours(th_img, storage, cv.CV_RETR_CCOMP, cv.CV_CHAIN_APPROX_SIMPLE)
+        points = []
+
+        while contour:
+            bound_rect = cv.BoundingRect(list(contour))
+            contour = contour.h_next()
+            pt1 = (bound_rect[0], bound_rect[1])
+            pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
+            points.append(pt1)
+            points.append(pt2)
+            #cv.Rectangle(img, pt1, pt2, cv.CV_RGB(255,0,0), 1)
+
+        center_point = None
+
+        if len(points):
+            center_point = reduce(lambda a, b: ((a[0] + b[0]) / 2, (a[1] + b[1]) / 2), points)
+            #cv.Circle(depth, center_point, 10, cv.CV_RGB(255, 255, 255), 1)
+
+        return center_point
+        
+    def _get_pos(self, img, debug=False):
+        th_img = self._get_threashold_image_hsv(img)
+        #sp = self._get_pos_spatial(th_img)
+        cp = self._get_pos_countour(th_img)
+
+        #if sp:
+        #    cv.Circle(img, (sp[0], sp[1]) , 10, cv.CV_RGB(255, 255, 255), 1)
+        if cp:        
+            cv.Circle(img, (cp[0], cp[1]) , 10, cv.CV_RGB(255, 0, 0), 1)
+
+        return cp
+
+    def _get_threashold_image_hsv(self, img, debug=False):
+        """Get the binary threashold image using a HSV method"""
+        imgHSV = cv.CreateImage(cv.GetSize(img), 8, 3);
+        cv.CvtColor(img, imgHSV, cv.CV_BGR2HSV);
+        cv.Smooth(imgHSV, imgHSV, cv.CV_GAUSSIAN, 3, 0)
+        img_th = cv.CreateImage(cv.GetSize(img), 8, 1);
+
+        # Filter out red colors
+        cv.InRangeS(imgHSV, cv.Scalar(160, 190, 30), cv.Scalar(180, 255, 200), img_th);
+        # Fix the image a bit by eroding and dilating
+        eroded = cv.CreateImage(cv.GetSize(img), 8, 1);
+        cv.Erode( img_th, eroded, None, 1)
+        dilated = cv.CreateImage(cv.GetSize(img), 8, 1);
+        cv.Dilate( eroded, dilated, None, 4)   
+
+        if debug:
+            cv.ShowImage('HSV', imgHSV)
+            cv.ShowImage('threas', img_th)
+            cv.SaveImage('threas-%d.png' % (time.time()), img_th)
+            cv.ShowImage('eroded', eroded)
+            cv.ShowImage('dialated', dilated)
+
+        return dilated
+
+    def _get_depth(self, depth_image, debug=False):
+        """Get the depth reading from the Kinect"""
+        depth = None
+
+        # Only use part of the span to avoid anything else than the Crazyflie
+        img_th = cv.CreateImage(cv.GetSize(depth_image), 8, 1);
+        cv.InRangeS(depth_image, 10, 210, img_th);
+
+        # Calculate the mean depth
+        depth = cv.Avg(depth_image, img_th)[0]
+
+        if debug:
+            font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 1, 1) 
+            s2 = "%d" % depth
+            cv.PutText(img_th, s2, (0,60),font, 200)
+            cv.ShowImage('depth th', img_th)
+
+        return depth
+
+
+    def find_position(self):
+        (kinect_depth,_), (rgb,_) = get_depth(), get_video()
+        self.img = video_cv(rgb)
+        depth_img = pretty_depth_cv(kinect_depth)
+ 
+        position = self._get_pos(self.img)
+
+        depth = self._get_depth(depth_img, debug=True)
+
+        font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 1, 1) 
+
+        fps = 1/(time.time() - self.lasttime)
+        s1 = "FPS:%.1f" % fps
+        self.lasttime = time.time()
+        cv.PutText(self.img,s1, (0,30),font, cv.CV_RGB(255, 0, 0))
+
+        dt = "Depth: %d" % depth
+        if position:
+            pt = "Pos: X=%d Y=%d" % (position[0], position[1])
+        else:
+            pt = "Pos: N/A"
+        cv.PutText(self.img, dt, (0,60),font, cv.CV_RGB(255, 0, 0))
+        cv.PutText(self.img, pt, (0,90),font, cv.CV_RGB(255, 0, 0))
+
+        offset = 120
+        for t in self.text:
+            cv.PutText(self.img, t, (0,offset),font, cv.CV_RGB(255, 0, 0))
+            offset += 30
+
+        cv.ShowImage('RGB', self.img)
+        #cv.SaveImage('RGB-%d.png' % (time.time()), self.img)
+        #cv.ShowImage('DEPTH', depth_img)
+        cv.WriteFrame(self.writer, self.img)
+        cv.WaitKey(5)
+
+        #cv.ShowImage('depth_mask', depth_mask)
+        try:
+            return (position[0], position[1], depth)
+        except:
+            return (None, None, None)
+
+    def show(self, texts):
+        self.text = texts
+
diff --git a/lib/kinect/pid.py b/lib/kinect/pid.py
new file mode 100644
index 0000000000000000000000000000000000000000..2c49748ec1d50c88e8d0979050808a1b26ad4fa5
--- /dev/null
+++ b/lib/kinect/pid.py
@@ -0,0 +1,155 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __                           
+#  +------+      / __ )(_) /_______________ _____  ___ 
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2013 Bitcraze AB
+#
+#  Crazyflie Nano Quadcopter Client
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#  
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+
+import math
+
+class PID:
+
+    def __init__(self, P=1.0, I=0.0, D=10.0, Derivator=0, Integrator=0,
+                 Integrator_max=200, Integrator_min=-200, set_point=0.0,
+                 power=1.0):
+
+        self.Kp=P
+        self.Ki=I
+        self.Kd=D
+        self.Derivator=Derivator
+        self.power = power
+        self.Integrator=Integrator
+        self.Integrator_max=Integrator_max
+        self.Integrator_min=Integrator_min
+        self.last_error = 0.0
+        self.last_value = 0.0
+
+        self.set_point=set_point
+        self.error=0.0
+
+    def update(self,current_value):
+        """
+        Calculate PID output value for given reference input and feedback
+        """
+
+        self.error = self.set_point - current_value
+
+        self.P_value = self.Kp * self.error
+        if (self.last_value >= current_value):
+            change = self.error - self.last_error
+        else:
+            change = 0.0
+
+        if self.error > 0.0:
+            self.I_value = self.Integrator * self.Ki
+        else:
+            self.I_value = (self.Integrator * self.Ki)*0.5
+
+
+        #self.D_value = self.Kd * ( self.error - self.Derivator)
+        self.D_value = self.Kd * change
+        self.Derivator = self.error
+
+        self.Integrator = self.Integrator + self.error/200.0
+
+        if self.Integrator > self.Integrator_max:
+            self.Integrator = self.Integrator_max
+        elif self.Integrator < self.Integrator_min:
+            self.Integrator = self.Integrator_min
+
+        self.last_error = self.error
+        self.last_value = current_value
+
+        PID = self.P_value + self.I_value + self.D_value
+
+        return PID
+
+    def set_point(self,set_point):
+        """Initilize the setpoint of PID"""
+        self.set_point = set_point
+        self.Integrator=0
+        self.Derivator=0
+
+class PID_RP:
+
+    def __init__(self, P=1.0, I=0.0, D=10.0, Derivator=0, Integrator=0,
+                 Integrator_max=250, Integrator_min=-100, set_point=0.0,
+                 power=1.0):
+
+        self.Kp=P
+        self.Ki=I
+        self.Kd=D
+        self.Derivator=Derivator
+        self.power = power
+        self.Integrator=Integrator
+        self.Integrator_max=Integrator_max
+        self.Integrator_min=Integrator_min
+        self.last_error = 0.0
+        self.last_value = 0.0
+
+        self.set_point=set_point
+        self.error=0.0
+
+    def update(self,current_value):
+        """
+        Calculate PID output value for given reference input and feedback
+        """
+
+        self.error = self.set_point - current_value
+
+        self.P_value = self.Kp * self.error
+        change = self.error - self.last_error
+        
+        if self.error > 0.0:
+            self.I_value = self.Integrator * self.Ki
+        else:
+            self.I_value = (self.Integrator * self.Ki)*0.5
+
+
+        #self.D_value = self.Kd * ( self.error - self.Derivator)
+        self.D_value = self.Kd * change
+        self.Derivator = self.error
+
+        self.Integrator = self.Integrator + self.error/200.0
+
+        if self.Integrator > self.Integrator_max:
+            self.Integrator = self.Integrator_max
+        elif self.Integrator < self.Integrator_min:
+            self.Integrator = self.Integrator_min
+
+        self.last_error = self.error
+        self.last_value = current_value
+
+        PID = self.P_value + self.I_value + self.D_value
+
+        return PID
+
+    def set_point(self,set_point):
+        """
+        Initilize the setpoint of PID
+        """
+        self.set_point = set_point
+        self.Integrator=0
+        self.Derivator=0
+