class InfraredNode():
def __init__(self):
rospy.init_node(INFRARED_NODE, anonymous=True)
self.board = PyMata("/dev/ttyS0", verbose=True)
self.__infrared_pub = rospy.Publisher(INFRARED_TOPIC,
Infrared,
queue_size=10)
def __signal_handler(self, sig, frame):
if self.board is not None:
self.board.reset()
sys.exit(0)
def publish_infrared(self):
signal.signal(signal.SIGINT, self.__signal_handler)
self.board.set_pin_mode(RIGHT_INFRARED_ANALOG_PIN, self.board.INPUT,
self.board.ANALOG)
self.board.set_pin_mode(CENTER_INFRARED_ANALOG_PIN, self.board.INPUT,
self.board.ANALOG)
self.board.set_pin_mode(LEFT_INFRARED_ANALOG_PIN, self.board.INPUT,
self.board.ANALOG)
r = rospy.Rate(2)
while not rospy.is_shutdown():
right_value = self.board.analog_read(RIGHT_INFRARED_ANALOG_PIN)
center_value = self.board.analog_read(CENTER_INFRARED_ANALOG_PIN)
left_value = self.board.analog_read(LEFT_INFRARED_ANALOG_PIN)
infrared = Infrared()
infrared.right_value = right_value
infrared.center_value = center_value
infrared.left_value = left_value
self.__infrared_pub.publish(infrared)
r.sleep()
class ArduinoBoard(object):
""" Represents an Arduino board. """
def __init__(self, port):
from PyMata.pymata import PyMata
self._port = port
self._board = PyMata(self._port, verbose=False)
def set_mode(self, pin, direction, mode):
""" Sets the mode and the direction of a given pin. """
if mode == 'analog' and direction == 'in':
self._board.set_pin_mode(pin,
self._board.INPUT,
self._board.ANALOG)
elif mode == 'analog' and direction == 'out':
self._board.set_pin_mode(pin,
self._board.OUTPUT,
self._board.ANALOG)
elif mode == 'digital' and direction == 'in':
self._board.set_pin_mode(pin,
self._board.OUTPUT,
self._board.DIGITAL)
elif mode == 'digital' and direction == 'out':
self._board.set_pin_mode(pin,
self._board.OUTPUT,
self._board.DIGITAL)
elif mode == 'pwm':
self._board.set_pin_mode(pin,
self._board.OUTPUT,
self._board.PWM)
def get_analog_inputs(self):
""" Get the values from the pins. """
self._board.capability_query()
return self._board.get_analog_response_table()
def set_digital_out_high(self, pin):
""" Sets a given digital pin to high. """
self._board.digital_write(pin, 1)
def set_digital_out_low(self, pin):
""" Sets a given digital pin to low. """
self._board.digital_write(pin, 0)
def get_digital_in(self, pin):
""" Gets the value from a given digital pin. """
self._board.digital_read(pin)
def get_analog_in(self, pin):
""" Gets the value from a given analog pin. """
self._board.analog_read(pin)
def get_firmata(self):
""" Return the version of the Firmata firmware. """
return self._board.get_firmata_version()
def disconnect(self):
""" Disconnects the board and closes the serial connection. """
self._board.reset()
self._board.close()
class ArduinoBoard(object):
""" Represents an Arduino board. """
def __init__(self, port):
self._port = port
self._board = PyMata(self._port, verbose=False)
def set_mode(self, pin, direction, mode):
""" Sets the mode and the direction of a given pin. """
if mode == 'analog' and direction == 'in':
self._board.set_pin_mode(pin,
self._board.INPUT,
self._board.ANALOG)
elif mode == 'analog' and direction == 'out':
self._board.set_pin_mode(pin,
self._board.OUTPUT,
self._board.ANALOG)
elif mode == 'digital' and direction == 'in':
self._board.set_pin_mode(pin,
self._board.OUTPUT,
self._board.DIGITAL)
elif mode == 'digital' and direction == 'out':
self._board.set_pin_mode(pin,
self._board.OUTPUT,
self._board.DIGITAL)
elif mode == 'pwm':
self._board.set_pin_mode(pin,
self._board.OUTPUT,
self._board.PWM)
def get_analog_inputs(self):
""" Get the values from the pins. """
self._board.capability_query()
return self._board.get_analog_response_table()
def set_digital_out_high(self, pin):
""" Sets a given digital pin to high. """
self._board.digital_write(pin, 1)
def set_digital_out_low(self, pin):
""" Sets a given digital pin to low. """
self._board.digital_write(pin, 0)
def get_digital_in(self, pin):
""" Gets the value from a given digital pin. """
self._board.digital_read(pin)
def get_analog_in(self, pin):
""" Gets the value from a given analog pin. """
self._board.analog_read(pin)
def get_firmata(self):
""" Return the version of the Firmata firmware. """
return self._board.get_firmata_version()
def disconnect(self):
""" Disconnects the board and closes the serial connection. """
self._board.reset()
self._board.close()
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)
class ArduinoFirmata(Board, AnalogInputCapable):
def __init__(self, port=None):
try:
from PyMata.pymata import PyMata
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)
detected_digital_pins = len(
self.firmata_client._command_handler.digital_response_table)
detected_analog_pins = len(
self.firmata_client._command_handler.analog_response_table)
self._add_pins([
DigitalPin(self, location)
for location in range(detected_digital_pins)
] + [
AnalogPin(self, 'A%s' % location, resolution=10)
for location in range(detected_analog_pins)
])
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 _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 _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 _get_pin_value(self, pin):
pin_id = int(pin.location[1:])
return self.firmata_client.analog_read(pin_id)
class smartkitdata(object):
def __init__(self):
self.board = PyMata(ARDUINO_ADDRESS)
self.board.set_pin_mode(1, self.board.INPUT, self.board.ANALOG)
self._data = self.gen_data()
def get_data(self):
return pd.DataFrame(self._data)[-60:]
def update_data(self):
self._data = np.vstack([self._data, self.gen_data()])
def gen_data(self):
return np.array(self.board.analog_read(1))
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)
"distance:",
int(distance),
"state:",
state,
"player:",
player,
]
if player:
vals.extend(["returncode:", player.returncode])
print(*vals, flush=True)
while True:
value = board.analog_read(1)
sleep(0.1)
try:
distance = 6762 / (value - 9) - 4
except:
distance = 9001
lugemid.append(distance)
distance = sum(lugemid) / len(lugemid)
if distance < 0:
distance = -distance
if distance < 40:
if state not in (State.play_normal, State.play_end,
State.play_stopped):
state = State.play_normal
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
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)
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
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)
print firmata.analog_read(POTENTIOMETER)
# set the button switch as a digital input
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)
# set digital pin 13 to be an output port
firmata.set_pin_mode(BOARD_LED, firmata.OUTPUT, firmata.DIGITAL)
firmata.set_pin_mode(BOARD_BAT, firmata.INPUT, firmata.ANALOG)
firmata.set_analog_latch(BOARD_BAT, firmata.ANALOG_LATCH_GTE, 1000)
time.sleep(5)
print("Blinking LED on pin 13")
# blink for 10 times
for x in range(10):
print(x + 1)
firmata.digital_write(BOARD_LED, 1)
# wait a half second between toggles.
time.sleep(0.2)
firmata.digital_write(BOARD_LED, 0)
time.sleep(0.2)
while 1:
count += 1
if count == 10:
print("bye bye")
# firmata.close()
analog = firmata.analog_read(BOARD_BAT)
print("Battery: ")
print(analog)
# close PyMata when we are done
# 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()
if 'Total Number' in output:
pass
else:
print 'No Firmata on /dev/ttyOP_FIRM'
sys.exit(0)
board = PyMata("/dev/ttyOP_FIRM")
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
init()
index = 0
length = len(channel_index) - 1
# A forever loop until user presses Control-C
while 1:
RawValue[index] = board.analog_read(channel[index])
time.sleep(0.100)
index += 1
if index > length:
index = 0
SignalK = '{"updates": [{"$source":"OPserial.ARD.ANA","values":[ '
Erg = ''
for i in channel_index:
Erg += '{"path": "' + SK_name[i] + '","value":' + str(
interpolread(i, RawValue[i])) + '},'
SignalK += Erg[0:-1] + ']}]}\n'
#print SignalK
sock.sendto(SignalK, ('127.0.0.1', 55559))
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)
signal.signal(signal.SIGINT, signal_handler)
#setup pins
for pin in PINS:
board.set_pin_mode(pin, board.INPUT, board.ANALOG)
import time
import sensor
s = sensor.Sensor(
name='arduino_analog',
admin_in='/topic/admin_in',
admin_out='/topic/admin_out',
sensor_spec=["analog", "analog1","analog3","analog4"],
sensors_dir='/home/hans/cortical_one_var/sensors/'
)
while True:
values = []
for pin in PINS:
analog = board.analog_read(pin)
#print analog
values.append(str(analog))
print ';'.join(values)
s.announcement_check()
s.send_payload(values)
s.check_recording(values)
time.sleep(0.5)
# set digital pin 13 to be an output port
firmata.set_pin_mode(BOARD_LED, firmata.OUTPUT, firmata.DIGITAL)
firmata.set_pin_mode(BOARD_BAT, firmata.INPUT, firmata.ANALOG)
firmata.set_analog_latch(BOARD_BAT, firmata.ANALOG_LATCH_GTE, 1000)
time.sleep(5)
print("Blinking LED on pin 13")
# blink for 10 times
for x in range(10):
print(x + 1)
firmata.digital_write(BOARD_LED, 1)
# wait a half second between toggles.
time.sleep(.2)
firmata.digital_write(BOARD_LED, 0)
time.sleep(.2)
while 1:
count += 1
if count == 10:
print('bye bye')
#firmata.close()
analog = firmata.analog_read(BOARD_BAT)
print('Battery: ' )
print(analog)
# close PyMata when we are done
#firmata.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()
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)
print firmata.analog_read(POTENTIOMETER)
# set the button switch as a digital input
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)
from PyMata.pymata import PyMata
analog_out = 0
digital_sw = 6
board = PyMata("/dev/ttyACM0", verbose=True)
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)
# Ustawienie moduĊu Pin A0 analog, D6 On\Off
board.set_pin_mode(analog_out, board.INPUT, board.ANALOG)
board.set_pin_mode(digital_sw, board.INPUT, board.DIGITAL)
board.digital_write(digital_sw, 1)
for x in range(100):
status = board.analog_read(analog_out)
#print(f"{x + 1}, przebieg")
print(status)
sleep(.01)
#board.digital_write(digital_sw, 0)
board.close()
# pymata_analogRead.py
# PyMata analog read example
from PyMata.pymata import PyMata
import time
# Pin no. A0
pinNo = 0
# connection port
PORT = '/dev/ttyACM0'
# Create PyMata instance
board = PyMata(PORT, verbose=True)
# Set digital pin 13 to be an output port
board.set_pin_mode(pinNo, board.INPUT, board.ANALOG)
while True:
pin = board.analog_read(pinNo)
print 'analog read : A0: {}'.format(pin)
time.sleep(0.5)
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
class ArduinoFirmata(Board, AnalogInputCapable):
def __init__(self, port=None):
try:
from PyMata.pymata import PyMata
except ImportError:
msg = 'pingo.arduino.Arduino requires PyMata installed'
raise ImportError(msg)
super(ArduinoFirmata, self).__init__()
self.port = port
self.PyMata = PyMata(self.port)
detected_digital_pins = len(self.PyMata._command_handler.digital_response_table)
detected_analog_pins = len(self.PyMata._command_handler.analog_response_table)
self._add_pins(
[DigitalPin(self, location)
for location in range(detected_digital_pins)] +
[AnalogPin(self, 'A%s' % location, resolution=10)
for location in range(detected_analog_pins)]
)
def cleanup(self):
#self.PyMata.close() has sys.exit(0)
if hasattr(self, 'PyMata'):
try:
self.PyMata.transport.close()
except AttributeError:
pass
def __repr__(self):
cls_name = self.__class__.__name__
return '<{cls_name} {self.port!r}>'.format(**locals())
def _set_pin_mode(self, pin, mode):
self.PyMata.set_pin_mode(
pin.location,
PIN_MODES[mode],
self.PyMata.DIGITAL
)
def _get_pin_state(self, pin):
_state = self.PyMata.digital_read(pin.location)
if _state == self.PyMata.HIGH:
return pingo.HIGH
return pingo.LOW
def _set_pin_state(self, pin, state):
self.PyMata.digital_write(
pin.location,
PIN_STATES[state]
)
def _set_analog_mode(self, pin, mode):
pin_id = int(pin.location[1:])
self.PyMata.set_pin_mode(
pin_id,
self.PyMata.INPUT,
self.PyMata.ANALOG
)
def _get_pin_value(self, pin):
pin_id = int(pin.location[1:])
return self.PyMata.analog_read(pin_id)
def print_vals():
vals = ["distance:", int(distance),
"state:", state,
"player:",player,
]
if player:
vals.extend(["returncode:", player.returncode])
print(*vals, flush=True)
while True:
value = board.analog_read(1)
sleep(0.1)
try:
distance = 6762 / (value -9) -4
except:
distance = 9001
lugemid.append(distance)
distance = sum(lugemid) / len(lugemid)
if distance < 0 :
distance = - distance
if distance < 40:
if state not in (State.play_normal, State.play_end, State.play_stopped):
state = State.play_normal
print_vals()
