<filename>scripts/uarm_core.py
#!/usr/bin/env python

'''

# File Name : uarm_core.py
# Author : <NAME>
# Version : V1.0
# Date : 6 Jan, 2016
# Modified Date : 6 Jan, 2016
# Description : This documents is for uarm ROS Library and ROS package
# Copyright(C) 2016 uArm Team. All right reserved.

'''


# All libraries needed to import
# Import system library
import sys
import time
import rospy

# Import uarm for python library
#from UArmForPython.uarm_python import Uarm
import pyuarm

# Import messages type
from std_msgs.msg import String
from std_msgs.msg import Float32
from std_msgs.msg import UInt8
from std_msgs.msg import Int32
from uarm.msg import Angles
from uarm.msg import Coords
from uarm.msg import CoordsWithTime
from uarm.msg import CoordsWithTS4

# Read current Coords function
def readCurrentCoords():
	cc = uarm.get_position()
	print 'Current location is x: %2.2fcm, y: %2.2fcm, z: %2.2fcm.' %(float(cc[0]), float(cc[1]), float(cc[2]))

	rospy.set_param('current_x', cc[0])
	rospy.set_param('current_y', float(cc[1]))
	rospy.set_param('current_z', float(cc[2]))

# Read current Angles function
def readCurrentAngles():
	ra = {}
	ra['s1'] = uarm.get_servo_angle(0)
	ra['s2'] = uarm.get_servo_angle(1)
	ra['s3'] = uarm.get_servo_angle(2)
	ra['s4'] = uarm.get_servo_angle(3)

	print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(ra['s1'], ra['s2'],ra['s3'],ra['s4'])

	rospy.set_param('servo_1',ra['s1'])
	rospy.set_param('servo_2',ra['s2'])
	rospy.set_param('servo_3',ra['s3'])
	rospy.set_param('servo_4',ra['s4'])

# Read stopper function
def readStopperStatus():
	val = uarm.get_tip_sensor()
	if val == True:
		print 'Stopper is actived'
		rospy.set_param('stopper_status','HIGH')
	else:
		print 'Stopper is not actived'
		rospy.set_param('stopper_status','LOW')


# Write single angle
def writeSingleAngle(num, angle):
	# limits of each servo
	s3_max = 170 - uarm.get_servo_angle(1)
	if num == 1:
		if angle > 160:
			angle = 160
		elif angle < 20:
			angle = 20
	elif num == 2:
		if angle > 140:
			angle = 140
		elif angle < 0:
			angle = 0
	elif num == 3:
		if angle > s3_max:
			angle = s3_max
		elif angle < 0:
			angle = 0
	elif num == 4:
		if angle > 180:
			angle = 180
		elif angle < 0:
			angle = 0
	else:
		print "Input incorrect: wrong servo number"

	uarm.set_servo_angle(num-1, angle)


# Write coordinate: x y z and speed
def writeCoordinate(coordinate):
	x = float(coordinate[1])
	y = float(coordinate[2])
	z = float(coordinate[3])
	if len(coordinate) == 4:
		speed = 5

	elif len(coordinate) == 5:
		speed = float(coordinate[4])

	else:
		print "Input incorrect: wrong number of arguments"

	if y > 0:
		y = -y

	uarm.set_position(x, y, z, speed)


# Main connect function
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()
			uarm.connect()
			connectionStatus = 1

			print 'uarm_core: Connected'
			return 21

		elif len(sys.argv) == 3:

			failed_number = 22

			if sys.argv[2] == 'l':
				failed_number = 23
				uarm = pyuarm.get_uarm()
				uarm.connect()
				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()
			uarm.connect()

			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
	#if sys.argv[1] == 'd'


