class Servo: """ 舵机对象 """ def __init__(self, pin_num, port="/dev/ttyACM0", debug=False): """ :param pin_num: 接入舵机的针脚编号,编号范围0~19 :param port: 虚谷连接舵机的COM口,默认为"/dev/ttyACM0" :param debug: 当为True的时候,会输出debug信息 """ pin_num, _ = check_pin_num(pin_num) self.pin_num = pin_num self.board = PyMata(port, bluetooth=False, verbose=debug) self.board.servo_config(pin_num) def angle(self, angle): """ 舵机转动方法 :param angle: 舵机转动角度 :return: """ self.board.analog_write(self.pin_num, angle) def speed(self, angle): """ 舵机持续转动方法 :param angle: 舵机转动角度 :return: """ while 1: self.board.analog_write(self.pin_num, angle) time.sleep(0.5)
def main(): port = '/dev/ttyACM0' base = 0.3 gust = numpy.sin(numpy.linspace(0, 2 * 3.14, 100)) wind = base + (0.1) * gust print(wind) board = PyMata(port, verbose=True) GAUGE_PIN = 6 POTENTIOMETER_ANALOG_PIN = 0 board.set_pin_mode(13, board.OUTPUT, board.DIGITAL) #board.set_pin_mode(FAN_PIN, board.PWM, board.ANALOG) board.servo_config(GAUGE_PIN) board.set_pin_mode(POTENTIOMETER_ANALOG_PIN, board.INPUT, board.ANALOG) while True: pot = board.analog_read(POTENTIOMETER_ANALOG_PIN) deg = 180 - int(180.0 * (pot / 1023.0)) print("{:4} => {:3}".format(pot, deg)) board.analog_write(GAUGE_PIN, deg) gevent.sleep(0.025) for _ in range(0, 10): for v in list(wind): print(v) board.analog_write(FAN_PIN, (int)(v * 255)) gevent.sleep(0.025)
def main(): notifier = sdnotify.SystemdNotifier() the_ipc_session = wp_ipc.Session() port = os.environ.get('FIRMATA_PORT', '/dev/ttyACM0') board = PyMata(port, verbose=True, bluetooth=False) board.set_pin_mode(LED_PIN, board.OUTPUT, board.DIGITAL) board.set_pin_mode(BUTTON_PIN, board.PULLUP, board.DIGITAL) board.set_pin_mode(FAN_PIN, board.PWM, board.ANALOG) board.servo_config(GAUGE_PIN) board.set_pin_mode(POTENTIOMETER_ANALOG_PIN, board.INPUT, board.ANALOG) notifier.notify("READY=1") while True: published_values = the_ipc_session.recv() for topic, value in published_values.items(): if "fan_duty" == topic: fan_duty255 = int(255 * value) if fan_duty255 > 255: fan_duty255 = 255 if fan_duty255 < 0: fan_duty255 = 0 board.analog_write(FAN_PIN, fan_duty255) elif "gauge_degrees" == topic: board.analog_write(GAUGE_PIN, 180 - value) elif "led" == topic: board.digital_write(LED_PIN, value) pot1024 = board.analog_read(POTENTIOMETER_ANALOG_PIN) pot = (1.0 / 1024.0) * pot1024 button_pressed = not bool(board.digital_read(BUTTON_PIN)) the_ipc_session.send("potentiometer", pot) the_ipc_session.send("button", button_pressed) gevent.sleep(0.100) notifier.notify("WATCHDOG=1")
def main(): port = '/dev/ttyACM0' base = 0.6 gust = numpy.sin(numpy.linspace(0, 2 * 3.14, 100)) wind = base + (0.3) * gust print(wind) board = PyMata(port, verbose=True) FAN_PIN = 3 GAUGE_PIN = 6 POTENTIOMETER_ANALOG_PIN = 0 board.set_pin_mode(13, board.OUTPUT, board.DIGITAL) board.set_pin_mode(FAN_PIN, board.PWM, board.ANALOG) board.servo_config(GAUGE_PIN) board.set_pin_mode(POTENTIOMETER_ANALOG_PIN, board.INPUT, board.ANALOG) for _ in range(0, 100): for v in list(wind): print(v) pot1024 = board.analog_read(POTENTIOMETER_ANALOG_PIN) pot = (1.0 / 1024.0) * pot1024 v_scaled = pot * v gauge_degrees = 180 - int(180.0 * v_scaled) board.analog_write(GAUGE_PIN, gauge_degrees) fan_duty = int(255 * v_scaled) board.analog_write(FAN_PIN, fan_duty) gevent.sleep(0.025)
import time import sys import signal from capture import pictureCapture from PyMata.pymata import PyMata from pymata_aio.pymata3 import PyMata3 import cv2 FLAG = 1 # 超声波传感器初始化 board = PyMata("COM5", verbose=True) board.sonar_config(12, 12) # 舵机初始化 SERVO_MOTOR = 9 board.servo_config(SERVO_MOTOR) # 视频初始化 # 杆子抬起放下操作 def arise_down(): board.analog_write(SERVO_MOTOR, 0) time.sleep(3) board.analog_write(SERVO_MOTOR, 95) # time.sleep(5) print('车辆通行') # board.analog_write(SERVO_MOTOR, 0) # 读取距离当距离小于一定值时,返回开始进行操作
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()
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")
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 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")
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 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() def signal_handler(self, sig, frame): print('Ctrl+C pressed') if self.board is not None: self.board.reset() sys.exit(0) class ParameterRequired(Exception): def __init__(self, message='A required parameter was not provided.'): super(Firmata.ParameterRequired, self).__init__(message) def __str__(self): return self.message
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()
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
# 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
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