def __init__(self,mode="twist"):
self.gripper_btn=0
self.selector_btn=1
self.arm_pos=[]
self.mode = mode
self.arm = pyuarm.get_uarm()
self.arm.set_gripper(0)
self.arm.set_pump(0)
self.gripstate = False
self.selectorstate = False
self.gripdb=0
self.selectordb=0
rospy.init_node('uarm_teleop',anonymous=False)
#Set a small queue size so we don't drink from the firehose too much
self.spnav=rospy.Subscriber("/spacenav/joy",Joy,self.spnavCB,queue_size=10)
#while not rospy.is_shutdown():
#self.arm_pos = self.arm.get_position()
rospy.spin()
def main():
arm = pyuarm.get_uarm()
while True:
loc = input("Pickup: XTotal YTotal XPixel YPixel Rotation: ")
c = loc.split(" ")
s1, s2, s3, s4 = armCoords(int(c[0]), int(c[1]), int(c[2]), int(c[3]),
int(c[4]))
loc = input("Drop: XTotal YTotal XPixel YPixel Rotation: ")
d = loc.split(" ")
s5, s6, s7, s8 = armCoords(int(d[0]), int(d[1]), int(d[2]), int(d[3]),
int(d[4]))
if (s1 >= s5):
r = 135
if (s1 < s5):
r = 45
movArm(arm, s1, s2 + 10, s3 - 10, r)
arm.set_pump(True)
time.sleep(0.5)
movArm(arm, s1, s2 + 4, s3 - 4, r)
time.sleep(0.5)
movArm(arm, s1, s2, s3, r)
time.sleep(0.5)
movArm(arm, s1, s2 + 4, s3 - 4, r)
time.sleep(0.65)
movArm(arm, s1, s2 + 6, s3 - 6, r)
time.sleep(0.65)
movArm(arm, s1, s2 + 10, s3 - 10, r)
time.sleep(0.5)
movArm(arm, (s1 + s5) / 2, (s2 + s6) / 2 + 10, (s3 + s7) / 2 - 10,
r + (s5 - s1))
time.sleep(0.5)
movArm(arm, s5, s6 + 10, s7 - 10, r + (s5 - s1))
time.sleep(0.5)
movArm(arm, s5, s6 + 4, s7 - 4, r + (s5 - s1))
time.sleep(0.5)
movArm(arm, s5, s6, s7, r + (s5 - s1))
arm.set_pump(False)
time.sleep(0.8)
movArm(arm, s5, s6 + 4, s7 - 4, r + (s5 - s1))
time.sleep(0.5)
movArm(arm, s5, s6 + 10, s7 - 10, r + (s5 - s1))
time.sleep(0.5)
def connect(self):
try:
self.uarm = pyuarm.get_uarm()
except Exception as e:
print "Connecting to uArm error, check connection: ", e
return False
try:
self.connected = True
self.load_parameters()
self.connect_to_ROS()
self.start_threads()
rospy.loginfo("Connected")
self.ready = True
return True
except Exception as e:
self.connected = False
err = str(e.message)
rospy.logerr("Connection error: " + err)
rospy.signal_shutdown("Connection error")
def connectFcn(): global failed_number global connectionStatus connectionStatus = 0 global uarm global controlFcnLoop global listenerFcn controlFcnLoop = True listenerFcn = True if len(sys.argv)<2: # 1 means no input argument failed_number = 20 controlFcnLoop = False listenerFcn = False print 'Input Incorrect' sys.exit(0) return 1 if sys.argv[1] == 'connect': print '=======================================================' print 'Connecting ...' print '=======================================================' if len(sys.argv) == 2: failed_number = 21 uarm = pyuarm.get_uarm() connectionStatus = 1 print 'Connected' return 21 elif len(sys.argv) == 3: failed_number = 22 if sys.argv[2] == 'l': failed_number = 23 uarm = pyuarm.get_uarm() controlFcnLoop = False print 'Connected' else: connectionStatus = 1 print 'Connected' return 22 # followed by l - directly listen to nodes elif len(sys.argv) == 4: failed_number = 23 uarm = pyuarm.get_uarm() connectionStatus = 1 print 'Connected' if sys.argv[3] == 'l': print 'Directly to listen mode' controlFcnLoop = False return 25 return 23 else: # 2 means input argument is wrong return 24
cv2.circle(img, (i[0], i[1]), 2, (0, 0, 255), 3)
cv2.imwrite("/home/pi/Desktop/EC601_door/ip/output/opt.jpg",
img)
cx = i[0] * 25.4 / (2 * dpi)
cy = i[1] * 25.4 / (2 * dpi)
flagg = 0
print("Center Coordinates", i[0], i[1])
if (flagg == 0):
break
except AttributeError:
print(" Not able to detect the radius")
print("Uarm")
#Uarm commands
uarm = pyuarm.get_uarm()
ch = w1 * 25.4 / (2 * dpi)
cv = h1 * 25.4 / (2 * dpi)
print("Ch =", ch)
print("Ch =", cx)
print("Cv =", cv)
print("Cv =", cy)
x, y, z = uarm.get_position()
print("Current arm position", x, y, z)
#Slope
if cx >= ch / 2 and cy <= cv / 2:
sl = 1
print(sl)
print("Move to", 0.7 * (x + abs(cx - ch)), y + 200,
0.7 * (z + abs(cy - cv)))