Python PyMata.servo_config方法代码示例

# 需要导入模块: from PyMata.pymata import PyMata [as 别名]
# 或者: from PyMata.pymata.PyMata import servo_config [as 别名]
from PyMata.pymata import PyMata

def signal_handler(sig, frame):
    print('You pressed Ctrl+C')
    if board is not None:
        board.reset()
    sys.exit(0)

signal.signal(signal.SIGINT, signal_handler)

# Create a PyMata instance
board = PyMata("/dev/ttyACM0")

# Set the pin mode
board.servo_config(5)
board.set_pin_mode(12, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(0, board.INPUT, board.ANALOG)

# Send query request to Arduino
board.capability_query()

# Some boards take a long time to respond - adjust as needed
time.sleep(5)
print("Pin Capability Report")
print(board.get_capability_query_results())

print("PyMata Digital Response Table")
print(board.get_digital_response_table())

print("PyMata Analog Response Table")

# 需要导入模块: from PyMata.pymata import PyMata [as 别名]
# 或者: from PyMata.pymata.PyMata import servo_config [as 别名]
SERVO_MOTOR = 9  # servo attached to this pin

# create a PyMata instance
board = PyMata("/dev/ttyACM0")


def signal_handler(sig, frame):
    print('You pressed Ctrl+C!!!!')
    if board is not None:
        board.reset()
    sys.exit(0)

signal.signal(signal.SIGINT, signal_handler)
# control the servo - note that you don't need to set pin mode
# configure the servo
board.servo_config(SERVO_MOTOR)

while (1):
    # move the servo to 20 degrees
    board.analog_write(SERVO_MOTOR, 20)
    time.sleep(1)

    # move the servo to 100 degrees
    board.analog_write(SERVO_MOTOR, 100)
    time.sleep(1)

# move the servo to 20 degrees
board.analog_write(SERVO_MOTOR, 20)

# close the interface down cleanly
board.close()

# 需要导入模块: from PyMata.pymata import PyMata [as 别名]
# 或者: from PyMata.pymata.PyMata import servo_config [as 别名]
class Firmata(Adaptor):

    def __init__(self, options):
        super(Firmata, self).__init__(options)

        if 'port' not in options:
            raise self.ParameterRequired(
                'A port must be specified for Firmata connection.'
            )

        self.port = options.get('port')
        self.board = PyMata('/dev/ttyACM0', verbose=True)

        signal.signal(signal.SIGINT, self.signal_handler)

        self.pins = {
            'digital': [],
            'analog': [],
            'pwm': [],
            'servo': [],
            'i2c': [],
        }

    def analog_write(self, pin_number, value):
        if pin_number not in self.pins['analog']:
            self.pins['analog'].append(pin_number)
            self.board.set_pin_mode(
                pin_number,
                self.board.OUTPUT,
                self.board.ANALOG
            )

        self.board.analog_write(pin_number, value)

    def analog_read(self, pin_number):
        if pin_number not in self.pins['analog']:
            self.pins['analog'].append(pin_number)
            self.board.set_pin_mode(
                pin_number,
                self.board.INPUT,
                self.board.ANALOG
            )

        return self.board.analog_read(pin_number)

    def digital_write(self, pin_number, value):
        if pin_number not in self.pins['digital']:
            self.pins['digital'].append(pin_number)
            self.board.set_pin_mode(
                pin_number,
                self.board.OUTPUT,
                self.board.DIGITAL
            )

        self.board.digital_write(pin_number, value)

    def digital_read(self, pin_number):
        if pin_number not in self.pins['digital']:
            self.pins['digital'].append(pin_number)
            self.board.set_pin_mode(
                pin_number,
                self.board.INPUT,
                self.board.DIGITAL
            )

        return self.board.analog_write(pin_number)

    def pwm_write(self, pin_number, value):
        if pin_number not in self.pins['pwm']:
            self.pins['pwm'].append(pin_number)
            self.board.set_pin_mode(
                pin_number,
                self.board.PWM,
                self.board.DIGITAL
            )

        return self.board.analog_write(pin_number, value)

    def pwm_read(self, pin_number):
        if pin_number not in self.pins['pwm']:
            self.pins['pwm'].append(pin_number)
            self.board.set_pin_mode(
                pin_number,
                self.board.PWM,
                self.board.DIGITAL
            )

        return self.board.analog_read(pin_number)

    def servo_write(self, pin_number, value):
        if pin_number not in self.pins['servo']:
            self.pins['servo'].append(pin_number)
            self.board.servo_config(pin_number)

        self.board.analog_write(pin_number, value)

    def disconnect(self):
        # Close the firmata interface down cleanly
        self.board.close()

#.........这里部分代码省略.........

# 需要导入模块: from PyMata.pymata import PyMata [as 别名]
# 或者: from PyMata.pymata.PyMata import servo_config [as 别名]
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()

# 需要导入模块: from PyMata.pymata import PyMata [as 别名]
# 或者: from PyMata.pymata.PyMata import servo_config [as 别名]
firmata.set_pin_mode(BUTTON_SWITCH, firmata.INPUT, firmata.DIGITAL)

# wait for the button switch to be pressed

while not firmata.digital_read(BUTTON_SWITCH):
    time.sleep(.1)
    pass
print firmata.digital_read(BUTTON_SWITCH)

# send out a beep in celebration of detecting the button press
# note that you don't need to set pin mode for play_tone
firmata.play_tone(BEEPER, firmata.TONE_TONE, 1000, 500)

# control the servo - note that you don't need to set pin mode
# configure the servo
firmata.servo_config(SERVO_MOTOR)

# move the servo to 20 degrees
firmata.analog_write(SERVO_MOTOR, 20)
time.sleep(.5)

# move the servo to 100 degrees
firmata.analog_write(SERVO_MOTOR, 100)
time.sleep(.5)

# move the servo to 20 degrees
firmata.analog_write(SERVO_MOTOR, 20)

# close the interface down cleanly
firmata.close()

# 需要导入模块: from PyMata.pymata import PyMata [as 别名]
# 或者: from PyMata.pymata.PyMata import servo_config [as 别名]
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()

# 需要导入模块: from PyMata.pymata import PyMata [as 别名]
# 或者: from PyMata.pymata.PyMata import servo_config [as 别名]
SENSOR = 0

# Indices for data passed to callback function
PIN_MODE = 0  # This is the PyMata Pin MODE = ANALOG = 2 and DIGITAL = 0x20:
PIN_NUMBER = 1
DATA_VALUE = 2


def cb_sensor(data):
    print("Analog Data: ", " Pin: ", data[PIN_NUMBER], " Pin Mode: ", data[PIN_MODE], " Data Value: ", data[DATA_VALUE])
    # data = data[2]
    # pub.publish(data)


arduino = PyMata("/dev/ttyACM0", verbose=True)
arduino.servo_config(SERVO_MOTOR)
arduino.set_pin_mode(SENSOR, arduino.INPUT, arduino.ANALOG, cb_sensor)
# arduino = serial.Serial('/dev/ttyACM0', 9600);


def callback(data):
    servo_pose = data.finger_pose
    arduino.analog_write(SERVO_MOTOR, servo_pose)
    rospy.loginfo(rospy.get_caller_id() + " I heard %d", servo_pose)


def listener():
    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'talker' node so that multiple talkers can

# 需要导入模块: from PyMata.pymata import PyMata [as 别名]
# 或者: from PyMata.pymata.PyMata import servo_config [as 别名]
# Indices for data passed to callback function
PIN_MODE = 0  # This is the PyMata Pin MODE = ANALOG = 2 and DIGITAL = 0x20:
PIN_NUMBER = 1
DATA_VALUE = 2

def cb_sensor(data):
    print("Analog Data: ",
          " Pin: ", data[PIN_NUMBER],
          " Pin Mode: ", data[PIN_MODE],
          " Data Value: ", data[DATA_VALUE])
    # data = data[2]
    # pub.publish(data)

arduino = PyMata("/dev/ttyACM0", verbose=True)
for pin in SERVO_PINS:
    arduino.servo_config(pin)
arduino.set_pin_mode(SENSOR, arduino.INPUT, arduino.ANALOG, cb_sensor)
# arduino = serial.Serial('/dev/ttyACM0', 9600);


def callback(data):
    servo_pose = data.finger_pose
    for pin in SERVO_PINS:
        arduino.analog_write(pin, servo_pose)
    rospy.loginfo(rospy.get_caller_id() + " I heard %d", servo_pose)

def listener():
    # In ROS, nodes are uniquely named. If two nodes with the same
    # node are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'talker' node so that multiple talkers can

# 需要导入模块: from PyMata.pymata import PyMata [as 别名]
# 或者: from PyMata.pymata.PyMata import servo_config [as 别名]
def servo_callback(data):
    if DEBUG:
        print("SUB:DATA:"+str(data.data))
    '''write to servo port'''
    pos=int(data.data)
    ''' fake map adc values 0-1024 to 0-180 for servo '''
    pos=int(pos/6)
    if DEBUG:
        print("Set servo to: "+str(pos))
    board.analog_write(SERVO_PIN, pos)

if __name__ == '__main__':
    try:
        ''' init ros node '''
        pub_adc = rospy.Publisher("pymata/adc", String, queue_size=10)
        rospy.init_node("pymata")
        rospy.Subscriber("pymata/adc", String, servo_callback)
        ''' init pymata and ports '''
        board = PyMata(USB_PORT)
        if DEBUG:
            board.refresh_report_firmware()
            print(board.get_firmata_firmware_version())
        board.servo_config(SERVO_PIN)
        board.set_pin_mode(ADC_PIN,board.INPUT,board.ANALOG,adc_callback)
        ''' ros loop '''
        rospy.spin()
    except rospy.ROSInterruptException: pass