- 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
This commit is contained in:
+6
-5
@@ -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()
|
||||
|
||||
+59
-15
@@ -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()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user