# Main control function
def controlFcn():

	while controlFcnLoop:
		commands = raw_input("Input Commands (Input h to see all commands): ")

		if commands == 'h':
			print ' '
			print 'Control mode'
			print '=================================================================='
			print 'h                  - show help and list all commands'
			print 'cc                 - current coordinates'
			print 'ra                 - read angles'
			print 'wa a1 a2 a3 a4     - write angles'
			print '                     for example: wa 120 80 40 40'
			print 'wsa servo a        - write single angle'
			print '                     for example: wsa 1 50'
			print 'mt x y z           - move to a point with speed(defaut: 5mm/s)'
			print '                     for example: mt 0 12 12'
			print 'mt x y z speed     - move to a point with speed(mm/s)'
			print '                     for example: mt 0 12 12 10'
			print 'pp 1/0             - pump on/off'
			print 'ss                 - status of stopper'
			print 'at                 - attach uarm'
			print 'de                 - detach uarm'
			print 'l                  - begin listener mode'
			print 'e                  - exit'
			print '=================================================================='
			print ' '

		elif commands == 'l':
			print 'Exit: Break the control fuction loop'
			break;

		elif commands == 'e':
			print 'Detach all servos and exit the program'
			uarm.set_servo_detach()
			sys.exit(0)

		elif len(commands) == 0:
			print 'len is 0'

		else:
			commands_split = commands.split()

			# Detach
			if commands_split[0] == 'detach'or commands_split[0] == 'de':
				if len(commands_split) == 1:
					uarm.set_servo_detach()
				else:
					print 'no other commands should be input'
				pass

			# Attach
			if commands_split[0] == 'attach'or commands_split[0] == 'at':
				if len(commands_split) == 1:
					uarm.set_servo_attach()
				else:
					print 'no other commands should be input'
				pass

			# Stopper Status
			elif commands_split[0] == 'stopperStatus'or commands_split[0] == 'ss':
				if len(commands_split) == 1:
					readStopperStatus()
				else:
					print 'no other commands should be input'
				pass

			# Pump Control
			elif commands_split[0] == 'pump' or commands_split[0] == 'pp':
				if len(commands_split) == 1:
					print 'Status should be input'
				elif len(commands_split) >2:
					print 'Too many inputs'
				else:
					if commands_split[1] == '1' or commands_split[1].lower() == 'high' or commands_split[1].lower() == 'on':
						uarm.set_pump(1)
					elif commands_split[1] == '0' or commands_split[1].lower() == 'low'or commands_split[1].lower() == 'off':
						uarm.set_pump(0)
					else:
						print 'Incorrect inputs, should input 1 / 0 / HIGH / LOW / ON / OFF'
				pass

			# Current Coordinates
			elif commands_split[0] == 'currentCoords' or commands_split[0] == 'cc':
				if len(commands_split) == 1:
					readCurrentCoords()
				else:
					print 'no other commands should be input'
				pass

			# Write Single Angles
			elif commands_split[0] == 'writeSingleAngles' or commands_split[0] == 'wsa':
				if len(commands_split) == 3:
					writeSingleAngle(int(commands_split[1]), float(commands_split[2]))
				else:
					print 'Input incorrect: Wrong number of arguments'
				pass

			# Write Angles
			elif commands_split[0] == 'wa':
				if len(commands_split) == 5:
					writeSingleAngle(1, float(commands_split[1]))
					writeSingleAngle(2, float(commands_split[2]))
					writeSingleAngle(3, float(commands_split[3]))
					writeSingleAngle(4, float(commands_split[4]))
				else:
					print 'Input incorrect: Wrong number of arguments'
				pass

			# Current Angles
			elif commands_split[0] == 'readAngles' or commands_split[0] == 'ra':
				if len(commands_split) == 1:
					 readCurrentAngles()
				else:
					print 'no other commands should be input'
				pass

			# Move Tos
			elif commands_split[0] == 'moveTo' or commands_split[0] == 'mt':
				if len(commands_split) == 4 or len(commands_split) == 5:
					writeCoordinate(commands_split)
				else:
					print 'Input incorrect: Wrong number of arguments'
			else:
				pass


# pump control function once received data from topic
def pumpCallack(data):

	data_input = data.data

	if data_input == 0:
		uarm.pump_control(0)
		print 'Pump: Off'
	elif data_input == 1:
		uarm.pump_control(1)
		print 'Pump: On'
	else:
		pass


