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 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):
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()
master = Tk()
master.title("RGB LED")
master.geometry('250x200')
r = Scale(master,
from_=255,
to=0,
command=(rd),
label="Red",
troughcolor="red")
r.pack(side=LEFT)
g = Scale(master,
from_=255,
to=0,
command=(gr),
label="Green",
troughcolor="green")
g.pack(side=RIGHT)
b = Scale(master,
from_=255,
to=0,
command=(bl),
label="Blue",
troughcolor="blue")
b.pack(side=RIGHT)
mainloop()
board.close()
board.reset()
# create a PyMata instance
firmata = PyMata("/dev/ttyACM0")
def signal_handler(sig, frm):
print('You pressed Ctrl+C!!!!')
if firmata is not None:
firmata.reset()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
# send the arduino a firmata reset
firmata.reset()
# configure the stepper to use pins 9.10,11,12 and specify 512 steps per revolution
firmata.stepper_config(512, [12, 11, 10, 9])
# allow time for config to complete
time.sleep(.5)
# ask Arduino to return the stepper library version number to PyMata
firmata.stepper_request_library_version()
# allow time for command and reply to go across the serial link
time.sleep(.5)
print(("Stepper Library Version", ))
print((firmata.get_stepper_version()))
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)
# create a PyMata instance
firmata = PyMata("/dev/ttyACM0")
def signal_handler(sig, frm):
print('You pressed Ctrl+C!!!!')
if firmata is not None:
firmata.reset()
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
# send the arduino a firmata reset
firmata.reset()
# configure the stepper to use pins 9.10,11,12 and specify 512 steps per revolution
firmata.stepper_config(512, [12, 11, 10, 9])
# allow time for config to complete
time.sleep(.5)
# ask Arduino to return the stepper library version number to PyMata
firmata.stepper_request_library_version()
# allow time for command and reply to go across the serial link
time.sleep(.5)
print("Stepper Library Version",)
print(firmata.get_stepper_version())
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 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
