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