# pump str control function once received data from topic
def pumpStrCallack(data):

	data_input = data.data
	print data_input

	if data_input.lower() == 'low' or data_input.lower() == 'off':
		uarm.set_pump(0)
		print 'Pump: Off'
	elif data_input.lower() == 'on' or data_input.lower() == 'high':
		uarm.set_pump(1)
		print 'Pump: On'
	else:
		pass


# angles control function once received data from topic
def writeAnglesCallback(servos):

	servo = {}
	servo['s1'] = servos.servo_1
	servo['s2'] = servos.servo_2
	servo['s3'] = servos.servo_3
	servo['s4'] = servos.servo_4
	for i in servo:
		if servo[i]>180: servo[i] = 180
		if servo[i]<0: servo[i] = 0

	uarm.set_servo_angle(0, servo['s1'])
	uarm.set_servo_angle(1, servo['s2'])
	uarm.set_servo_angle(2, servo['s3'])
	uarm.set_servo_angle(3, servo['s4'])

	print 'Movement: Moved Once'


# attach or detach uarm function once received data from topic
def attchCallback(attachStatus):
	data_input = attachStatus.data

	if data_input.lower() == 'attach' :
		uarm.set_servo_attach()
		print 'uArm: Attach'
	elif data_input.lower() == 'detach':
		uarm.set_servo_detach()
		print 'uArm: Detach'
	else:
		pass


# move to function once received data from topic
def moveToCallback(coords):
	x = coords.x
	y = coords.y
#	if y>0:
#		y = -y
	z = coords.z
	uarm.set_position(x, y, z, 2)
	print 'moveToCallback: [' + str(x) + ', ' + str(y) + ', ' + str(z) + ']'
#	print 'Movement: Moved Once'


# moveto functions once received data from topic
def moveToTimeCallback(coordsAndT):
	x = coordsAndT.x
	y = coordsAndT.y
#	if y>0:
#		y = -y
	z = coordsAndT.z
	time = coordsAndT.time
	if time == 0:
		uarm.set_position(x, y, z, 2)
	else:
		uarm.set_position(x, y, z, 2)

	print 'moveToTimeCallback: [' + str(x) + ', ' + str(y) + ', ' + str(z) + ']'
#	print 'Movement: Moved Once'
	pass


# moveto functions once received data from topic
def moveToTimeAndS4Callback(coordsAndTS4):

	x = coordsAndTS4.x
	y = coordsAndTS4.y
#	if y>0:
#		y = -y
	z = coordsAndTS4.z
	time = coordsAndTS4.time
	s4 = coordsAndTS4.servo_4
	if s4 > 180: s4 = 180
	if s4 <0 : s4 =0
	uarm.set_position(x, y, z, s4, 0, time, 0, 0)
	print 'moveToTimeAndS4Callback: [' + str(x) + ', ' + str(y) + ', ' + str(z) + ']'
#	print 'Movement: Moved Once'
	pass


# print current coords once received data from topic
def currentCoordsCallback(times):

	Times = times.data
	if Times == 1:
		readCurrentCoords()
	elif Times < 0:
		pass
	else:
		for i in range(0,Times):
			readCurrentCoords()
			time.sleep(1)


# print current angles once received data from topic
def readAnglesCallback(times):

	Times = times.data
	if Times == 1:
		readCurrentAngles()
	elif Times < 0:
		pass
	else:
		for i in range(0,Times):
			readCurrentAngles()
			time.sleep(1)


# print stoppers status once received data from topic
def stopperStatusCallback(times):
	Times = times.data
	if Times == 1:
		readStopperStatus()
	elif Times < 0:
		pass
	else:
		for i in range(0,Times):
			readStopperStatus()
			time.sleep(1)


