diff --git a/lib/cfkinect.py b/lib/cfkinect.py
index 4c401fe251cb72f808acf63bbdc3db6d64863e37..95b006f023eb51acab9528e3449af427e0f3ca00 100644
--- a/lib/cfkinect.py
+++ b/lib/cfkinect.py
@@ -58,14 +58,17 @@ class KinectPilot():
self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache",
rw_cache=sys.path[1]+"/cache")
- self.kinect = Kinect()
+ self.kinect = Kinect(self)
# 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)
+ self.r_pid = PID_RP(P=0.05, D=1.0, I=0.00025, set_point=0.0)
+ self.p_pid = PID_RP(P=0.1, D=1.0, I=0.00025, set_point=0.0)
+ self.t_pid = PID(P=30.0, D=500.0, I=40.0, set_point=0.0)
+
+ self.sp_x = 320
+ self.sp_y = 240
signal.signal(signal.SIGINT, signal.SIG_DFL)
@@ -83,6 +86,10 @@ class KinectPilot():
"""Convert a percentage to raw thrust"""
return int(65000 * (percentage / 100.0))
+ def set_sp_callback(self, x, y):
+ self.sp_x = x
+ self.sp_y = y
+
def control(self, dry=False):
"""Control loop for Kinect control"""
safety = 10
@@ -90,12 +97,12 @@ class KinectPilot():
(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 = self.r_pid.update(self.sp_x-x)
+ pitch = self.p_pid.update(180-depth)
+ thrust = self.t_pid.update(self.sp_y-y)
roll_sp = -roll
pitch_sp = -pitch
- thrust_sp = thrust+40000
+ thrust_sp = thrust+38000
if (roll_sp > CAP):
roll_sp = CAP
if (roll_sp < -CAP):
@@ -113,19 +120,28 @@ class KinectPilot():
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]
+ #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]
+ #texts = ["R=%.2f,P=%.2f,T=%.2f" % (roll_sp, pitch_sp, thrust_sp),
+ # "R: P=%.2f" % self.r_pid.P_value,
+ # "R: I=%.2f" % self.r_pid.I_value,
+ # "R: D=%.2f" % self.r_pid.D_value,
+ # "P: P=%.2f" % self.p_pid.P_value,
+ # "P: I=%.2f" % self.p_pid.I_value,
+ # "P: D=%.2f" % self.p_pid.D_value]
+ texts = []
self.kinect.show(texts)
+ self.kinect.show_xy_sp(self.sp_x, self.sp_y)
if not dry:
- self._cf.commander.send_setpoint(roll_sp, pitch_sp, 0, thrust_sp)
+ self._cf.commander.send_setpoint(roll_sp, pitch_sp-2.0, 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"""
diff --git a/lib/kinect/kinect.py b/lib/kinect/kinect.py
index 4a9a268f9c55adeacfd0543b9c778446456b213f..1855e874255b8c9cd28c57e0943fb902402aeec6 100644
--- a/lib/kinect/kinect.py
+++ b/lib/kinect/kinect.py
@@ -41,11 +41,24 @@ IMG_HEIGHT = 480
class Kinect:
- def __init__(self):
+ def __init__(self, controller):
self.text = []
- self.writer = cv.CreateVideoWriter("out.avi", cv.CV_FOURCC('F', 'L', 'V', '1'), 25, (IMG_WIDTH, IMG_HEIGHT), True)
+ self.writer = cv.CreateVideoWriter("out.avi", cv.CV_FOURCC('M', 'J', 'P', 'G'), 25, (IMG_WIDTH, IMG_HEIGHT), True)
self.lasttime = time.time()
- return
+ self.sp = (0,0)
+ cv.NamedWindow('RGB', cv.CV_WINDOW_AUTOSIZE)
+ cv.SetMouseCallback('RGB', self.mouse_ev)
+ self.sp_callback = controller.set_sp_callback
+ self.track = False
+
+ def mouse_ev(self, ev, x, y, flag, param):
+ if (ev == cv.CV_EVENT_LBUTTONDOWN):
+ self.track = True
+ if (ev == cv.CV_EVENT_LBUTTONUP):
+ self.track = False
+ if (self.track):
+ print "%d,%d" % (x,y)
+ self.sp_callback(x,y)
def _get_pos_spatial(self, th_img):
moments = cv.Moments(cv.GetMat(th_img))
@@ -87,14 +100,14 @@ class Kinect:
return center_point
def _get_pos(self, img, debug=False):
- th_img = self._get_threashold_image_hsv(img)
+ th_img = self._get_threashold_image_hsv(img, debug=debug)
#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)
+ cv.Circle(img, (cp[0], cp[1]) , 10, cv.CV_RGB(0, 0, 255), 1)
return cp
@@ -106,7 +119,7 @@ class Kinect:
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);
+ cv.InRangeS(imgHSV, cv.Scalar(140, 150, 30), cv.Scalar(180, 255, 255), 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)
@@ -149,7 +162,7 @@ class Kinect:
position = self._get_pos(self.img)
- depth = self._get_depth(depth_img, debug=True)
+ depth = self._get_depth(depth_img, debug=False)
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 1, 1)
@@ -171,8 +184,10 @@ class Kinect:
cv.PutText(self.img, t, (0,offset),font, cv.CV_RGB(255, 0, 0))
offset += 30
+ cv.Circle(self.img, (self.sp[0], self.sp[1]) , 10, cv.CV_RGB(0, 255, 0), 1)
+
cv.ShowImage('RGB', self.img)
- #cv.SaveImage('RGB-%d.png' % (time.time()), self.img)
+ #cv.SaveImage('RGB-%d.png' % (time.time()*100), self.img)
#cv.ShowImage('DEPTH', depth_img)
cv.WriteFrame(self.writer, self.img)
cv.WaitKey(5)
@@ -183,6 +198,9 @@ class Kinect:
except:
return (None, None, None)
+ def show_xy_sp(self, x, y):
+ self.sp = (x,y)
+
def show(self, texts):
self.text = texts
diff --git a/lib/kinect/pid.py b/lib/kinect/pid.py
index 2c49748ec1d50c88e8d0979050808a1b26ad4fa5..87bf8f37471dd8466f14e659d2d465806a3ad677 100644
--- a/lib/kinect/pid.py
+++ b/lib/kinect/pid.py
@@ -31,7 +31,7 @@ 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,
+ Integrator_max=300, Integrator_min=-200, set_point=0.0,
power=1.0):
self.Kp=P
@@ -64,7 +64,7 @@ class PID:
if self.error > 0.0:
self.I_value = self.Integrator * self.Ki
else:
- self.I_value = (self.Integrator * self.Ki)*0.5
+ self.I_value = (self.Integrator * self.Ki)
#self.D_value = self.Kd * ( self.error - self.Derivator)
@@ -94,7 +94,7 @@ class PID:
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,
+ Integrator_max=20000, Integrator_min=-20000, set_point=0.0,
power=1.0):
self.Kp=P
@@ -121,17 +121,13 @@ class PID_RP:
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.I_value = self.Integrator * self.Ki
#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
+ self.Integrator = self.Integrator + self.error
if self.Integrator > self.Integrator_max:
self.Integrator = self.Integrator_max