jens/opencv
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

- improved ev3Server

Browse Source 
- improved laserTracker



git-svn-id: http://moon:8086/svn/software/trunk/projects/opencv@345 b431acfa-c32f-4a4a-93f1-934dc6c82436
This commit is contained in:
jens
2016-10-30 10:47:28 +00:00
parent 6d9a535671
commit 7580c7376d
2 changed files with 22 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ev3/Ev3Server.py
+2 -1
View File
@@ -116,10 +116,11 @@ if ("__main__" == __name__):
# print (keyValueDict) # print (keyValueDict)
if keyValueDict['Protocol'] == 'EV3': if keyValueDict['Protocol'] == 'EV3':
sockUdp.sendto(b'A', addr)
if localTcpServer is None: if localTcpServer is None:
sockUdp.sendto(b'A', addr)
print ("EV3 discovered") print ("EV3 discovered")
try: try:
time.sleep(0.2)
localTcpServer = LocalServer(addr[0], int(keyValueDict['Port'])) localTcpServer = LocalServer(addr[0], int(keyValueDict['Port']))
localTcpServer.start() localTcpServer.start()
except Exception as e: except Exception as e:
lasertrack.py
+20 -9
View File
@@ -28,6 +28,8 @@ ap.add_argument("-n", "--seconds", type=int, default=10,
help="# of seconds to run") help="# of seconds to run")
ap.add_argument("-r", "--framerate", type=int, default=30, ap.add_argument("-r", "--framerate", type=int, default=30,
help="Framerate in frame/s") help="Framerate in frame/s")
ap.add_argument("-p", "--with-preview", type=str, default='off',
help="Show camera preview")
ap.add_argument("-e", "--with-ev3", type=str, default='off', ap.add_argument("-e", "--with-ev3", type=str, default='off',
help="Connect to ev3") help="Connect to ev3")
ap.add_argument("-c", "--conn-addr", type=str, default='127.0.0.1:5555', ap.add_argument("-c", "--conn-addr", type=str, default='127.0.0.1:5555',
@@ -38,17 +40,18 @@ ap.add_argument("-x", "--width", type=int, default=320,
help="width") help="width")
ap.add_argument("-y", "--height", type=int, default=240, ap.add_argument("-y", "--height", type=int, default=240,
help="height") help="height")
ap.add_argument("-kp", "--kp", type=float, default=1.5, ap.add_argument("-kp", "--kp", type=float, default=2.0, # 2.0
help="kp") help="kp")
ap.add_argument("-ki", "--ki", type=float, default=0.1, ap.add_argument("-ki", "--ki", type=float, default=0.1, # 0.2
help="ki") help="ki")
ap.add_argument("-kd", "--kd", type=float, default=5.0, ap.add_argument("-kd", "--kd", type=float, default=4.0, # 8.0
help="kd") help="kd")
args = vars(ap.parse_args()) args = vars(ap.parse_args())
videoFile = args["filename"] videoFile = args["filename"]
framerate = args["framerate"] framerate = args["framerate"]
seconds = args["seconds"] seconds = args["seconds"]
with_preview = args["with_preview"] == 'on'
with_ev3 = args["with_ev3"] == 'on' with_ev3 = args["with_ev3"] == 'on'
width = args["width"] width = args["width"]
height = args["height"] height = args["height"]
@@ -76,17 +79,20 @@ class PidControl(object):
d = self.kpid[2]*(error - self.errorLast) d = self.kpid[2]*(error - self.errorLast)
self.errorLast = error self.errorLast = error
return self.kpid[0]*error + self.errorAccu + d return self.kpid[0]*(error + self.errorAccu + d)
class LaserTrack(RgbProcess): class LaserTrack(RgbProcess):
def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None, brick=None, kpid=(0,0,0)): def __init__(self, name, resolution=(320, 240, 3), numEntries=2, next=None, brick=None, kpid=(0,0,0), preview=None):
super(LaserTrack, self).__init__(name, resolution, numEntries, next) super(LaserTrack, self).__init__(name, resolution, numEntries, next)
self.motorX = -1000
self.motorY = -1000
self.brick = brick self.brick = brick
self.isVisible = False self.isVisible = False
self.frameSkip = 3 self.frameSkip = 3
self.frameSkipCounter = 0 self.frameSkipCounter = 0
self.pidCtrlX = PidControl(kpid) self.pidCtrlX = PidControl(kpid)
self.pidCtrlY = PidControl(kpid) self.pidCtrlY = PidControl(kpid)
self.preview = preview
def onFirstFrame(self): def onFirstFrame(self):
self.sendBrick(0, 0) self.sendBrick(0, 0)
@@ -165,7 +171,7 @@ class LaserTrack(RgbProcess):
self.frameSkipCounter -= 1 self.frameSkipCounter -= 1
else: else:
self.frameSkipCounter = self.frameSkip-1 self.frameSkipCounter = self.frameSkip-1
showFrame = True showFrame = self.preview
if showFrame: if showFrame:
cv2.imshow('Objects', img1_objects) cv2.imshow('Objects', img1_objects)
@@ -175,7 +181,12 @@ class LaserTrack(RgbProcess):
self.next.write(data) self.next.write(data)
def sendBrick(self, motorX, motorY): def sendBrick(self, motorX, motorY):
if self.brick is not None: doSend = motorX != self.motorX
doSend |= motorY != self.motorY
self.motorX = motorX
self.motorY = motorY
if self.brick is not None and doSend:
data = struct.pack("f", motorX) data = struct.pack("f", motorX)
ev3.system_command.write_mailbox(self.brick, 'MotorX', data) ev3.system_command.write_mailbox(self.brick, 'MotorX', data)
data = struct.pack("f", motorY) data = struct.pack("f", motorY)
@@ -195,7 +206,7 @@ if with_ev3:
with ev3.EV3(connAddr) as brick: with ev3.EV3(connAddr) as brick:
videoSource = VideoSource(videoFile, (width, height,3), framerate) videoSource = VideoSource(videoFile, (width, height,3), framerate)
proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, brick=brick, kpid=kpid) proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, brick=brick, kpid=kpid, preview=with_preview)
videoSource.processorAdd(proc) videoSource.processorAdd(proc)
videoSource.start() videoSource.start()
@@ -205,7 +216,7 @@ if with_ev3:
proc.stop() proc.stop()
else: else:
videoSource = VideoSource(videoFile, (width, height,3), framerate) videoSource = VideoSource(videoFile, (width, height,3), framerate)
proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, kpid=kpid) proc = LaserTrack('LaserTrackProcess', (width, height, 3), 4, kpid=kpid, preview=with_preview)
videoSource.processorAdd(proc) videoSource.processorAdd(proc)
videoSource.start() videoSource.start()