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)
signal.signal(signal.SIGINT, signal_handler)
# Set pin modes
board.set_pin_mode(GREEN_LED, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(RED_LED, board.PWM, board.DIGITAL)
board.set_pin_mode(PUSH_BUTTON, board.INPUT, board.DIGITAL)
board.set_pin_mode(POTENTIOMETER, board.INPUT, board.ANALOG)
board.set_analog_latch(POTENTIOMETER, board.ANALOG_LATCH_GTE, 1000)
# Do nothing loop - program exits when latch data event occurs for
# potentiometer
while 1:
count += 1
if count == 300:
print('Terminated')
board.close()
analog = board.analog_read(POTENTIOMETER)
board.analog_write(RED_LED, analog // 4)
digital = board.digital_read(PUSH_BUTTON)
board.digital_write(GREEN_LED, digital)
latch = board.get_analog_latch_data(POTENTIOMETER)
if latch[1] == board.LATCH_LATCHED:
board.analog_write(RED_LED, 0)
board.digital_write(GREEN_LED, 0)
print('Latching Event Occurred at: ' + \
time.asctime(time.gmtime(latch[3])))
board.close()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
# set pin modes
board.set_pin_mode(GREEN_LED, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(RED_LED, board.PWM, board.DIGITAL)
board.set_pin_mode(PUSH_BUTTON, board.INPUT, board.DIGITAL)
board.set_pin_mode(POTENTIOMETER, board.INPUT, board.ANALOG)
board.set_analog_latch(POTENTIOMETER, board.ANALOG_LATCH_GTE, 1000)
# do nothing loop - program exits when latch data event occurs for potentiometer
while 1:
count += 1
if count == 300:
print('bye bye')
board.close()
analog = board.analog_read(POTENTIOMETER)
board.analog_write(RED_LED, analog // 4)
digital = board.digital_read(PUSH_BUTTON)
board.digital_write(GREEN_LED, digital)
latch = board.get_analog_latch_data(POTENTIOMETER)
if latch[1] == board.LATCH_LATCHED:
board.analog_write(RED_LED, 0)
board.digital_write(GREEN_LED, 0)
print('Latching Event Occurred at: ' + time.asctime(time.gmtime(latch[3])))
board.close()
sys.exit(0)
# Set the pin mode for a digital output pin firmata.set_pin_mode(RED_LED, firmata.OUTPUT, firmata.DIGITAL) # Turn on the RED LED firmata.digital_write(RED_LED, 1) # Wait 3 seconds and turn it off time.sleep(3) firmata.digital_write(RED_LED, 0) # Set the white led for pwm operation firmata.set_pin_mode(WHITE_LED, firmata.PWM, firmata.DIGITAL) # Set the white led to full brightness (255) for 1 second firmata.analog_write(WHITE_LED, 255) time.sleep(1) # now set it to half brightness for 1 second firmata.analog_write(WHITE_LED, 128) time.sleep(1) # and finally extinguish it firmata.analog_write(WHITE_LED, 0) # set potentiometer pin as an analog input firmata.set_pin_mode(POTENTIOMETER, firmata.INPUT, firmata.ANALOG) # allow some time for the first data to arrive from the Arduino and be # processed. time.sleep(.2)
This example illustrates using callbacks for digital and analog input.
Monitor the current analog input and digital input of two pins.
"""
import signal
import sys
import time
from PyMata.pymata import PyMata
# Instantiate PyMata
board = PyMata("/dev/ttyACM0", verbose=True)
board.encoder_config(7,2)
# Set up pin modes for both pins with callbacks for each
board.set_pin_mode(5, board.PWM, board.DIGITAL)
board.set_pin_mode(6, board.PWM, board.DIGITAL)
board.analog_write(5, 255)
board.analog_write(6, 0)
time.sleep(2)
count = board.digital_read(2)
print count
time.sleep(2)
board.analog_write(5, 0)
class ArduinoFirmata(Board, AnalogInputCapable, PwmOutputCapable):
def __init__(self, port=None):
try:
from PyMata.pymata import PyMata as PyMata # noqa
except ImportError:
msg = 'pingo.arduino.Arduino requires PyMata installed'
raise ImportError(msg)
super(ArduinoFirmata, self).__init__()
self.port = port
self.firmata_client = PyMata(self.port, verbose=VERBOSE)
self.firmata_client.capability_query()
time.sleep(10) # TODO: Find a small and safe value
capability_query_results = self.firmata_client.get_capability_query_results()
capability_dict = pin_list_to_board_dict(capability_query_results)
self._add_pins(
[DigitalPin(self, location)
for location in capability_dict['digital']] +
[PwmPin(self, location)
for location in capability_dict['pwm']] +
[AnalogPin(self, 'A%s' % location, resolution=10)
for location in capability_dict['analog']]
)
def cleanup(self):
# self.firmata_client.close() has sys.exit(0)
if hasattr(self, 'PyMata'):
try:
self.firmata_client.transport.close()
except AttributeError:
pass
def __repr__(self):
cls_name = self.__class__.__name__
return '<{cls_name} {self.port!r}>'.format(**locals())
def _set_digital_mode(self, pin, mode):
self.firmata_client.set_pin_mode(
pin.location,
PIN_MODES[mode],
self.firmata_client.DIGITAL
)
def _set_analog_mode(self, pin, mode):
pin_id = int(pin.location[1:])
self.firmata_client.set_pin_mode(
pin_id,
self.firmata_client.INPUT,
self.firmata_client.ANALOG
)
def _set_pwm_mode(self, pin, mode):
pin_id = int(pin.location)
self.firmata_client.set_pin_mode(
pin_id,
self.firmata_client.PWM,
self.firmata_client.DIGITAL
)
def _get_pin_state(self, pin):
_state = self.firmata_client.digital_read(pin.location)
if _state == self.firmata_client.HIGH:
return pingo.HIGH
return pingo.LOW
def _set_pin_state(self, pin, state):
self.firmata_client.digital_write(
pin.location,
PIN_STATES[state]
)
def _get_pin_value(self, pin):
pin_id = int(pin.location[1:])
return self.firmata_client.analog_read(pin_id)
def _set_pwm_duty_cycle(self, pin, value):
pin_id = int(pin.location)
firmata_value = int(value * 255)
return self.firmata_client.analog_write(pin_id, firmata_value)
def _set_pwm_frequency(self, pin, value):
raise NotImplementedError
# 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)
# move the servo to 20 degrees
board.analog_write(SERVO_MOTOR, 20)
time.sleep(.5)
# move the servo to 100 degrees
board.analog_write(SERVO_MOTOR, 100)
time.sleep(.5)
# move the servo to 20 degrees
board.analog_write(SERVO_MOTOR, 20)
# close the interface down cleanly
board.close()
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()
gforce = conn.add_stream(getattr, flight, "g_force")
speed = conn.add_stream(getattr, flight, "speed")
control = vessel.control
sas = conn.add_stream(getattr, control, "sas")
gear = conn.add_stream(getattr, control, "gear")
lights = conn.add_stream(getattr, control, "lights")
brakes = conn.add_stream(getattr, control, "brakes")
# Your Application Continues Below This Point
print "We are go!"
while 1 :
s = speed()
if s > 400:
s = 400
board.analog_write(SPEEDOMETER, int(s * 255 / 400))
f = 0
mf = maxFuel()
if mf > 0 :
f = fuel() / mf
board.analog_write(FUELGAUGE, int(f * 255))
g = math.fabs(gforce()) / 40
if g > 1:
g = 1
board.analog_write(GFORCEMETER, int(g * 255))
board.digital_write(SASLAMP, sas())
board.digital_write(GEARLAMP, gear())
board.digital_write(LAMPLAMP, lights())
class Motors(Thread):
MOTOR_1_PWM = 2
MOTOR_1_A = 3
MOTOR_1_B = 4
MOTOR_2_PWM = 5
MOTOR_2_A = 6
MOTOR_2_B = 7
MOTOR_3_PWM = 8
MOTOR_3_A = 9
MOTOR_3_B = 10
def __init__(self):
Thread.__init__(self)
self.daemon = True
self.board = PyMata()
def signal_handler(sig, frame):
self.stop_motors()
self.board.reset()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
self.board.set_pin_mode(self.MOTOR_1_PWM, self.board.PWM, self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_1_A, self.board.OUTPUT, self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_1_B, self.board.OUTPUT, self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_2_PWM, self.board.PWM, self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_2_A, self.board.OUTPUT, self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_2_B, self.board.OUTPUT, self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_3_PWM, self.board.PWM, self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_3_A, self.board.OUTPUT, self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_3_B, self.board.OUTPUT, self.board.DIGITAL)
self.dx, self.dy = 0, 0
def stop_motors(self):
self.board.digital_write(self.MOTOR_1_B, 0)
self.board.digital_write(self.MOTOR_1_A, 0)
self.board.digital_write(self.MOTOR_2_B, 0)
self.board.digital_write(self.MOTOR_2_A, 0)
self.board.digital_write(self.MOTOR_3_B, 0)
self.board.digital_write(self.MOTOR_3_A, 0)
def run(self):
while True:
# Reset all direction pins to avoid damaging H-bridges
# TODO: USE dx,dy now in (-1,1)+(None,None) range
self.stop_motors()
dist = abs(self.dx)
if dist > 0.2: #was 2
if self.dx > 0:
print("Turning left")
self.board.digital_write(self.MOTOR_1_B, 1)
self.board.digital_write(self.MOTOR_2_B, 1)
self.board.digital_write(self.MOTOR_3_B, 1)
else:
print("Turning right")
self.board.digital_write(self.MOTOR_1_A, 1)
self.board.digital_write(self.MOTOR_2_A, 1)
self.board.digital_write(self.MOTOR_3_A, 1)
self.board.analog_write(self.MOTOR_1_PWM, int(dist ** 0.7 + 25))
self.board.analog_write(self.MOTOR_2_PWM, int(dist ** 0.7 + 25))
self.board.analog_write(self.MOTOR_3_PWM, int(dist ** 0.7 + 25))
# elif self.dy > 30:
else:
print("Going forward")
self.board.digital_write(self.MOTOR_1_B, 1)
self.board.digital_write(self.MOTOR_3_A, 1)
self.board.analog_write(self.MOTOR_1_PWM, int(self.dy ** 0.5 )+30)
self.board.analog_write(self.MOTOR_2_PWM, 0)
self.board.analog_write(self.MOTOR_3_PWM, int(self.dy ** 0.5 )+30)
sleep(0.03)
# show the pygame window
pygame.init()
screen = pygame.display.set_mode((400,300))
pygame.display.set_caption("Pygame Example")
# loop around until the user presses escape or Q
looping = True
red = 0
green = 0
blue = 0
while looping:
# fill the screen in the random colour
screen.fill((red, green, blue))
pygame.display.flip()
firmata.analog_write( REDPIN, red )
firmata.analog_write( GREENPIN, green )
firmata.analog_write( BLUEPIN, blue )
# wait for a key to be pressed
key = wait_for_key()
# Test for Xbox Pad Buttons
if key == pygame.K_b:
red=red+8
if red>255:
red=0
if key == pygame.K_a:
green=green+8
if green>255:
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
board.digital_write(MOTOR_3_A, 0)
board.analog_write(MOTOR_1_PWM, 255)
board.analog_write(MOTOR_2_PWM, 255)
board.analog_write(MOTOR_3_PWM, 255)
exit(signal.SIGKILL)
signal.signal(signal.SIGINT, signal_handler)
from math import sin
while True:
speed = int(sin(time()/8) * 255)
print(255-abs(speed))
board.analog_write(MOTOR_1_PWM, 255)
board.digital_write(MOTOR_1_A, 0)
board.digital_write(MOTOR_1_B, 0)
board.digital_write(MOTOR_1_A, speed < 0)
board.digital_write(MOTOR_1_B, speed > 0)
board.analog_write(MOTOR_1_PWM, 255-abs(speed))
board.analog_write(MOTOR_2_PWM, 255)
board.digital_write(MOTOR_2_A, 0)
board.digital_write(MOTOR_2_B, 0)
board.digital_write(MOTOR_2_A, speed < 0)
board.digital_write(MOTOR_2_B, speed > 0)
board.analog_write(MOTOR_2_PWM, 255-abs(speed))
board.analog_write(MOTOR_3_PWM, 255)
class ArduinoFirmata(Board, AnalogInputCapable, PwmOutputCapable):
def __init__(self, port=None):
try:
from PyMata.pymata import PyMata as PyMata # noqa
except ImportError:
msg = 'pingo.arduino.Arduino requires PyMata installed'
raise ImportError(msg)
super(ArduinoFirmata, self).__init__()
self.port = port
self.firmata_client = PyMata(self.port, verbose=VERBOSE)
self.firmata_client.capability_query()
time.sleep(10) # TODO: Find a small and safe value
capability_query_results = self.firmata_client.get_capability_query_results(
)
capability_dict = pin_list_to_board_dict(capability_query_results)
self._add_pins([
DigitalPin(self, location)
for location in capability_dict['digital']
] + [PwmPin(self, location) for location in capability_dict['pwm']] + [
AnalogPin(self, 'A%s' % location, resolution=10)
for location in capability_dict['analog']
])
def cleanup(self):
# self.firmata_client.close() has sys.exit(0)
if hasattr(self, 'PyMata'):
try:
self.firmata_client.transport.close()
except AttributeError:
pass
def __repr__(self):
cls_name = self.__class__.__name__
return '<{cls_name} {self.port!r}>'.format(**locals())
def _set_digital_mode(self, pin, mode):
self.firmata_client.set_pin_mode(pin.location, PIN_MODES[mode],
self.firmata_client.DIGITAL)
def _set_analog_mode(self, pin, mode):
pin_id = int(pin.location[1:])
self.firmata_client.set_pin_mode(pin_id, self.firmata_client.INPUT,
self.firmata_client.ANALOG)
def _set_pwm_mode(self, pin, mode):
pin_id = int(pin.location)
self.firmata_client.set_pin_mode(pin_id, self.firmata_client.PWM,
self.firmata_client.DIGITAL)
def _get_pin_state(self, pin):
_state = self.firmata_client.digital_read(pin.location)
if _state == self.firmata_client.HIGH:
return pingo.HIGH
return pingo.LOW
def _set_pin_state(self, pin, state):
self.firmata_client.digital_write(pin.location, PIN_STATES[state])
def _get_pin_value(self, pin):
pin_id = int(pin.location[1:])
return self.firmata_client.analog_read(pin_id)
def _set_pwm_duty_cycle(self, pin, value):
pin_id = int(pin.location)
firmata_value = int(value * 255)
return self.firmata_client.analog_write(pin_id, firmata_value)
def _set_pwm_frequency(self, pin, value):
raise NotImplementedError
# Set the pin mode for a digital output pin firmata.set_pin_mode(RED_LED, firmata.OUTPUT, firmata.DIGITAL) # Turn on the RED LED firmata.digital_write(RED_LED, 1) # Wait 3 seconds and turn it off time.sleep(3) firmata.digital_write(RED_LED, 0) # Set the white led for pwm operation firmata.set_pin_mode(WHITE_LED, firmata.PWM, firmata.DIGITAL) # Set the white led to full brightness (255) for 1 second firmata.analog_write(WHITE_LED, 255) time.sleep(1) # now set it to half brightness for 1 second firmata.analog_write(WHITE_LED, 128) time.sleep(1) # and finally extinguish it firmata.analog_write(WHITE_LED, 0) # set potentiometer pin as an analog input firmata.set_pin_mode(POTENTIOMETER, firmata.INPUT, firmata.ANALOG) # allow some time for the first data to arrive from the Arduino and be # processed. time.sleep(.2)
FADESPEED = 5
# Create an instance of PyMata.
SERIAL_PORT = "/dev/ttyS0"
firmata = PyMata( SERIAL_PORT, max_wait_time=5 )
# initialize the digital pin as an output.
firmata.set_pin_mode( REDPIN, firmata.PWM, firmata.DIGITAL)
firmata.set_pin_mode( GREENPIN, firmata.PWM, firmata.DIGITAL)
firmata.set_pin_mode( BLUEPIN, firmata.PWM, firmata.DIGITAL)
try:
# run in a loop over and over again forever:
while True:
firmata.analog_write( BLUEPIN, 0 )
for bright in range (0,255,8):
firmata.analog_write( GREENPIN, bright )
firmata.analog_write( REDPIN, bright )
# firmata.analog_write( GREENPIN, 255 )
# firmata.analog_write( BLUEPIN, 255 )
time.sleep(0.1) # wait for a second
except KeyboardInterrupt:
# Catch exception raised by using Ctrl+C to quit
pass
# close the interface down cleanly
firmata.close()
# 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 retranslateUi(self, MainWindow):
global country_name
global weather_font
weather_font = QtGui.QFont("impact")
weather_font.setPointSize(20)
_translate = QtCore.QCoreApplication.translate
MainWindow.setWindowTitle(
_translate("MainWindow", "Weather Information"))
self.degree_label.setText(_translate("MainWindow", "℃"))
self.degree_label.setFont(weather_font)
self.main_frame.hide()
self.resetButtons()
#=====
#Clicked Method
self.list_pushButton.clicked.connect(self.clickList)
self.bangkok_pushButton.clicked.connect(self.clickBangkok)
self.berlin_pushButton.clicked.connect(self.clickBerlin)
self.bogota_pushButton.clicked.connect(self.clickBogota)
self.cairo_pushButton.clicked.connect(self.clickCairo)
self.dublin_pushButton.clicked.connect(self.clickDublin)
self.honolulu_pushButton.clicked.connect(self.clickHonolulu)
self.london_pushButton.clicked.connect(self.clickLondon)
self.madrid_pushButton.clicked.connect(self.clickMadrid)
self.moscow_pushButton.clicked.connect(self.clickMoscow)
self.nairobi_pushButton.clicked.connect(self.clickNairobi)
self.newyork_pushButton.clicked.connect(self.clickNewyork)
self.seoul_pushButton.clicked.connect(self.clickSeoul)
self.sydney_pushButton.clicked.connect(self.clickSydney)
self.taipei_pushButton.clicked.connect(self.clickTaipei)
self.tokyo_pushButton.clicked.connect(self.clickTokyo)
self.vancouver_pushButton.clicked.connect(self.clickVancouver)
#set motor board
global board
try:
board = PyMata('/dev/ttyACM0')
except SerialTimeoutException:
log.exception("error")
global fog_pin
fog_pin = 4
board.set_pin_mode(fog_pin, board.OUTPUT, board.DIGITAL)
board.digital_write(fog_pin, 0)
global wind_dir, wind_speed
wind_dir = 12
wind_speed = 10
board.set_pin_mode(wind_speed, board.PWM, board.DIGITAL)
board.set_pin_mode(wind_dir, board.OUTPUT, board.DIGITAL)
board.digital_write(wind_dir, 0)
board.analog_write(wind_speed, 0)
global rain_dir, rain_speed
rain_dir = 13
rain_speed = 11
board.set_pin_mode(rain_speed, board.PWM, board.DIGITAL)
board.set_pin_mode(rain_dir, board.OUTPUT, board.DIGITAL)
board.digital_write(rain_dir, 1)
board.analog_write(rain_speed, 0)
#set lights
global sand_l, dust_l, thunder_l, day_l, spring_l, summer_l, winter_l
sand_l = 2
dust_l = 3
thunder_l = 5
day_l = 6
spring_l = 7
summer_l = 8
winter_l = 9
sleep(1)
board.set_pin_mode(sand_l, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(dust_l, board.OUTPUT, board.DIGITAL)
sleep(1)
board.set_pin_mode(thunder_l, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(day_l, board.OUTPUT, board.DIGITAL)
sleep(1)
board.set_pin_mode(spring_l, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(summer_l, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(winter_l, board.OUTPUT, board.DIGITAL)
# board.analog_write(wind_speed, 80);
global flag_rain
flag_rain = 0
global flag_thunder
flag_thunder = 0
global flag_clear, flag_clouds, flag_drizzle
flag_clear = 0
flag_clouds = 0
flag_drizzle = 0
self.reset_motor()
class Motors(Thread):
MOTOR_1_PWM = 2
MOTOR_1_A = 3
MOTOR_1_B = 4
MOTOR_2_PWM = 5
MOTOR_2_A = 6
MOTOR_2_B = 7
MOTOR_3_PWM = 8
MOTOR_3_A = 9
MOTOR_3_B = 10
def __init__(self):
Thread.__init__(self)
self.daemon = True
self.board = PyMata()
def signal_handler(sig, frame):
self.stop_motors()
self.board.reset()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
self.board.set_pin_mode(self.MOTOR_1_PWM, self.board.PWM,
self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_1_A, self.board.OUTPUT,
self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_1_B, self.board.OUTPUT,
self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_2_PWM, self.board.PWM,
self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_2_A, self.board.OUTPUT,
self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_2_B, self.board.OUTPUT,
self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_3_PWM, self.board.PWM,
self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_3_A, self.board.OUTPUT,
self.board.DIGITAL)
self.board.set_pin_mode(self.MOTOR_3_B, self.board.OUTPUT,
self.board.DIGITAL)
self.dx, self.dy = 0, 0
def stop_motors(self):
self.board.digital_write(self.MOTOR_1_B, 0)
self.board.digital_write(self.MOTOR_1_A, 0)
self.board.digital_write(self.MOTOR_2_B, 0)
self.board.digital_write(self.MOTOR_2_A, 0)
self.board.digital_write(self.MOTOR_3_B, 0)
self.board.digital_write(self.MOTOR_3_A, 0)
def run(self):
while True:
# Reset all direction pins to avoid damaging H-bridges
# TODO: USE dx,dy now in (-1,1)+(None,None) range
self.stop_motors()
dist = abs(self.dx)
if dist > 0.2: #was 2
if self.dx > 0:
print("Turning left")
self.board.digital_write(self.MOTOR_1_B, 1)
self.board.digital_write(self.MOTOR_2_B, 1)
self.board.digital_write(self.MOTOR_3_B, 1)
else:
print("Turning right")
self.board.digital_write(self.MOTOR_1_A, 1)
self.board.digital_write(self.MOTOR_2_A, 1)
self.board.digital_write(self.MOTOR_3_A, 1)
self.board.analog_write(self.MOTOR_1_PWM,
int(dist**0.7 + 25))
self.board.analog_write(self.MOTOR_2_PWM,
int(dist**0.7 + 25))
self.board.analog_write(self.MOTOR_3_PWM,
int(dist**0.7 + 25))
# elif self.dy > 30:
else:
print("Going forward")
self.board.digital_write(self.MOTOR_1_B, 1)
self.board.digital_write(self.MOTOR_3_A, 1)
self.board.analog_write(self.MOTOR_1_PWM,
int(self.dy**0.5) + 30)
self.board.analog_write(self.MOTOR_2_PWM, 0)
self.board.analog_write(self.MOTOR_3_PWM,
int(self.dy**0.5) + 30)
sleep(0.03)
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()
