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()
def setupLED():
global PWM_pin
global board
PWM_pin = 5
# use pin D5 as pwm to adjest LED.
board = PyMata('/dev/ttyS0', verbose = True)
board.set_pin_mode(PWM_pin, board.PWM, board.DIGITAL)
print "Start PWM test by pin D5"
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))
class blech_board:
def __init__(self, port = '/dev/ttyACM0'):
#Warn experimenter that ports are going to switch on, before they are turned off again after initialization
raw_input('WARNING: Switch off your instruments, all ports are going to turn on. Press ANY KEY to continue')
self.board = PyMata(port)
#Look at pymata.py and pymata_command_handler.py if you want to follow the logic. PyMata instances have a _command_handler instance in them - this _command_handler instance has two variables - total_pins_discovered and number_of_analog_pins_discovered. This sets digital pins 0 to (total_pins_discovered - number_of analog_pins_discovered - 1) to 0
dio = self.board._command_handler.total_pins_discovered - self.board._command_handler.number_of_analog_pins_discovered
for i in range(dio):
self.board.digital_write(i, 0)
print 'It is safe to turn your instruments on now. All digital ports have been switched off'
def calibrate(self, port, on_time, repeats, iti):
for i in range(repeats):
sleep(iti)
start = time()*1000.0
self.board.digital_write(port, 1)
while True:
if (time()*1000.0 - start) >= on_time*1000.0:
self.board.digital_write(port, 0)
break
def passive_deliveries_constant(self, ports = [0, 1, 2, 3], iti = 15.0, deliveries_per_channel = 30, on_time = [0.02,0.02,0.02,0.02]):
num_tastes = len(ports)
total_deliveries = num_tastes*deliveries_per_channel
self.delivery_arr = []
for i in range(total_deliveries):
self.delivery_arr.append(i%num_tastes)
shuffle(self.delivery_arr)
for i in self.delivery_arr:
sleep(iti)
start = time()*1000.0
self.board.digital_write(ports[i], 1)
while True:
if (time()*1000.0 - start) >= on_time[i]*1000.0:
self.board.digital_write(ports[i], 0)
break
def clear_tastes(self, ports = [2,3,4,5], open_time = 200.0):
for i in ports:
self.board.digital_write(i, 1)
sleep(open_time)
for i in ports:
self.board.digital_write(port, 0)
def np_no_switch(self, poke_ports = [8, 9], taste_ports = [2, 3], deliveries_per_channel = 50, iti = 5.0, on_time = [0.02, 0.02]):
for i in poke_ports:
self.board.set_pin_mode(i, self.board.INPUT, self.board.DIGITAL)
sleep(2.0)
total_deliveries = len(poke_ports)*deliveries_per_channel
delivery_counter = 0
while (delivery_counter<total_deliveries):
sleep(iti)
for i in range(len(poke_ports)):
if self.board.digital_read(poke_ports[i]) == 0:
start = time()*1000.0
self.board.digital_write(taste_ports[i], 1)
while True:
if (time()*1000.0 - start) >= on_time[i]*1000.0:
self.board.digital_write(taste_ports[i], 0)
break
print 'Trial ', delivery_counter+1,' of ', total_deliveries, ' done'
delivery_counter += 1
print 'All done'
def np_switching(self, poke_ports = [8, 9], taste_ports = [2, 3], deliveries_per_channel = 30, iti = 15.0, switching_every = 1, on_time = [0.02, 0.02]):
for i in poke_ports:
self.board.set_pin_mode(i, self.board.INPUT, self.board.DIGITAL)
sleep(2.0)
total_deliveries = len(poke_ports)*deliveries_per_channel
self.poke_array = []
for i in range(total_deliveries):
self.poke_array.append((i/switching_every)%len(poke_ports))
delivery_counter = 0
for i in self.poke_array:
sleep(iti)
while True:
if self.board.digital_read(poke_ports[i]) == 0:
start = time()*1000.0
self.board.digital_write(taste_ports[i], 1)
while True:
if (time()*1000.0 - start) >= on_time[i]*1000.0:
self.board.digital_write(taste_ports[i], 0)
break
print 'Trial ', delivery_counter+1,' of ', total_deliveries, ' done'
delivery_counter += 1
break
print 'All done'
# Create a PyMata instance
board = PyMata("/dev/ttyUSB0", 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)
# Set digital pin 13 to be an output port
board.set_pin_mode(BOARD_LED, board.OUTPUT, board.DIGITAL)
time.sleep(2)
print("Blinking LED on pin 13 for 10 times ...")
# Blink for 10 times
for x in range(10):
print(x + 1)
# Set the output to 1 = High
board.digital_write(BOARD_LED, 1)
# Wait a half second between toggles.
time.sleep(.5)
# Set the output to 0 = Low
board.digital_write(BOARD_LED, 0)
time.sleep(.5)
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")
print(board.get_analog_response_table())
# pymata_digitalOut.py # PyMata digital out example from PyMata.pymata import PyMata import time # Pin no. pinNo = 8 # connection port PORT = '/dev/ttyACM0' # Create PyMata instance board = PyMata(PORT, verbose=True) # Set digital pin 8 to be an output port board.set_pin_mode(pinNo,board.OUTPUT,board.DIGITAL) # Blink for 10 times for x in range(10): board.digital_write(pinNo,1) time.sleep(.5) board.digital_write(pinNo,0) time.sleep(.5) # Close PyMata board.close()
GEARLAMP = 8
BRAKELAMP = 10
LAMPLAMP = 11
# signal handler function called when Control-C occurs
def signal_handler(signal, frame):
print('You pressed Ctrl+C!!!!')
if board != None:
board.reset()
sys.exit(0)
# listen for SIGINT
signal.signal(signal.SIGINT, signal_handler)
board.set_pin_mode(SPEEDOMETER, board.PWM, board.DIGITAL)
board.set_pin_mode(FUELGAUGE, board.PWM, board.DIGITAL)
board.set_pin_mode(GFORCEMETER, board.PWM, board.DIGITAL)
board.set_pin_mode(SASLAMP, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(GEARLAMP, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(BRAKELAMP, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(LAMPLAMP, board.OUTPUT, board.DIGITAL)
conn = krpc.connect(name="Device")
vessel = conn.space_center.active_vessel
throttle = conn.add_stream(getattr, vessel.control, 'throttle')
refframe = vessel.orbit.body.reference_frame
# Create a PyMata instance
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)
#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:
This
See the README.md file for instructions on compiling
"""
# import the API class
import time
from PyMata.pymata import PyMata
# instantiate PyMata
firmata = PyMata("/dev/ttyACM0")
time.sleep(2)
# configure a group of pins to control 8 LEDs
firmata.set_pin_mode(46,firmata.OUTPUT,firmata.DIGITAL)
firmata.set_pin_mode(47,firmata.OUTPUT,firmata.DIGITAL)
firmata.set_pin_mode(48,firmata.OUTPUT,firmata.DIGITAL)
firmata.set_pin_mode(49,firmata.OUTPUT,firmata.DIGITAL)
firmata.set_pin_mode(50,firmata.OUTPUT,firmata.DIGITAL)
firmata.set_pin_mode(51,firmata.OUTPUT,firmata.DIGITAL)
firmata.set_pin_mode(52,firmata.OUTPUT,firmata.DIGITAL)
firmata.set_pin_mode(53,firmata.OUTPUT,firmata.DIGITAL)
# configure 4 pins for 4 SONAR modules
firmata.sonar_config(6,6)
time.sleep(.1)
firmata.sonar_config(7,7)
time.sleep(.1)
firmata.sonar_config(37,37)
time.sleep(.1)
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)
"Analog Data: Pin Mode - ",
data[PIN_MODE],
" Pin Number -",
data[PIN_NUMBER],
" Data Value - ",
data[DATA_VALUE],
)
def cb_push_button(data):
print(
"Digital Data: Pin Mode - ",
data[PIN_MODE],
" Pin Number -",
data[PIN_NUMBER],
" Data Value - ",
data[DATA_VALUE],
)
signal.signal(signal.SIGINT, signal_handler) # Instantiate PyMata
board = PyMata("/dev/ttyACM1")
# set up pin modes for both pins with callbacks for each
board.set_pin_mode(PUSH_BUTTON, board.INPUT, board.DIGITAL, cb_push_button)
board.set_pin_mode(
POTENTIOMETER, board.INPUT, board.ANALOG, cb_potentiometer
) # A forever loop until user presses Control=c
while 1:
pass
# Re-arm the latch to fire on the next transition to high
board.set_digital_latch(PUSH_BUTTON, board.DIGITAL_LATCH_HIGH,
cb_push_button)
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", verbose=True)
# Set pin modes
# Set the pin to digital output to light the green LED
board.set_pin_mode(GREEN_LED, board.OUTPUT, board.DIGITAL)
# Set the pin to digital input to receive button presses
board.set_pin_mode(PUSH_BUTTON, board.INPUT, board.DIGITAL)
# Arm the digital latch to detect when the button is pressed
board.set_digital_latch(PUSH_BUTTON, board.DIGITAL_LATCH_HIGH, cb_push_button)
# A forever loop until user presses Ctrl+C
while 1:
pass
PIN_NUMBER = 10
DATA_VALUE = 2
# Control-C signal handler to suppress exceptions if user presses Crtl+C
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)
def cb_push_button(data):
print("Digital Data:",
" Pin: ", data[PIN_NUMBER],
" Pin Mode: ", data[PIN_MODE],
" Data Value: ", data[DATA_VALUE])
# Instantiate PyMata
board = PyMata("/dev/ttyACM0", verbose=True)
# Set up pin modes for both pins with callbacks for each
board.set_pin_mode(PUSH_BUTTON, board.INPUT, board.DIGITAL, cb_push_button)
# A forever loop until user presses Ctrl+C
while 1:
pass
# 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
# run simultaneously.
class ArduinoBoard(object):
"""Representation of an Arduino board."""
def __init__(self, port):
"""Initialize the board."""
from PyMata.pymata import PyMata
self._port = port
self._board = PyMata(self._port, verbose=False)
def set_mode(self, pin, direction, mode):
"""Set 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.INPUT,
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):
"""Set a given digital pin to high."""
self._board.digital_write(pin, 1)
def set_digital_out_low(self, pin):
"""Set a given digital pin to low."""
self._board.digital_write(pin, 0)
def get_digital_in(self, pin):
"""Get the value from a given digital pin."""
self._board.digital_read(pin)
def get_analog_in(self, pin):
"""Get 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):
"""Disconnect the board and close the serial connection."""
self._board.reset()
self._board.close()
import textwrap
import subprocess
import struct
from PyMata.pymata import PyMata
# Give the pins names:
REDPIN = 5
GREENPIN = 3
BLUEPIN = 6
# 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)
api_key=''
api_secret=''
access_token_key=''
access_token_secret=''
class DisplayLoop(StreamListener):
"""
This class is a listener for tweet stream data.
It's also callable so it can run the main display
thread loop to update the display.
"""
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")
print(board.get_analog_response_table())
board.analog_write(6, i)
for i in range(6, 10):
board.analog_write(5, i)
board.analog_write(6, i)
def left_handler(left):
for i in range(1, 10):
board.analog_write(5, i)
board.analog_write(6, i)
for i in range(1, 10):
board.analog_write(5, i)
board.analog_write(6, i)
def right_handler(right):
for i in range(2, 1):
board.analog_write(5, i)
board.analog_write(6, i)
for i in range(2, 1):
board.analog_write(5, i)
board.analog_write(6, i)
button_right.clicked.connect(right_handler)
button_left.clicked.connect(left_handler)
button_distance.clicked.connect(distance_handler)
button_exit.clicked.connect(exit_handler)
button_dancing.clicked.connect(dancing_handler)
button_go.clicked.connect(go_handler)
signal.signal(signal.SIGINT, exit_handler)
board = PyMata()
board.set_pin_mode(5, board.PWM, board.DIGITAL)
board.set_pin_mode(6, board.PWM, board.DIGITAL)
board.set_pin_mode(1, board.INPUT, board.ANALOG)
sys.exit(app.exec_()) # This is basically infinite loop here, it blocks everything else!
# Create a PyMata instance
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)
# Set digital pin 13 to be an output port
board.set_pin_mode(BOARD_LED, board.OUTPUT, board.DIGITAL)
time.sleep(2)
print("Blinking LED on pin 13 for 10 times ...")
# Blink for 10 times
for x in range(10):
print((x + 1))
# Set the output to 1 = High
board.digital_write(BOARD_LED, 1)
# Wait a half second between toggles.
time.sleep(.5)
# Set the output to 0 = Low
board.digital_write(BOARD_LED, 0)
time.sleep(.5)
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.running = True
self.board = PyMata()
def signal_handler(sig, frame):
self.running = False
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 run(self):
while self.running:
# Reset all direction pins to avoid damaging H-bridges
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)
dist = abs(self.dx)
if dist > 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.5 + 25))
self.board.analog_write(self.MOTOR_2_PWM, int(dist**0.5 + 25))
self.board.analog_write(self.MOTOR_3_PWM, int(dist**0.5 + 25))
elif self.dy > 30:
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)
# noinspection PyUnusedLocal
def signal_handler(sig, frame):
print('You pressed Ctrl+C!!!!')
if board is not None:
board.reset()
sys.exit(0)
# data change callback functions
def cb_potentiometer(data):
print("Analog Data: Pin Mode - ", data[PIN_MODE], " Pin Number -",
data[PIN_NUMBER], " Data Value - ", data[DATA_VALUE])
def cb_push_button(data):
print("Digital Data: Pin Mode - ", data[PIN_MODE], " Pin Number -",
data[PIN_NUMBER], " Data Value - ", data[DATA_VALUE])
signal.signal(signal.SIGINT, signal_handler) # Instantiate PyMata
board = PyMata("/dev/ttyACM1")
# set up pin modes for both pins with callbacks for each
board.set_pin_mode(PUSH_BUTTON, board.INPUT, board.DIGITAL, cb_push_button)
board.set_pin_mode(
POTENTIOMETER, board.INPUT, board.ANALOG,
cb_potentiometer) # A forever loop until user presses Control=c
while 1:
pass
trials = 5 # Number of trials tasteport1 = 2 # Identify output ports tasteport2 = 3 pokeport1 = 8 # Identify input ports pokeport2 = 9 tasteduration1 = 2 # Time valve should be open in seconds tasteduration2 = 2 iti = 5 # Set ITI # Set mode on input channels and clear outputs board.set_pin_mode(pokeport1, board.INPUT, board.DIGITAL) board.set_pin_mode(pokeport2, board.INPUT, board.DIGITAL) #board.digital_write(tasteport1, 0) #board.digital_write(tasteport2, 0) trial = 1 while trial < trials: if board.digital_read(pokeport1) == 1: board.digital_write(tasteport1, 1) sleep(tasteduration1) board.digital_write(tasteport1, 0) print 'Completed trial number', trial sleep(iti) trial = trial+1 elif board.digital_read(pokeport2) == 1:
SERVO1 = 9
SERVO2 = 10
deg_min = 1
deg_max = 180
deg_mid = 90
deg_step = 10
# ARDUINO PORT
firmata = PyMata("/dev/ttyACM0")
firmata.refresh_report_firmware()
print firmata.get_firmata_firmware_version()
firmata.set_pin_mode(BOARD_LED, firmata.OUTPUT, firmata.DIGITAL)
#RPM dependant from voltage without load
#9.0V = 120 RPM
#7.5V = 80 RPM
# with load 60-80 rpm is a good average
MAX_RPM=80.0
RPS=MAX_RPM/60.0
MPS=RPS*WHEEL_PERIMETER
PWRDIV=500*RPS
rospy.loginfo("PWRDIV:"+str(PWRDIV))
FWDIR=-1
class Mqtt_Pymata:
def __init__(self, name, broker, port, keepalive, state_topic, command_topic, qos, retain,
payload_on, payload_off, optimistic, value_template,serial_port,switch_pin):
self._switch_pin = switch_pin
self._board = PyMata(serial_port,False,verbose=True)
self._board.set_pin_mode(self._switch_pin,self._board.OUTPUT,self._board.DIGITAL)
A0 = 7
def pin_callback(data):
print("Analog Data: ",
" Pin: ", data[PIN_NUMBER],
" Pin Mode: ", data[PIN_MODE],
" Data Value: ", data[DATA_VALUE])
self._board.set_analog_latch(A0,self._board.ANALOG_LATCH_GT,500,pin_callback)
# self._board.set_pin_mode(A0, self._board.INPUT, self._board.ANALOG)
# self._board.set_sampling_interval(100)
# self._board.set_analog_latch(A0,self._board.ANALOG_LATCH_GT,500,pin_callback)
# while True:
# time.sleep(0.1)
# value = self._board.analog_read(A0)
# print(value)
self._mqtt = mqtt.Client()
self._switch = None
self._state_topic = state_topic
self._command_topic = command_topic
self._qos = qos
self._retain = retain
self._payload_on = payload_on
self._payload_off = payload_off
self._optimistic = optimistic
self._state = False
def on_message( _mqtt, userdata, msg):
topic = msg.topic,
qos = msg.qos,
payload = msg.payload.decode('utf-8'),
payload = payload[0]
print(msg.topic+" "+payload)
if payload == payload_on:
self.turn_on()
elif payload == payload_off:
self.turn_off()
self._mqtt.on_message = on_message
self._mqtt.connect(broker, port, keepalive)
self._mqtt.subscribe(command_topic,qos)
self._mqtt.loop_forever()
def turn_on(self):
self._state = True
self.apply_state()
self.report_state()
def turn_off(self):
self._state = False
self.apply_state()
self.report_state()
def apply_state(self):
self._board.digital_write(self._switch_pin,self._board.HIGH if self._state else self._board.LOW)
def report_state(self):
self._mqtt.publish( self._state_topic, self._payload_on if self._state else self._payload_off,
self._qos, self._retain)
# 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)
# 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)
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
# some constants POTENTIOMETER = 2 # this A2, an analog input BUTTON_SWITCH = 12 # a digital input to read a push button switch # Instantiate PyMata - in this case using the default '/dev/ttyACM0' value. firmata = PyMata() # Refresh, Retrieve and print Arduino Firmware version information firmata.refresh_report_firmware() print firmata.get_firmata_version() # Print PyMata's version number print firmata.get_pymata_version() # Setup pin A2 for input firmata.set_pin_mode(POTENTIOMETER, firmata.INPUT, firmata.ANALOG) # Setup pin pin 12 for the switch firmata.set_pin_mode(BUTTON_SWITCH, firmata.INPUT, firmata.DIGITAL) # Arm pin A2 for latching a value >= 678 firmata.set_analog_latch(POTENTIOMETER, firmata.ANALOG_LATCH_GTE, 678) # Arm pin 12 for latching when the pin goes high firmata.set_digital_latch(BUTTON_SWITCH, firmata.DIGITAL_LATCH_HIGH) print "start waiting" # wait for 5 seconds to allow user interaction with switch and pot # during this time press and release the switch and turn the pot to maximum time.sleep(5)
time.gmtime(data[LATCH_TIME_STAMP]))))
# shut things down
board.close()
# create a PyMata instance
board = PyMata("/dev/ttyACM0", True, False)
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)
# 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, cb_push_button)
board.set_pin_mode(POTENTIOMETER, board.INPUT, board.ANALOG, cb_potentiometer)
# set the latch
board.set_analog_latch(POTENTIOMETER, board.ANALOG_LATCH_GTE, 1000, cb_potentiometer_latch)
# do nothing loop - program exits when latch data event occurs for potentiometer or timer expires
time.sleep(15)
print('Timer expired')
board.close()
# create a PyMata instance
firmata = PyMata("/dev/ttyACM0")
def signal_handler(sig, frame):
print("You pressed Ctrl+C!!!!")
if firmata is not None:
firmata.reset()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
# 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)
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 an instance of PyMata.
# The PyMata constructor will print status to the console and will return
# when PyMata is ready to accept commands or will exit if unsuccessful
firmata = PyMata("/dev/ttyACM0")
# Retrieve and print Arduino Firmware version information
firmata.refresh_report_firmware()
print firmata.get_firmata_firmware_version()
# 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)
# some constants POTENTIOMETER = 2 # this A2, an analog input BUTTON_SWITCH = 12 # a digital input to read a push button switch # Instantiate PyMata - in this case using the default '/dev/ttyACM0' value. firmata = PyMata() # Refresh, Retrieve and print Arduino Firmware version information firmata.refresh_report_firmware() print firmata.get_firmata_version() # Print PyMata's version number print firmata.get_pymata_version() # Setup pin A2 for input firmata.set_pin_mode(POTENTIOMETER, firmata.INPUT, firmata.ANALOG) # Setup pin pin 12 for the switch firmata.set_pin_mode(BUTTON_SWITCH, firmata.INPUT, firmata.DIGITAL) # Arm pin A2 for latching a value >= 678 firmata.set_analog_latch(POTENTIOMETER, firmata.ANALOG_LATCH_GTE, 678) # Arm pin 12 for latching when the pin goes high firmata.set_digital_latch(BUTTON_SWITCH, firmata.DIGITAL_LATCH_HIGH) print "start waiting" # wait for 5 seconds to allow user interaction with switch and pot # during this time press and release the switch and turn the pot to maximum time.sleep(5)
# Create a PyMata instance
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)
# 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, cb_push_button)
board.set_pin_mode(POTENTIOMETER, board.INPUT, board.ANALOG, cb_potentiometer)
# Set the latch
board.set_analog_latch(POTENTIOMETER, board.ANALOG_LATCH_GTE, 1000,
cb_potentiometer_latch)
# Do nothing loop - program exits when latch data event occurs for
# potentiometer or timer expires
time.sleep(15)
print('Timer expired')
board.close()
# re-arm the latch to fire on the next transition to high
board.set_digital_latch(PUSH_BUTTON, board.DIGITAL_LATCH_HIGH, cb_push_button)
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 pin modes
# set the pin to light the green led
board.set_pin_mode(GREEN_LED, board.OUTPUT, board.DIGITAL)
# set the pin to receive button presses
board.set_pin_mode(PUSH_BUTTON, board.INPUT, board.DIGITAL)
# arm the digital latch to detect when the button is pressed
board.set_digital_latch(PUSH_BUTTON, board.DIGITAL_LATCH_HIGH, cb_push_button)
while 1:
pass
This
See the README.md file for instructions on compiling
"""
# import the API class
import time
from PyMata.pymata import PyMata
# instantiate PyMata
firmata = PyMata("/dev/ttyACM0")
time.sleep(2)
# configure a group of pins to control 8 LEDs
firmata.set_pin_mode(46, firmata.OUTPUT, firmata.DIGITAL)
firmata.set_pin_mode(47, firmata.OUTPUT, firmata.DIGITAL)
firmata.set_pin_mode(48, firmata.OUTPUT, firmata.DIGITAL)
firmata.set_pin_mode(49, firmata.OUTPUT, firmata.DIGITAL)
firmata.set_pin_mode(50, firmata.OUTPUT, firmata.DIGITAL)
firmata.set_pin_mode(51, firmata.OUTPUT, firmata.DIGITAL)
firmata.set_pin_mode(52, firmata.OUTPUT, firmata.DIGITAL)
firmata.set_pin_mode(53, firmata.OUTPUT, firmata.DIGITAL)
# configure 4 pins for 4 SONAR modules
firmata.sonar_config(6, 6)
time.sleep(.1)
firmata.sonar_config(7, 7)
time.sleep(.1)
firmata.sonar_config(37, 37)
time.sleep(.1)
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 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
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 signal_handler(sig, frame):
board.reset()
# Here we initialize the motor pins on Arduino
board = PyMata(bluetooth=False)
signal.signal(signal.SIGINT, signal_handler)
board.set_pin_mode(MOTOR_1_PWM, board.PWM, board.DIGITAL)
board.set_pin_mode(MOTOR_1_A, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(MOTOR_1_B, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(MOTOR_2_PWM, board.PWM, board.DIGITAL)
board.set_pin_mode(MOTOR_2_A, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(MOTOR_2_B, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(MOTOR_3_PWM, board.PWM, board.DIGITAL)
board.set_pin_mode(MOTOR_3_A, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(MOTOR_3_B, board.OUTPUT, board.DIGITAL)
class MotorThread(Thread):
def __init__(self):
Thread.__init__(self)
self.daemon = True
self.lock = Event()
self.set(0, 0, 0)
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)
signal.signal(signal.SIGINT, signal_handler)
# Data change callback functions
def cb_potentiometer(data):
print("Analog Data: ",
" Pin: ", data[PIN_NUMBER],
" Pin Mode: ", data[PIN_MODE],
" Data Value: ", data[DATA_VALUE])
def cb_push_button(data):
print("Digital Data:",
" Pin: ", data[PIN_NUMBER],
" Pin Mode: ", data[PIN_MODE],
" Data Value: ", data[DATA_VALUE])
# Instantiate PyMata
board = PyMata("/dev/ttyACM0", verbose=True)
# Set up pin modes for both pins with callbacks for each
board.set_pin_mode(PUSH_BUTTON, board.INPUT, board.DIGITAL, cb_push_button)
board.set_pin_mode(POTENTIOMETER, board.INPUT, board.ANALOG,
cb_potentiometer)
# A forever loop until user presses Ctrl+C
while 1:
pass
print('You pressed Ctrl+C')
if board is not None:
board.reset()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
# Data change callback functions
def cb_potentiometer(data):
print(("Analog Data: ", " Pin: ", data[PIN_NUMBER], " Pin Mode: ",
data[PIN_MODE], " Data Value: ", data[DATA_VALUE]))
def cb_push_button(data):
print(("Digital Data:", " Pin: ", data[PIN_NUMBER], " Pin Mode: ",
data[PIN_MODE], " Data Value: ", data[DATA_VALUE]))
# Instantiate PyMata
board = PyMata("/dev/ttyACM0", verbose=True)
# Set up pin modes for both pins with callbacks for each
board.set_pin_mode(PUSH_BUTTON, board.INPUT, board.DIGITAL, cb_push_button)
board.set_pin_mode(POTENTIOMETER, board.INPUT, board.ANALOG, cb_potentiometer)
# A forever loop until user presses Ctrl+C
while 1:
pass