# monitor mode for listening to all topics
def listener():
	print ' '
	print 'Begin monitor mode - listening to all fucntional topics'
	print '======================================================='
	print '         Use rqt_graph to check the connection         '
	print '======================================================='

	rospy.init_node('uarm_core',anonymous=True)

	rospy.Subscriber("uarm_status",String, attchCallback)
	rospy.Subscriber("pump_control",UInt8, pumpCallack)
	rospy.Subscriber("pump_str_control",String, pumpStrCallack)

	rospy.Subscriber("read_coords",Int32, currentCoordsCallback)
	rospy.Subscriber("read_angles",Int32, readAnglesCallback)
	rospy.Subscriber("stopper_status",Int32, stopperStatusCallback)

	rospy.Subscriber("write_angles",Angles, writeAnglesCallback)
	rospy.Subscriber("move_to",Coords, moveToCallback)
	rospy.Subscriber("move_to_time",CoordsWithTime, moveToTimeCallback)
	rospy.Subscriber("move_to_time_s4",CoordsWithTS4, moveToTimeAndS4Callback)

	rospy.spin()
	pass


# show eroors
def processFailedNum(failed_number):

	if failed_number > 20 and failed_number < 26:
		print 'ERROR: Input Connection Address Is Incorrect (failed_number = ' + str(failed_number) + ')'
	if failed_number == 20:
		print 'uArm: Please Connect uArm first '


if __name__ == '__main__':

	try:
		# Connect uarm first
		return_value = connectFcn()

		# Control uarm through commands
		controlFcn()

		# Monitor mode
		if listenerFcn == True:
			listener()

	except:
		processFailedNum(failed_number)
		print 'ERROR: Execution Failed'
		pass

	finally:
		print 'DONE: Program Stopped'
		pass

<filename>scripts/uarm_core.py
#!/usr/bin/env python

'''

# File Name : uarm_core.py
# Author : <NAME>
# Version : V1.0
# Date : 6 Jan, 2016
# Modified Date : 6 Jan, 2016
# Description : This documents is for uarm ROS Library and ROS package
# Copyright(C) 2016 uArm Team. All right reserved.

'''


# All libraries needed to import
# Import system library
import sys
import time
import rospy

# Import uarm for python library
#from UArmForPython.uarm_python import Uarm
import pyuarm

# Import messages type
from std_msgs.msg import String
from std_msgs.msg import Float32
from std_msgs.msg import UInt8
from std_msgs.msg import Int32
from uarm.msg import Angles
from uarm.msg import Coords
from uarm.msg import CoordsWithTime
from uarm.msg import CoordsWithTS4

# Read current Coords function
def readCurrentCoords():
	cc = uarm.get_position()
	print 'Current location is x: %2.2fcm, y: %2.2fcm, z: %2.2fcm.' %(float(cc[0]), float(cc[1]), float(cc[2]))

	rospy.set_param('current_x', cc[0])
	rospy.set_param('current_y', float(cc[1]))
	rospy.set_param('current_z', float(cc[2]))

# Read current Angles function
def readCurrentAngles():
	ra = {}
	ra['s1'] = uarm.get_servo_angle(0)
	ra['s2'] = uarm.get_servo_angle(1)
	ra['s3'] = uarm.get_servo_angle(2)
	ra['s4'] = uarm.get_servo_angle(3)

	print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(ra['s1'], ra['s2'],ra['s3'],ra['s4'])

	rospy.set_param('servo_1',ra['s1'])
	rospy.set_param('servo_2',ra['s2'])
	rospy.set_param('servo_3',ra['s3'])
	rospy.set_param('servo_4',ra['s4'])

# Read stopper function
def readStopperStatus():
	val = uarm.get_tip_sensor()
	if val == True:
		print 'Stopper is actived'
		rospy.set_param('stopper_status','HIGH')
	else:
		print 'Stopper is not actived'
		rospy.set_param('stopper_status','LOW')


# Write single angle
def writeSingleAngle(num, angle):
	# limits of each servo
	s3_max = 170 - uarm.get_servo_angle(1)
	if num == 1:
		if angle > 160:
			angle = 160
		elif angle < 20:
			angle = 20
	elif num == 2:
		if angle > 140:
			angle = 140
		elif angle < 0:
			angle = 0
	elif num == 3:
		if angle > s3_max:
			angle = s3_max
		elif angle < 0:
			angle = 0
	elif num == 4:
		if angle > 180:
			angle = 180
		elif angle < 0:
			angle = 0
	else:
		print "Input incorrect: wrong servo number"

	uarm.set_servo_angle(num-1, angle)


