class FSMHand():
def __init__(self):
# Initialize fingers and wrist pos
self.t_pos = 0
self.i_pos = 0
self.m_pos = 0
self.r_pos = 0
self.l_pos = 0
self.w_pos = 90
# Initialize sensor values
self.t_sen = 0
self.i_sen = 0
self.m_sen = 0
self.r_sen = 0
self.l_sen = 0
# Initialize the arduino
self.arduino = PyMata("/dev/ttyACM0", verbose=True)
# Initialize the servos and sensors on arduino
for pin in SERVO_PINS:
self.arduino.servo_config(pin)
sensor_pin = 0
self.arduino.enable_analog_reporting(sensor_pin)
sensor_pin += 1
# Initialize the hand states
self.curstate = 'open'
self.states = {}
self.transitionTable = {}
#states are a dictionary of name/function pairints stored in a dictionary
#i.e. {'open':self.Open}
def AddFSMState(self,name,function):
self.states[name] = function
#each state gets its own transition table
#a transition table is a list of states to switch to
#given a "event"
def AddFSMTransition(self,name,transitionDict):
#yes we are making a dictionary the value bound to a dictionary key
self.transitionTable[name] = transitionDict
def move_callback(self, data):
servo_pose = data.finger_pose
if self.curstate == 'open':
self.arduino.analog_write(MIDDLE_SERVO, servo_pose)
self.m_pos = servo_pose
rospy.loginfo(rospy.get_caller_id() + " I heard %d", servo_pose)
def RunFSM(self):
pub = rospy.Publisher('finger_status', Pressure, queue_size=10)
rate = rospy.Rate(50)
while not rospy.is_shutdown():
self.m_sen = self.arduino.analog_read(THUMB_SENSOR)
outdata = Pressure()
outdata.sensor1 = self.m_sen
pub.publish(outdata)
if self.m_sen > 500 or self.m_pos == 181:
self.curstate = 'close'
else:
self.curstate = 'open'
print "Current State: ", self.curstate
rate.sleep()
class FSMHand():
def __init__(self):
# Initialize fingers and wrist pos
self.finger_pos = [0, 0, 0, 0, 0, 90]
# Initialize sensor values
self.sensor_val = [0, 0, 0, 0, 0]
# Initialize the arduino
self.arduino = PyMata("/dev/ttyACM0", verbose=True)
# Initialize the servos and sensors on arduino
for servo_pin in SERVO_PINS:
self.arduino.servo_config(servo_pin)
for sensor_pin in SENSOR_PINS:
self.arduino.enable_analog_reporting(sensor_pin)
# Initialize the hand states
self.curstate = 'open'
self.states = {}
self.transitionTable = {}
#states are a dictionary of name/function pairints stored in a dictionary
#i.e. {'open':self.Open}
def AddFSMState(self,name,function):
self.states[name] = function
#each state gets its own transition table
#a transition table is a list of states to switch to
#given a "event"
def AddFSMTransition(self,name,transitionDict):
#yes we are making a dictionary the value bound to a dictionary key
self.transitionTable[name] = transitionDict
def Move_callback(self, data):
servo_pose = data.finger_pose
if self.curstate == 'open':
for i, pin in enumerate(FINGER_PINS):
self.arduino.analog_write(pin, servo_pose)
self.finger_pos[i] = servo_pose
rospy.loginfo(rospy.get_caller_id() + " I heard %d", servo_pose)
def Event_handler(self):
curstatefunction = self.states[self.curestate]
curstatefunction()
def RunFSM(self):
pub = rospy.Publisher('finger_status', Pressure, queue_size=10)
rate = rospy.Rate(50)
while not rospy.is_shutdown():
for i, sensor_pin in enumerate(SENSOR_PINS):
self.sensor_val[i] = self.arduino.analog_read(sensor_pin)
outdata = Pressure()
outdata.sensor1 = self.sensor_val[0]
pub.publish(outdata)
if max(self.sensor_val) > 500 or max(self.finger_pos) == 150:
self.curstate = 'close'
else:
self.curstate = 'open'
print "Current State: ", self.curstate
rate.sleep()
