diff --git a/PythonWorkspace/camera.py b/PythonWorkspace/camera.py
index bcf97a6a44bf51090c9c9e5879a05fe2e131f2bf..8cd969c8a74493225e1fb9ea41c9e4649f674631 100644
--- a/PythonWorkspace/camera.py
+++ b/PythonWorkspace/camera.py
@@ -11,8 +11,6 @@ import numpy as np
import os
import colorProb
-#Create probabilistic models of each color
-#the string argument is the color sample file name.
orange = colorProb.colorProb("orange")
yellow = colorProb.colorProb("yellow")
green = colorProb.colorProb("green")
@@ -23,15 +21,16 @@ red = colorProb.colorProb("red")
colorListObj = [red,green,blue,yellow,orange,white]
colorList = ['R','G','B','Y','O','W']
class camera:
- def __init__(self):
- a = 1
+ def __init__(self,startX,endX,startY,endY):
+ a = 1
+ self.startX =startX
+ self.startY = startY
+ self.endX = endX
+ self.endY = endY
+
def loadImage(self,name):
img = cv2.imread(name)
- startY =1100 #Pixel index of upper left corner of the cube
- endY =1180 #Pixel index of lower right corner of the cube
- startX =593 #Pixel index of upper left corner of the cube
- endX =674 #Pixel index of lower right corner of the cube
- newImg = img[startX:endX,startY:endY]
+ newImg = img[self.startX:self.endX,self.startY:self.endY]
newImg = cv2.blur(newImg,(5,5))
hsv = cv2.cvtColor(newImg, cv2.COLOR_BGR2HSV)
return hsv, newImg
@@ -40,40 +39,37 @@ class camera:
sidePT = ['','','','','','']
index = 0 ;
maxValue = 0;
- d = 6 #This variable narrows the segements used for identification
- segmenter = [hsv[d:(cy-d),d:(cx/2 -d)],hsv[d:(cy-d),(cx*1.5+d):-d],hsv[(cy+d):-d,d:(cx-d)],hsv[(cy+d):-d,(cx+d):(-d)]]
+ d = 6
+ segmenter = [hsv[d:(cy-d),d:(int(cx/2) -d)],hsv[d:(cy-d),(int(cx*1.5)+d):-d],hsv[(cy+d):-d,d:(cx-d)],hsv[(cy+d):-d,(cx+d):(-d)]]
i = 0
for segment in segmenter :
maxValue = 0
sumP = 0
- for color in colorList: ##############################################Fixed indentation
- h,s,v = cv2.split(segment)
- colorIndex = colorList.index(color)
- #This is for comparing each segment with the probabilisitc models
- #for the different colors
- p = colorListObj[colorIndex].normPdf([np.mean(h[:]),np.mean(s[:]),np.mean(v[:])])
- sidePT[colorIndex] = p
- sumP += p
- #Save the color returning the highest probability
- if(p > maxValue):
- maxValue = p
- side[i] = color
- #print np.around(np.array(sidePT)/sumP , decimals=2)
+ for color in colorList:
+ sumTemp = segment
+ h,s,v = cv2.split(sumTemp)
+ colorIndex = colorList.index(color)
+ p = colorListObj[colorIndex].normPdf([np.mean(h[:]),np.mean(s[:]),np.mean(v[:])])
+ sidePT[colorIndex] = p
+ sumP += p
+ if(p > maxValue):
+ maxValue = p
+ side[i] = color
+ #print np.around(np.array(sidePT)/sumP , decimals=2)
i +=1
return side
def returnSideColors(self):
- #Take a picture of the cube
+
os.system('./robotikkurs http://labcamera10.cs.lth.se/jpg/image.jpg >image.jpg')
name = 'image.jpg'
hsv,imgOrg = self.loadImage(name)
imageSize = imgOrg.shape
- cx = imageSize[0]/2 #Center of image
- cy = imageSize[1]/2 #Center of image
+ cx = imageSize[0]//2
+ cy = imageSize[1]//2
- #Do the analysis
sideColor = self.ascribeColor(colorList,cx,cy,hsv,imgOrg)
print sideColor
diff --git a/PythonWorkspace/colorProb.py b/PythonWorkspace/colorProb.py
index 44ef25686dea81f89361c9424fa2e5167b855667..3cf65f650733d3ff2ea074f2de1ebc82bb4ab2b3 100644
--- a/PythonWorkspace/colorProb.py
+++ b/PythonWorkspace/colorProb.py
@@ -7,17 +7,15 @@ from numpy.linalg import inv
@author: johannes
"""
+
class colorProb:
def __init__(self,name):
- #Load the sample image with name "name"
self.im = cv2.imread("segment/"+name+".png")
self.im = cv2.blur(self.im,(5,5))
self.hsv = cv2.cvtColor(self.im, cv2.COLOR_BGR2HSV)
self.mu = self.calcMean()
self.sigma = self.calcVar()
-
- #Calculate the mean of the hsv values
def calcMean(self):
mu = []
h,s,v = cv2.split(self.hsv)
@@ -25,8 +23,6 @@ class colorProb:
mu.append(numpy.mean(s[:]))
mu.append(numpy.mean(v[:]))
return numpy.array(mu)
-
- #Calculate the variance of the hsv values
def calcVar(self):
sigma = []
h,s,v = cv2.split(self.hsv)
@@ -34,8 +30,7 @@ class colorProb:
sigma.append(numpy.var(s[:]))
sigma.append(numpy.var(v[:]))
return numpy.diag(sigma)
-
- #Calculate the probability of x given the Gaussian model N(mu, sigma)
+
def normPdf(self,x):
size = len(x)
if size == len(self.mu) and (size, size) == self.sigma.shape:
@@ -51,3 +46,5 @@ class colorProb:
return norm_const * result
else:
raise NameError("The dimensions of the inumpyut don't match")
+
+
diff --git a/PythonWorkspace/communication.py b/PythonWorkspace/communication.py
index 0c972d5b7a4d9a9e9221292d3d57f839ef7fffcf..729341b8cb4be5990962983aa8a75af0060696f8 100644
--- a/PythonWorkspace/communication.py
+++ b/PythonWorkspace/communication.py
@@ -8,17 +8,16 @@ import sys
class communication:
def __init__(self,port1, port2, host):
- self.host = host
+ self.host = host#'localhost'
self.port1 = port1
self.port2 = port2
self.size = 1024
self.start()
- #Send command to the left arm
def sendToLeft(self,command):
self.sendTo(self.s1,command)
- #Send command to the right arm
+
def sendToRight(self,command):
self.sendTo(self.s2,command)
@@ -30,16 +29,15 @@ class communication:
except socket.error, (value,message):
print "Could not send to socket: " + message
- #Try to connect to the server
def start(self):
self.s1 = None
self.s2 = None
try:
self.s1 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
- self.s1.settimeout(60000)
+ self.s1.settimeout(60000)
self.s1.connect((self.host,self.port1))
self.s2 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
- self.s2.settimeout(60000)
+ self.s2.settimeout(60000)
self.s2.connect((self.host,self.port2))
except socket.error, (value,message):
if self.s1:
diff --git a/PythonWorkspace/cube.py b/PythonWorkspace/cube.py
index 9b33382a17d345392f07435349dd8d1d809ab18a..c0eae1929a1981f95e452ccff97703368fee5791 100644
--- a/PythonWorkspace/cube.py
+++ b/PythonWorkspace/cube.py
@@ -19,19 +19,19 @@ import numpy as np
#- - U V - - - -
#- - W X - - - -
-#Face indicies in the string representation of the cube
-uFace = [0, 1, 2, 3] #Upper
-lFace = [4, 5, 12, 13] #Left
-fFace = [6, 7, 14, 15] #Front
-rFace = [8, 9, 16, 17] # Right
-bFace = [10, 11, 18, 19] #Back
-dFace = [20, 21, 22, 23] #Down
+
+uFace = [0, 1, 2, 3]
+lFace = [4, 5, 12, 13]
+fFace = [6, 7, 14, 15]
+rFace = [8, 9, 16, 17]
+bFace = [10, 11, 18, 19]
+dFace = [20, 21, 22, 23]
class cube:
def __init__(self):
self.cubeList = [[] for i in range(24)]
- self.indexHolder = [uFace, lFace, fFace, rFace, bFace, dFace] # Holds the different index (in the solver string)
+ self.indexHolder = [uFace, lFace, fFace, rFace, bFace, dFace] # Holds the different index (in the sovlerstring)
self.sideList = ['uFace', 'lFace', 'fFace', 'rFace', 'bFace', 'dFace'] # Keep track of the index-notation
def solveCube(self):
@@ -42,7 +42,6 @@ class cube:
#print uniqueElements
- #To determine the colors of the unseen face
if len(uniqueElements) == 6: # If 6 different colors has been observed
# print uniqueElements
permList = []
@@ -63,11 +62,9 @@ class cube:
a = ''.join(self.cubeList) # Calculate the solution for the config.
self.sol = solver.solv(a)
# print self.sol
- if self.sol != None: # Examine if "a" is a solution.
+ if self.sol != None: # Examine if "a" can be a solution.
break
else:
- #If only five colors has been observed the remaining face must
- #consist of only one color
for idx in self.indexHolder:
self.cubeList[idx[0]] = 'Q'
self.cubeList[idx[1]] = 'Q'
@@ -77,12 +74,12 @@ class cube:
a = ''.join(self.cubeList)
self.sol = solver.solv(a)
- print a
- print ''.join(self.sol)
+ print('Configuration: ' + a)
+ #print ''.join(self.sol)
#----------------------------------------------------------------------------------------
- # Insert the different colors in the solver string
+ # Insert the different colors in the solverstring
def setUface(self, color):
for i in range(4):
self.cubeList[uFace[i]] = color[i]
diff --git a/PythonWorkspace/initConfig.py b/PythonWorkspace/initConfig.py
new file mode 100644
index 0000000000000000000000000000000000000000..4b9e96d25567101b1228ba7e6d63be78de89e46b
--- /dev/null
+++ b/PythonWorkspace/initConfig.py
@@ -0,0 +1,52 @@
+import cv2
+import numpy as np
+import os
+
+
+class CoordinateStore:
+ def __init__(self,img):
+ self.points = []
+ self.img = img
+
+ def select_point(self,event,x,y,flags,param):
+ if event == cv2.EVENT_LBUTTONDBLCLK:
+ cv2.circle(self.img,(x,y),3,(255,0,255),-1)
+ self.points.append((x,y))
+
+
+def getCoord():
+ imageName = "image.jpg"
+ os.system('./robotikkurs http://labcamera10.cs.lth.se/jpg/image.jpg >' + imageName)
+ global img
+ img = cv2.imread(imageName)
+
+
+ imgOrd = img
+ coordinateStore1 = CoordinateStore(img)
+
+
+ cv2.namedWindow('image')
+ cv2.setMouseCallback('image',coordinateStore1.select_point)
+
+
+ while(1):
+ cv2.imshow('image',img)
+ k = cv2.waitKey(20) & 0xFF
+ if len(coordinateStore1.points)>=2:
+ break
+ if k == 27:
+ break
+ cv2.destroyAllWindows()
+
+ start = coordinateStore1.points[0]
+ startY = start[0]
+ startX = start[1]
+
+ end= coordinateStore1.points[1]
+ endY = end[0]
+ endX = end[1]
+
+ return startX,endX,startY,endY
+
+
+
diff --git a/PythonWorkspace/movement.py b/PythonWorkspace/movement.py
index 9ef244b6e650a51667c31afb1faca2ab2c05c5db..12c9f8dea73f5db606a2ce2fc69ccf1c94b6e511 100644
--- a/PythonWorkspace/movement.py
+++ b/PythonWorkspace/movement.py
@@ -1,186 +1,206 @@
-import communication
-import time
+import communication
+import time
import cube
import camera
-characters = ['U','F','R']
+characters = ['U', 'F', 'R']
emptySpace = " "
"""
@author: johannes
"""
+
+
class movement:
- def __init__(self):
- self.a = 1
-
- #Show for the camera then solve the cube
- def start(self):
- self.showForcamera()
- self.cube.solveCube()
- self.setSolution(self.cube.returnSolution())
- self.startMovement()
-
- def setCommunication(self,com):
- self.com = com
-
- def setSolution(self,sol):
- self.sol = sol
- if len(self.sol) == 0: #The cube is already solved
- self.endMove()
- exit()
-
- def setCube(self,cube):
- self.cube = cube
-
- def setCamera(self,camera):
- self.cam = camera
-
- def moveR (self,direc,nbr):
- self.com.sendToLeft("MoveRight"+" " + self.isPos(-1)) # Move away from center (L)
-
- self.com.sendToRight("RotateSide"+" " + self.isPos(direc*nbr)) # Rotate (R) nbr*90 degrees then rotate back
-
- self.com.sendToLeft("MoveRight"+" " + self.isPos(1)) # Move to the center (L)
-
- def moveF (self,direc,nbr):
- self.com.sendToRight("MoveOffz"+" " + self.isPos(1)) # Move out (R)
-
- #self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(-direc*1)) # Rotate -direc*90 degrees (L)
- self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(1)) # Rotate 90 degrees (L)
-
- self.com.sendToRight("MoveOffz"+" " + self.isPos(-1)) # Move in (R)
-
- #self.com.sendToLeft("Reset"+" " + self.isPos(direc*1)) # Reset (L)
- self.com.sendToLeft("Reset"+" " + self.isPos(-1)) # Reset (L)
-
- self.com.sendToLeft("MoveRight"+" " + self.isPos(-1)) # Move away from center (L)
-
- self.com.sendToRight("RotateSide"+" " + self.isPos(direc*nbr)) # Rotate direc*nbr*90 degrees (L) FRONT - rotation
- # it will rotate back
-
- self.com.sendToLeft("MoveRight"+" " + self.isPos(1)) # Move to the center (L)
-
- self.com.sendToRight("MoveOffz"+" " + self.isPos(1)) # Move out (R)
-
- #self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(direc)) # Rotate back 90 degrees (L)
- self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(-1)) # Rotate back 90 degrees (L)
-
- self.com.sendToRight("MoveOffz"+" " + self.isPos(-1)) # Move in with (R)
-
- self.com.sendToLeft("Reset"+" " + self.isPos(1)) # Reset (L)
-
-
- def moveU (self,direc,nbr):
- self.com.sendToRight("MoveDown"+" "+ self.isPos(-1)) # Move down (R)
-
- self.com.sendToLeft("RotateTop"+" " + self.isPos(direc*nbr)) #RotateTop direc*nbr*90 degrees
-
- self.com.sendToRight("MoveDown"+ " "+ self.isPos(1)) # Move upp (R)
-
- def initMove(self,arm):
- if (arm =='L' ):
- self.com.sendToLeft('InitPath'+emptySpace) #Init (L)
- if (arm =='R' ):
- self.com.sendToRight('InitPath'+emptySpace) # Init (R)
-
- #def rotateSixthJointL(self):
- # self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(1)) # Init (L)
-
+ def __init__(self):
+ self.a = 1
+
+ def configCamera(self, done=False):
+ if done == False:
+ self.initMove("L")
+ else:
+ self.com.sendToLeft("End" + emptySpace) # End (L)
+
+ # Show for the camera then solve the cube
+ def start(self):
+ self.showForcamera()
+ self.cube.solveCube()
+ self.setSolution(self.cube.returnSolution())
+ self.startMovement()
+
+ def setCommunication(self, com):
+ self.com = com
+
+ def setSolution(self, sol):
+ self.sol = sol
+ if self.sol == None : # The cube is already solved
+ self.endMove()
+ exit()
+ print('Solution: ' + ' '.join(self.sol))
+
+ def setCube(self, cube):
+ self.cube = cube
+
+ def setCamera(self, camera):
+ self.cam = camera
+
+ def moveR(self, direc, nbr):
+ self.com.sendToLeft("MoveRight" + " " + self.isPos(-1)) # Move away from center (L)
+
+ self.com.sendToRight("RotateSide" + " " + self.isPos(direc * nbr)) # Rotate (R) nbr*90 degrees then rotate back
+
+ self.com.sendToLeft("MoveRight" + " " + self.isPos(1)) # Move to the center (L)
+
+ def moveF(self, direc, nbr):
+ self.com.sendToRight("MoveOffz" + " " + self.isPos(1)) # Move out (R)
+
+ # self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(-direc*1)) # Rotate -direc*90 degrees (L)
+ self.com.sendToLeft("RotateSixthJoint" + " " + self.isPos(-1)) # Rotate 90 degrees (L) =======1
+
+ self.com.sendToRight("MoveOffz" + " " + self.isPos(-1)) # Move in (R)
+
+ # self.com.sendToLeft("Reset"+" " + self.isPos(direc*1)) # Reset (L)
+ self.com.sendToLeft("Reset" + " " + self.isPos(-1)) # Reset (L)
+
+ self.com.sendToLeft("MoveRight" + " " + self.isPos(-1)) # Move away from center (L)
+
+ self.com.sendToRight(
+ "RotateSide" + " " + self.isPos(direc * nbr)) # Rotate direc*nbr*90 degrees (L) FRONT - rotation
+ # it will rotate back
+
+ self.com.sendToLeft("MoveRight" + " " + self.isPos(1)) # Move to the center (L)
+
+ self.com.sendToRight("MoveOffz" + " " + self.isPos(1)) # Move out (R)
+
+ # self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(direc)) # Rotate back 90 degrees (L)
+ self.com.sendToLeft("RotateSixthJoint" + " " + self.isPos(1)) # Rotate back 90 degrees (L) === -1
+
+ self.com.sendToRight("MoveOffz" + " " + self.isPos(-1)) # Move in with (R)
+
+ self.com.sendToLeft("Reset" + " " + self.isPos(1)) # Reset (L)
+
+
+ def moveU(self, direc, nbr):
+ self.com.sendToRight("MoveDown" + " " + self.isPos(-1)) # Move down (R)
+
+ self.com.sendToLeft("RotateTop" + " " + self.isPos(direc * nbr)) # RotateTop direc*nbr*90 degrees
+
+ self.com.sendToRight("MoveDown" + " " + self.isPos(1)) # Move upp (R)
+
+
+ def initMove(self, arm):
+ if (arm == 'L'):
+ self.com.sendToLeft('InitPath' + emptySpace) # Init (L)
+ if (arm == 'R'):
+ self.com.sendToRight('InitPath' + emptySpace) # Init (R)
+
+ # def rotateSixthJointL(self):
+ # self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(1)) # Init (L)
+
+
def endMove(self):
- self.com.sendToRight("EndLoop"+emptySpace) # End (R)
- self.com.sendToLeft("EndLoop"+emptySpace) # End (L)
- self.com.closeAllSockets() #Close all sockets
-
- #If a is positive add a '+' sign to the string in order to get the right
- #format of the string when the robot parses the command
- def isPos (self,a):
- ret = a
- if a > 0 :
- ret = "+"+str(a)
- return str(ret)
-
- def showForcamera(self):
- self.initMove("L") #Pick up the cube and move it to the working position (L)
+ self.com.sendToRight("EndLoop" + emptySpace) # End (R)
+ self.com.sendToLeft("EndLoop" + emptySpace) # End (L)
+ self.com.closeAllSockets() # Close all sockets
+
+ # If a is positive add a '+' sign to the string in order to get the right
+ # format of the string when the robot parses the command
+
+
+ def isPos(self, a):
+ ret = a
+ if a > 0:
+ ret = "+" + str(a)
+ return str(ret)
+
- self.cube.setFface(self.cam.returnSideColors()) #Determine and set the colors of the front face
-
- self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(1)) #Rotate the cube 90 degrees (L)
-
- self.cube.setRface(self.cam.returnSideColors()) #Determine and set the colors of the right face
+ def showForcamera(self):
+ self.initMove("L") # Pick up the cube and move it to the working position (L)
- self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(1)) #Rotate the cube 90 degrees (L)
+ self.cube.setFface(self.cam.returnSideColors()) # Determine and set the colors of the front face
- self.cube.setBface(self.cam.returnSideColors()) #Determine and set the colors of the back face
+ self.com.sendToLeft("RotateSixthJoint" + " " + self.isPos(1)) # Rotate the cube 90 degrees (L)
- self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(-3)) #Rotate the cube -270 degrees (L)
+ self.cube.setRface(self.cam.returnSideColors()) # Determine and set the colors of the right face
- self.cube.setLface(self.cam.returnSideColors()) #Determine and set the colors of the left face
+ self.com.sendToLeft("RotateSixthJoint" + " " + self.isPos(1)) # Rotate the cube 90 degrees (L)
- self.com.sendToLeft("RotateSixthJoint"+" " + self.isPos(1)) #Rotate the cube 90 degrees (to initial position) (L)
+ self.cube.setBface(self.cam.returnSideColors()) # Determine and set the colors of the back face
- self.initMove("R") #Grab the cube with the right arm (R)
- self.com.sendToLeft("MoveUp"+" " + self.isPos(1)) #Release the cube with the left arm and move away (L)
+ self.com.sendToLeft("RotateSixthJoint" + " " + self.isPos(-3)) # Rotate the cube -270 degrees (L)
- self.com.sendToRight("RotateSixthJoint"+" " + self.isPos(1)) #Rotate the cube 90 degrees (R)
+ self.cube.setLface(self.cam.returnSideColors()) # Determine and set the colors of the left face
- self.cube.setDface(self.cam.returnSideColors()) #Determine and set the colors of the down face
-
- self.com.sendToRight("RotateSixthJoint"+" " + self.isPos(-1)) #Rotate the cube -90 degrees (R)
-
- self.com.sendToLeft("MoveUp"+" " + self.isPos(-1)) #Move down and grab the cube with the left arm (L)
+ self.com.sendToLeft("RotateSixthJoint" + " " + self.isPos(1)) # Rotate the cube 90 degrees (to initial position) (L)
- #Parse the output from the solver and transform it to robot moves
+ self.initMove("R") # Grab the cube with the right arm (R)
+
+ self.com.sendToLeft("MoveUp" + " " + self.isPos(1)) # Release the cube with the left arm and move away (L)
+
+ self.com.sendToRight("RotateSixthJoint" + " " + self.isPos(1)) # Rotate the cube 90 degrees (R)
+
+ self.cube.setDface(self.cam.returnSideColors()) # Determine and set the colors of the down face
+
+ self.com.sendToRight("RotateSixthJoint" + " " + self.isPos(-1)) # Rotate the cube -90 degrees (R)
+
+ self.com.sendToLeft("MoveUp" + " " + self.isPos(-1)) # Move down and grab the cube with the left arm (L)
+
+
+ # Parse the output from the solver and transform it to robot moves
# ' means counterclockwise rotation.
- #nbr number of consecutive 90 degree rotations.
- def startMovement(self):
- charX = ''
- tempList = []
- tempList.append(self.sol[0])
- length = len(self.sol)-1
- i = 0;
- for move in self.sol[1:]:
- i+=1
- if move in characters or i == length :
- if(i == length):
- tempList.append(move)
- print tempList
- direc = 1
- nbr =1
- if("'" in tempList):
- direc = -1
- if filter(str.isdigit,''.join(tempList)) != '':
- nbr = int(filter(str.isdigit,''.join(tempList)))
- if(tempList[0] ==characters[0]):
- a = 1
- self.moveU(direc,nbr)
- elif(tempList[0] ==characters[1]):
- a = 1
- self.moveF(direc,nbr)
- elif(tempList[0] ==characters[2]):
- a = 1
- self.moveR(direc,nbr)
- tempList = []
- tempList.append(move)
- elif move != ' ':
- #print move
- tempList.append(move)
-
- #Open the gripper on the arm
- def Open(self,arm):
- if (arm == L ):
- self.com.sendToLeft('Open'+emptySpace)
- if (arm == R ):
- self.com.sendToRight('Open'+emptySpace)
-
- #Close the gripper on the arm
- def Close(self,arm):
- if (arm == L ):
- self.com.sendToLeft('Close'+emptySpace)
- if (arm == R ):
- self.com.sendToRight('Close'+emptySpace)
-
-
-
-
-
+ # nbr number of consecutive 90 degree rotations.
+ def startMovement(self):
+ charX = ''
+ tempList = []
+ tempList.append(self.sol[0])
+ length = len(self.sol) - 1
+ i = 0;
+ for move in self.sol[1:]:
+ i += 1
+ if move in characters or i == length:
+ if (i == length):
+ tempList.append(move)
+ print tempList
+ direc = 1
+ nbr = 1
+ if ("'" in tempList):
+ direc = -1
+ if filter(str.isdigit, ''.join(tempList)) != '':
+ nbr = int(filter(str.isdigit, ''.join(tempList)))
+ if (tempList[0] == characters[0]):
+ a = 1
+ self.moveU(direc, nbr)
+ elif (tempList[0] == characters[1]):
+ a = 1
+ self.moveF(direc, nbr)
+ elif (tempList[0] == characters[2]):
+ a = 1
+ self.moveR(direc, nbr)
+ tempList = []
+ tempList.append(move)
+ elif move != ' ':
+ # print move
+ tempList.append(move)
+
+ # Open the gripper on the arm
+
+
+ def Open(self, arm):
+ if (arm == L):
+ self.com.sendToLeft('Open' + emptySpace)
+ if (arm == R):
+ self.com.sendToRight('Open' + emptySpace)
+
+ # Close the gripper on the arm
+
+
+ def Close(self, arm):
+ if (arm == L):
+ self.com.sendToLeft('Close' + emptySpace)
+ if (arm == R):
+ self.com.sendToRight('Close' + emptySpace)
+
+
+
+
diff --git a/PythonWorkspace/pixelConfig.txt b/PythonWorkspace/pixelConfig.txt
new file mode 100644
index 0000000000000000000000000000000000000000..aa4f755da8d2061883310bef922aac055944ed6d
--- /dev/null
+++ b/PythonWorkspace/pixelConfig.txt
@@ -0,0 +1 @@
+412 479 1018 1092
\ No newline at end of file
diff --git a/PythonWorkspace/run.py b/PythonWorkspace/run.py
index 8b448a59906fe7147b193c4aded3eecedbbca610..b5caf69bcd4b2b54d1b6908a755b83c6621621cd 100644
--- a/PythonWorkspace/run.py
+++ b/PythonWorkspace/run.py
@@ -1,3 +1,4 @@
+
""""
@author: johannes
"""
@@ -7,23 +8,41 @@ import movement
import communication
import cubeOpenCv
import time
-import camera
+import camera
+import json
+import initConfig
-#Initialize the different modules and solve the cube
-c = cube.cube()
-leftPort = 1025
-rightPort = 1026
-yuMiAddress = "192.168.125.1"
-com = communication.communication(leftPort,rightPort,yuMiAddress)
-cam = camera.camera()
-move = movement.movement()
+a = ""
+a = raw_input("Do you want to configure [y/n]: ")
+move = movement.movement()
+com = communication.communication(1025,1026,"192.168.125.1")
move.setCommunication(com)
+
+if str(a) == "yes" or str(a) == "y":
+ move.configCamera()
+ coords = initConfig.getCoord()
+ coordsInput = []
+ for coord in coords:
+ coordsInput.append(str(coord))
+ with open("pixelConfig.txt",'w') as f :
+ f.write(' '.join(coordsInput))
+ move.configCamera(True)
+else:
+ with open("pixelConfig.txt",'r') as f:
+ coordsInput = f.read().split()
+ coords = []
+ for coord in coordsInput:
+ coords.append(int(coord))
+
+cam = camera.camera(startX=coords[0],endX=coords[1],startY=coords[2],endY=coords[3])
+c = cube.cube()
+
+
move.setCube(c)
move.setCamera(cam)
-#Start solving the cube
+
move.start()
-#Send command to wrap up, terminate the servers and close the sockets
move.endMove()
diff --git a/PythonWorkspace/solver.py b/PythonWorkspace/solver.py
index 93168bd50f2c640ee432ce9f06253cb1aad1d1d1..bbf67dc30ad4abd9b77e174887f5e63025ada252 100644
--- a/PythonWorkspace/solver.py
+++ b/PythonWorkspace/solver.py
@@ -3,8 +3,8 @@ import sys;o=''.join;
""""
@author: johannes
"""
-# Solver retrieved 2016-09-12 from
-# http://codegolf.stackexchange.com/questions/35002/solve-the-rubiks-pocket-cube
+
+
def solv(a):
#define permutations for R,U,F
diff --git a/Rapid/L/MainModule.mod b/Rapid/L/MainModule.mod
index 6784c531b6ef466dc5cfffc73e95ecb1b497ce61..5a34a985665c772e13fe4b9758134b401badc327 100644
--- a/Rapid/L/MainModule.mod
+++ b/Rapid/L/MainModule.mod
@@ -25,14 +25,9 @@ MODULE MainModule
SocketAccept server_socket,client_socket\ClientAddress:=client_ip\Time:=WAIT_MAX;
WHILE loop DO
SocketReceive client_socket\Str:=recieve_string;
-
- !Arguments are passed in the last three characters, hence we must cut
- !the string three characters from the end
nbrStr:=StrPart(recieve_string,StrLen(recieve_string)-cut+1,cut);
final_string:=StrPart(recieve_string,1,StrLen(recieve_string)-cut);
ok:=StrtoVal(nbrStr,nbr);
-
- !if conversion from string to number succeded then arguments were sent
IF ok THEN
%final_string %nbr;
ELSE
@@ -57,4 +52,4 @@ MODULE MainModule
End;
ENDPROC
-ENDMODULE
+ENDMODULE
\ No newline at end of file
diff --git a/Rapid/L/Module1.mod b/Rapid/L/Module1.mod
index 28b18120547b14028e7f61d45360e0cd8c249a71..5ed2fcf6533bec46ff13be96a47ac6e6f9cf5967 100644
--- a/Rapid/L/Module1.mod
+++ b/Rapid/L/Module1.mod
@@ -5,9 +5,8 @@ MODULE Module1
CONST robtarget WorkPos:=[[0,0,-24],[1,0,0,0],[-2,-3,-1,11],[107.815872837,9E9,9E9,9E9,9E9,9E9]];
CONST robtarget OnTheWayGetCube:=[[333.567066184,380.674204623,319.504312057],[0.008289474,0.497010931,-0.861167812,0.106308131],[0,0,-1,11],[-116.911029453,9E9,9E9,9E9,9E9,9E9]];
CONST num t:=1;
- CONST num offZ:=45;
+ CONST num offZ:=42;
- !Move the arm to the cube, pick it up and then move to the working position
PROC InitPath()
open;
MoveJ AboveGetCube,v1000,z10,Servo\WObj:=CubeInitL;
@@ -17,8 +16,6 @@ MODULE Module1
MoveJ WorkPos,v1000,z10,Servo\WObj:=CubeWorkL;
ENDPROC
- !Rotate the top of the cube 90 degrees nbr times.If nbr is positive it will rotate
- !clockwise otherwise counterclockwise
PROC RotateTop(num nbr)
VAR jointtarget j6;
WaitRob\ZeroSpeed;
@@ -39,7 +36,6 @@ MODULE Module1
Close;
ENDPROC
- !Move the arm up and away from the cube
PROC MoveUp(num dir)
Open;
IF dir<0 THEN
@@ -51,8 +47,6 @@ MODULE Module1
ENDIF
ENDPROC
- !Resets the arm by moving it up relative the cube, rotate 90 degrees in dir direction
- !and then move down to the cube again
PROC Reset(num dir)
VAR robtarget pos;
VAR jointtarget j6;
@@ -70,8 +64,6 @@ MODULE Module1
Close;
ENDPROC
- !Rotates 90 degrees. Positive dir is rotation in clockwise direction
- !otherwise counterclockwise
PROC RotateSixthJoint(num dir)
VAR jointtarget j6;
j6:=CJointT();
@@ -79,7 +71,6 @@ MODULE Module1
MoveAbsJ j6,v1000,fine,Servo\WObj:=CubeWorkL;
ENDPROC
- !Put the cube back to the initial position
PROC End()
MoveJ AboveGetCube,v1000,z10,Servo\WObj:=CubeInitL;
WaitRob\ZeroSpeed;
@@ -98,4 +89,4 @@ MODULE Module1
WaitRob\ZeroSpeed;
ENDPROC
-ENDMODULE
+ENDMODULE
\ No newline at end of file
diff --git a/Rapid/R/MainModule.mod b/Rapid/R/MainModule.mod
index 4fad6db2a68af2b6230950a832f3099ed334a12d..45b3a3acdf3e1a269993160a95ca5c4ec399457f 100644
--- a/Rapid/R/MainModule.mod
+++ b/Rapid/R/MainModule.mod
@@ -25,14 +25,9 @@ MODULE MainModule
SocketAccept server_socket,client_socket\ClientAddress:=client_ip\Time:=WAIT_MAX;
WHILE loop DO
SocketReceive client_socket\Str:=recieve_string;
-
- !Arguments are passed in the last three characters, hence we must cut
- !the string three characters from the end
nbrStr:=StrPart(recieve_string,StrLen(recieve_string)-cut+1,cut);
final_string:=StrPart(recieve_string,1,StrLen(recieve_string)-cut);
ok:=StrtoVal(nbrStr,nbr);
-
- !if conversion from string to number succeded then arguments were sent
IF ok THEN
%final_string %nbr;
ELSE
@@ -57,4 +52,4 @@ MODULE MainModule
End;
ENDPROC
-ENDMODULE
+ENDMODULE
\ No newline at end of file
diff --git a/Rapid/R/Module1.mod b/Rapid/R/Module1.mod
index fe13968ced2dadbcea6a9893c0fd46e04446c60e..d67a5c00c5e1a2b47d219f1e044ba5a44b4b0bce 100644
--- a/Rapid/R/Module1.mod
+++ b/Rapid/R/Module1.mod
@@ -12,7 +12,7 @@ MODULE Module1
Close;
ENDPROC
- !If dir is positive, move the arm down otherwise up relative the cube
+ !Move the
PROC MoveDown(num dir)
VAR robtarget pos;
Open;
@@ -21,7 +21,7 @@ MODULE Module1
Close;
ENDPROC
- !Pos dir moves out from the cube, negative moves towards the cube
+ !Pos dir moves out
PROC MoveOffZ(num dir)
Open;
IF dir<0 THEN
@@ -33,8 +33,6 @@ MODULE Module1
ENDIF
ENDPROC
- !Rotate the side 90 degrees nbr times. If nbr is positive it will rotate clockwise
- !otherwise counterclockwise
PROC RotateSide(num nbr)
VAR robtarget pos;
VAR jointtarget j6;
@@ -45,8 +43,7 @@ MODULE Module1
Reset -nbr;
ENDPROC
- !Resets the arm by moving it out from the cube, rotate 90 degrees in dir direction
- !and then move in to the cube again
+ !Private proc
PROC Reset(num dir)
VAR robtarget pos;
VAR jointtarget j6;
@@ -64,7 +61,6 @@ MODULE Module1
Close;
ENDPROC
- !Rotates 90 degrees. Positive dir is rotation in clockwise direction
PROC RotateSixthJoint(num dir)
VAR jointtarget j6;
j6:=CJointT();
@@ -72,7 +68,6 @@ MODULE Module1
MoveAbsJ j6,v5,fine,Servo\WObj:=CubeWorkR;
ENDPROC
- !Open the gripper and move away from the cube
PROC End()
VAR robtarget pos;
Open;
@@ -90,4 +85,4 @@ MODULE Module1
CloseHand;
WaitRob\ZeroSpeed;
ENDPROC
-ENDMODULE
+ENDMODULE
\ No newline at end of file
diff --git a/Rapid/YuMiRubiks.rspag b/Rapid/YuMiRubiks.rspag
new file mode 100644
index 0000000000000000000000000000000000000000..372a3c024ca6dbc1ae06c2f9f7040c1fd71d8a93
Binary files /dev/null and b/Rapid/YuMiRubiks.rspag differ