# Write coordinate: x y z and speed
def writeCoordinate(coordinate):
	x = float(coordinate[1])
	y = float(coordinate[2])
	z = float(coordinate[3])
	if len(coordinate) == 4:
		speed = 5

	elif len(coordinate) == 5:
		speed = float(coordinate[4])

	else:
		print "Input incorrect: wrong number of arguments"

	if y > 0:
		y = -y

	uarm.set_position(x, y, z, speed)


# Main connect function
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()
			uarm.connect()
			connectionStatus = 1

			print 'uarm_core: Connected'
			return 21

		elif len(sys.argv) == 3:

			failed_number = 22

			if sys.argv[2] == 'l':
				failed_number = 23
				uarm = pyuarm.get_uarm()
				uarm.connect()
				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()
			uarm.connect()

			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
	#if sys.argv[1] == 'd'


# Main control function
def controlFcn():

	while controlFcnLoop:
		commands = raw_input("Input Commands (Input h to see all commands): ")

		if commands == 'h':
			print ' '
			print 'Control mode'
			print '=================================================================='
			print 'h                  - show help and list all commands'
			print 'cc                 - current coordinates'
			print 'ra                 - read angles'
			print 'wa a1 a2 a3 a4     - write angles'
			print '                     for example: wa 120 80 40 40'
			print 'wsa servo a        - write single angle'
			print '                     for example: wsa 1 50'
			print 'mt x y z           - move to a point with speed(defaut: 5mm/s)'
			print '                     for example: mt 0 12 12'
			print 'mt x y z speed     - move to a point with speed(mm/s)'
			print '                     for example: mt 0 12 12 10'
			print 'pp 1/0             - pump on/off'
			print 'ss                 - status of stopper'
			print 'at                 - attach uarm'
			print 'de                 - detach uarm'
			print 'l                  - begin listener mode'
			print 'e                  - exit'
			print '=================================================================='
			print ' '

		elif commands == 'l':
			print 'Exit: Break the control fuction loop'
			break;

		elif commands == 'e':
			print 'Detach all servos and exit the program'
			uarm.set_servo_detach()
			sys.exit(0)

		elif len(commands) == 0:
			print 'len is 0'

		else:
			commands_split = commands.split()

			# Detach
			if commands_split[0] == 'detach'or commands_split[0] == 'de':
				if len(commands_split) == 1:
					uarm.set_servo_detach()
				else:
					print 'no other commands should be input'
				pass

			# Attach
			if commands_split[0] == 'attach'or commands_split[0] == 'at':
				if len(commands_split) == 1:
					uarm.set_servo_attach()
				else:
					print 'no other commands should be input'
				pass

			# Stopper Status
			elif commands_split[0] == 'stopperStatus'or commands_split[0] == 'ss':
				if len(commands_split) == 1:
					readStopperStatus()
				else:
					print 'no other commands should be input'
				pass

			# Pump Control
			elif commands_split[0] == 'pump' or commands_split[0] == 'pp':
				if len(commands_split) == 1:
					print 'Status should be input'
				elif len(commands_split) >2:
					print 'Too many inputs'
				else:
					if commands_split[1] == '1' or commands_split[1].lower() == 'high' or commands_split[1].lower() == 'on':
						uarm.set_pump(1)
					elif commands_split[1] == '0' or commands_split[1].lower() == 'low'or commands_split[1].lower() == 'off':
						uarm.set_pump(0)
					else:
						print 'Incorrect inputs, should input 1 / 0 / HIGH / LOW / ON / OFF'
				pass

			# Current Coordinates
			elif commands_split[0] == 'currentCoords' or commands_split[0] == 'cc':
				if len(commands_split) == 1:
					readCurrentCoords()
				else:
					print 'no other commands should be input'
				pass

			# Write Single Angles
			elif commands_split[0] == 'writeSingleAngles' or commands_split[0] == 'wsa':
				if len(commands_split) == 3:
					writeSingleAngle(int(commands_split[1]), float(commands_split[2]))
				else:
					print 'Input incorrect: Wrong number of arguments'
				pass

			# Write Angles
			elif commands_split[0] == 'wa':
				if len(commands_split) == 5:
					writeSingleAngle(1, float(commands_split[1]))
					writeSingleAngle(2, float(commands_split[2]))
					writeSingleAngle(3, float(commands_split[3]))
					writeSingleAngle(4, float(commands_split[4]))
				else:
					print 'Input incorrect: Wrong number of arguments'
				pass

			# Current Angles
			elif commands_split[0] == 'readAngles' or commands_split[0] == 'ra':
				if len(commands_split) == 1:
					 readCurrentAngles()
				else:
					print 'no other commands should be input'
				pass

			# Move Tos
			elif commands_split[0] == 'moveTo' or commands_split[0] == 'mt':
				if len(commands_split) == 4 or len(commands_split) == 5:
					writeCoordinate(commands_split)
				else:
					print 'Input incorrect: Wrong number of arguments'
			else:
				pass


