From 64308aa3b7dbfd348a879b13bab33b0eafc30293 Mon Sep 17 00:00:00 2001 From: Jens Ahrensfeld Date: Tue, 25 Oct 2016 18:05:21 +0000 Subject: [PATCH] - improved Ev3Server - implemted PID controller in Lasertrack. Provide motor power directly to EV3 (use lasertracker.ev3) git-svn-id: http://moon:8086/svn/software/trunk/projects/opencv@343 b431acfa-c32f-4a4a-93f1-934dc6c82436 --- ev3/Ev3Server.py | 11 +++---- lasertrack.py | 74 ++++++++++++++++++++++++++++++++++++++---------- 2 files changed, 65 insertions(+), 20 deletions(-) diff --git a/ev3/Ev3Server.py b/ev3/Ev3Server.py index f892dee..d0ebfe0 100755 --- a/ev3/Ev3Server.py +++ b/ev3/Ev3Server.py @@ -12,6 +12,7 @@ class LocalServer(object): self.thread = None self.cancel = False self.sockTcpEv3 = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.sockTcpEv3.settimeout(1.0) self.sockTcpEv3.connect((ip, port)) print ("EV3 connected") self.sockTcpEv3.send(b'ET /target?sn=') @@ -59,7 +60,7 @@ class LocalServer(object): if sock == conn: data = sock.recv(1024) if data: - print ("Client: Received data (" + str(len(data)) + ")") +# print ("Client: Received data (" + str(len(data)) + ")") self.sockTcpEv3.send(data) # send to EV3 else: print ('Client: Connection closed:' + str(addr)) @@ -67,7 +68,7 @@ class LocalServer(object): if sock == self.sockTcpEv3: data = sock.recv(1024) if data: - print ("EV3: Received data (" + str(len(data)) + ")") +# print ("EV3: Received data (" + str(len(data)) + ")") conn.send(data) # send to Client else: print ('EV3: Connection closed:' + str(addr)) @@ -88,7 +89,7 @@ if ("__main__" == __name__): UDP_IP = "0.0.0.0" UDP_PORT = 3015 sockUdp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - sockUdp.settimeout(10.0) + sockUdp.settimeout(30.0) sockUdp.bind((UDP_IP, UDP_PORT)) localTcpServer = None while True: @@ -114,17 +115,17 @@ if ("__main__" == __name__): # print (keyValueDict) if keyValueDict['Protocol'] == 'EV3': - print ("EV3 discovered") sockUdp.sendto(b'A', addr) if localTcpServer is None: + print ("EV3 discovered") try: localTcpServer = LocalServer(addr[0], int(keyValueDict['Port'])) localTcpServer.start() except Exception as e: print (e) if localTcpServer is not None: - localTcpServer = None del localTcpServer + localTcpServer = None if localTcpServer is not None: localTcpServer.stop() diff --git a/lasertrack.py b/lasertrack.py index 9cf921b..47d6775 100755 --- a/lasertrack.py +++ b/lasertrack.py @@ -38,6 +38,12 @@ ap.add_argument("-x", "--width", type=int, default=320, help="width") ap.add_argument("-y", "--height", type=int, default=240, help="height") +ap.add_argument("-kp", "--kp", type=float, default=1.5, + help="kp") +ap.add_argument("-ki", "--ki", type=float, default=0.1, + help="ki") +ap.add_argument("-kd", "--kd", type=float, default=5.0, + help="kd") args = vars(ap.parse_args()) videoFile = args["filename"] @@ -47,22 +53,50 @@ with_ev3 = args["with_ev3"] == 'on' width = args["width"] height = args["height"] connAddr = args["conn_addr"] +kpid = (args["kp"], args["ki"], args["kd"]) -class MyRgbProcess(RgbProcess): - def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None, brick=None): - super(MyRgbProcess, self).__init__(name, resolution, numEntries, next) +class PidControl(object): + def __init__(self, kpid): + self.kpid = kpid + self.errorLast = 0 + self.errorAccu = 0 + + def paramSet(self, kpid): + self.kpid = kpid + + def paramGet(self): + return self.kpid + + def reset(self): + self.errorLast = 0 + self.errorAccu = 0 + + def process(self, error): + self.errorAccu += self.kpid[1]*error + d = self.kpid[2]*(error - self.errorLast) + self.errorLast = error + + return self.kpid[0]*error + self.errorAccu + d + +class LaserTrack(RgbProcess): + def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None, brick=None, kpid=(0,0,0)): + super(LaserTrack, self).__init__(name, resolution, numEntries, next) self.brick = brick self.isVisible = False self.frameSkip = 3 self.frameSkipCounter = 0 + self.pidCtrlX = PidControl(kpid) + self.pidCtrlY = PidControl(kpid) + def onFirstFrame(self): self.sendBrick(0, 0) def onLastFrame(self): + print('onLastFrame') self.sendBrick(0, 0) def onProcess(self): - data = self.read(1.0) + data = self.read(0.1) if data is not None: gray = cv2.cvtColor(data,cv2.COLOR_BGR2GRAY) @@ -101,20 +135,30 @@ class MyRgbProcess(RgbProcess): break # Is object found? + errorX = 0 + errorY = 0 if rect is not None: if not self.isVisible: print ("Visible") self.isVisible = True img1_objects = cv2.rectangle(img1_objects,rect[0],rect[1],(255,0,0),1) - errorX = float(x)-self.res[0]/2 - errorY = float(y)-self.res[1]/2 - self.sendBrick(errorX/2, errorY/2) + errorX = -100.0*(float(x)/self.res[0] - 0.5) + errorY = -100.0*(float(y)/self.res[1] - 0.5) else: if self.isVisible: print ("NOT Visible") - self.sendBrick(0, 0) self.isVisible = False + # update PID + motorX = self.pidCtrlX.process(errorX) + motorY = self.pidCtrlY.process(errorY) + + # Update brick + if self.isVisible: + self.sendBrick(motorX, motorY) + else: + self.sendBrick(0, 0) + # Update preview showFrame = False if self.frameSkipCounter > 0: @@ -130,12 +174,12 @@ class MyRgbProcess(RgbProcess): if self.next is not None: self.next.write(data) - def sendBrick(self, errorX, errorY): + def sendBrick(self, motorX, motorY): if self.brick is not None: - data = struct.pack("f", errorX) - ev3.system_command.write_mailbox(self.brick, 'ErrorX', data) - data = struct.pack("f", errorY) - ev3.system_command.write_mailbox(self.brick, 'ErrorY', data) + data = struct.pack("f", motorX) + ev3.system_command.write_mailbox(self.brick, 'MotorX', data) + data = struct.pack("f", motorY) + ev3.system_command.write_mailbox(self.brick, 'MotorY', data) def getROI(self, img, x, y, w, h): w2 = int((w-1)/2) @@ -151,7 +195,7 @@ if with_ev3: with ev3.EV3(connAddr) as brick: videoSource = VideoSource(videoFile, (width, height,3), framerate) - proc = MyRgbProcess('MyRgbProcess', (width, height, 3), 4, brick=brick) + proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, brick=brick, kpid=kpid) videoSource.processorAdd(proc) videoSource.start() @@ -161,7 +205,7 @@ if with_ev3: proc.stop() else: videoSource = VideoSource(videoFile, (width, height,3), framerate) - proc = MyRgbProcess('MyRgbProcess', (width, height, 3), 4) + proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, kpid=kpid) videoSource.processorAdd(proc) videoSource.start()