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)))