# pump control function once received data from topic
def pumpCallack(data):

	data_input = data.data

	if data_input == 0:
		uarm.pump_control(0)
		print 'Pump: Off'
	elif data_input == 1:
		uarm.pump_control(1)
		print 'Pump: On'
	else:
		pass


# pump str control function once received data from topic
def pumpStrCallack(data):

	data_input = data.data
	print data_input

	if data_input.lower() == 'low' or data_input.lower() == 'off':
		uarm.set_pump(0)
		print 'Pump: Off'
	elif data_input.lower() == 'on' or data_input.lower() == 'high':
		uarm.set_pump(1)
		print 'Pump: On'
	else:
		pass


# angles control function once received data from topic
def writeAnglesCallback(servos):

	servo = {}
	servo['s1'] = servos.servo_1
	servo['s2'] = servos.servo_2
	servo['s3'] = servos.servo_3
	servo['s4'] = servos.servo_4
	for i in servo:
		if servo[i]>180: servo[i] = 180
		if servo[i]<0: servo[i] = 0

	uarm.set_servo_angle(0, servo['s1'])
	uarm.set_servo_angle(1, servo['s2'])
	uarm.set_servo_angle(2, servo['s3'])
	uarm.set_servo_angle(3, servo['s4'])

	print 'Movement: Moved Once'


# attach or detach uarm function once received data from topic
def attchCallback(attachStatus):
	data_input = attachStatus.data

	if data_input.lower() == 'attach' :
		uarm.set_servo_attach()
		print 'uArm: Attach'
	elif data_input.lower() == 'detach':
		uarm.set_servo_detach()
		print 'uArm: Detach'
	else:
		pass


# move to function once received data from topic
def moveToCallback(coords):
	x = coords.x
	y = coords.y
#	if y>0:
#		y = -y
	z = coords.z
	uarm.set_position(x, y, z, 2)
	print 'moveToCallback: [' + str(x) + ', ' + str(y) + ', ' + str(z) + ']'
#	print 'Movement: Moved Once'


# moveto functions once received data from topic
def moveToTimeCallback(coordsAndT):
	x = coordsAndT.x
	y = coordsAndT.y
#	if y>0:
#		y = -y
	z = coordsAndT.z
	time = coordsAndT.time
	if time == 0:
		uarm.set_position(x, y, z, 2)
	else:
		uarm.set_position(x, y, z, 2)

	print 'moveToTimeCallback: [' + str(x) + ', ' + str(y) + ', ' + str(z) + ']'
#	print 'Movement: Moved Once'
	pass


# moveto functions once received data from topic
def moveToTimeAndS4Callback(coordsAndTS4):

	x = coordsAndTS4.x
	y = coordsAndTS4.y
#	if y>0:
#		y = -y
	z = coordsAndTS4.z
	time = coordsAndTS4.time
	s4 = coordsAndTS4.servo_4
	if s4 > 180: s4 = 180
	if s4 <0 : s4 =0
	uarm.set_position(x, y, z, s4, 0, time, 0, 0)
	print 'moveToTimeAndS4Callback: [' + str(x) + ', ' + str(y) + ', ' + str(z) + ']'
#	print 'Movement: Moved Once'
	pass


# print current coords once received data from topic
def currentCoordsCallback(times):

	Times = times.data
	if Times == 1:
		readCurrentCoords()
	elif Times < 0:
		pass
	else:
		for i in range(0,Times):
			readCurrentCoords()
			time.sleep(1)


# print current angles once received data from topic
def readAnglesCallback(times):

	Times = times.data
	if Times == 1:
		readCurrentAngles()
	elif Times < 0:
		pass
	else:
		for i in range(0,Times):
			readCurrentAngles()
			time.sleep(1)


# print stoppers status once received data from topic
def stopperStatusCallback(times):
	Times = times.data
	if Times == 1:
		readStopperStatus()
	elif Times < 0:
		pass
	else:
		for i in range(0,Times):
			readStopperStatus()
			time.sleep(1)


# monitor mode for listening to all topics
def listener():
	print ' '
	print 'Begin monitor mode - listening to all fucntional topics'
	print '======================================================='
	print '         Use rqt_graph to check the connection         '
	print '======================================================='

	rospy.init_node('uarm_core',anonymous=True)

	rospy.Subscriber("uarm_status",String, attchCallback)
	rospy.Subscriber("pump_control",UInt8, pumpCallack)
	rospy.Subscriber("pump_str_control",String, pumpStrCallack)

	rospy.Subscriber("read_coords",Int32, currentCoordsCallback)
	rospy.Subscriber("read_angles",Int32, readAnglesCallback)
	rospy.Subscriber("stopper_status",Int32, stopperStatusCallback)

	rospy.Subscriber("write_angles",Angles, writeAnglesCallback)
	rospy.Subscriber("move_to",Coords, moveToCallback)
	rospy.Subscriber("move_to_time",CoordsWithTime, moveToTimeCallback)
	rospy.Subscriber("move_to_time_s4",CoordsWithTS4, moveToTimeAndS4Callback)

	rospy.spin()
	pass


# show eroors
def processFailedNum(failed_number):

	if failed_number > 20 and failed_number < 26:
		print 'ERROR: Input Connection Address Is Incorrect (failed_number = ' + str(failed_number) + ')'
	if failed_number == 20:
		print 'uArm: Please Connect uArm first '


if __name__ == '__main__':

	try:
		# Connect uarm first
		return_value = connectFcn()

		# Control uarm through commands
		controlFcn()

		# Monitor mode
		if listenerFcn == True:
			listener()

	except:
		processFailedNum(failed_number)
		print 'ERROR: Execution Failed'
		pass

	finally:
		print 'DONE: Program Stopped'
		pass

#!/usr/bin/env python # File Name : uarm_core.py # Author : <NAME> # Version : V1.0 # Date : 6 Jan, 2016 # Modified Date : 6 Jan, 2016 # Description : This documents is for uarm ROS Library and ROS package # Copyright(C) 2016 uArm Team. All right reserved. # All libraries needed to import # Import system library # Import uarm for python library #from UArmForPython.uarm_python import Uarm # Import messages type # Read current Coords function # Read current Angles function # Read stopper function # Write single angle # limits of each servo # Write coordinate: x y z and speed # Main connect function # 1 means no input argument # followed by l - directly listen to nodes # 2 means input argument is wrong #if sys.argv[1] == 'd' # Main control function # Detach # Attach # Stopper Status # Pump Control # Current Coordinates # Write Single Angles # Write Angles # Current Angles # Move Tos # pump control function once received data from topic # pump str control function once received data from topic # angles control function once received data from topic # attach or detach uarm function once received data from topic # move to function once received data from topic #	if y>0: #		y = -y #	print 'Movement: Moved Once' # moveto functions once received data from topic #	if y>0: #		y = -y #	print 'Movement: Moved Once' # moveto functions once received data from topic #	if y>0: #		y = -y #	print 'Movement: Moved Once' # print current coords once received data from topic # print current angles once received data from topic # print stoppers status once received data from topic # monitor mode for listening to all topics # show eroors # Connect uarm first # Control uarm through commands # Monitor mode