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()
class Pin:
"""
阵脚对象
"""
IN = PyMata.INPUT
OUT = PyMata.OUTPUT
PWM = PyMata.PWM
DIGITAL = PyMata.DIGITAL
ANALOG = PyMata.ANALOG
def __init__(self, pin_num, pin_model, pin_type=None, port="/dev/ttyACM0",
debug=False):
"""
:param pin_num: 针脚，已a开头的表示为模拟信号针脚，已d开头的表示为数字信号针脚,编号范围0~19
:param pin_model: 接受INPUT或者OUTPUT类型
:param port: 虚谷连接arduino的COM口，默认为"/dev/ttyACM0"
:param debug: 当为True的时候，会输出debug信息
"""
pin_num, _pin_type = check_pin_num(pin_num)
if not pin_type:
pin_type = _pin_type
self.pin_num = pin_num
self.board = PyMata(port, bluetooth=False, verbose=debug)
self.board.set_pin_mode(self.pin_num, pin_model, pin_type)
def high(self):
"""
输出数字信号高电位值
:return:
"""
self.board.digital_write(self.pin_num, 1)
def on(self):
"""
设置数字信号高电位
:return:
"""
self.board.digital_write(self.pin_num, 1)
def low(self):
"""
输出数字信号低电位值
:return:
"""
self.board.digital_write(self.pin_num, 0)
def off(self):
"""
设置数字信号为低电位值
:return:
"""
self.board.digital_write(self.pin_num, 0)
def value(self):
"""
获取数字信号电位值
:return:
"""
return self.board.digital_read(self.pin_num)
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)
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")
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)
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'
signal.signal(signal.SIGINT, signal_handler)
# Set digital pins
board.set_pin_mode(COLL_RIGHT, board.INPUT, board.DIGITAL)
board.set_pin_mode(COLL_CENTER, board.INPUT, board.DIGITAL)
board.set_pin_mode(COLL_LEFT, board.INPUT, board.DIGITAL)
board.set_pin_mode(TOGGLE_SWITCH, board.OUTPUT, board.DIGITAL)
board.set_pin_mode(SCAN_ENABLE, board.OUTPUT, board.DIGITAL)
time.sleep(2)
print("Start Scanning")
board.digital_write(SCAN_ENABLE, 1)
time.sleep(5)
print("toggle on")
board.digital_write(TOGGLE_SWITCH, 1)
time.sleep(5)
print("toggle off")
board.digital_write(TOGGLE_SWITCH, 0)
time.sleep(5)
while 1:
if (board.digital_read(COLL_RIGHT) == 1):
print "Collision on the right"
elif (board.digital_read(COLL_CENTER) == 1):
print "Collision on center"
elif (board.digital_read(COLL_LEFT) == 1):
print "Collision on the left"
else:
print("no obstacle ahead")
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)
import sys
import signal
from PyMata.pymata import PyMata
ENCODER_A = 2
ENCODER_B = 4
prev_value = 0
# 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)
# configure the pins for the encoder
board.encoder_config(ENCODER_B, ENCODER_A)
while 1:
value = board.digital_read(ENCODER_B)
if value != prev_value:
prev_value = value
print(board.digital_read(ENCODER_B))
pass
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'
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
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)
# control the servo - note that you don't need to set pin mode
# configure the servo
firmata.servo_config(SERVO_MOTOR)
# move the servo to 20 degrees
firmata.analog_write(SERVO_MOTOR, 20)
time.sleep(.5)
class ShotManager():
def __init__(self):
# see the shotlist in app/shots/shots.py
self.currentShot = shots.APP_SHOT_NONE
self.currentModeIndex = DEFAULT_APM_MODE
self.curController = None
def Start(self, vehicle):
logger.log("+-+-+-+-+-+-+ Starting up %s +-+-+-+-+-+-+" % VERSION)
### initialize dronekit vehicle ###
self.vehicle = vehicle
### switch vehicle to loiter mode ###
self.vehicle.mode = VehicleMode("LOITER")
### initialize rc manager ###
self.rcMgr = rcManager.rcManager(self)
### initialize app manager ###
self.appMgr = appManager.appManager(self)
### initialize button manager ###
self.buttonManager = buttonManager.buttonManager(self)
### initialize gopro manager ###
self.goproManager = GoProManager.GoProManager(self)
### Initialize GeoFence manager ###
self.geoFenceManager = GeoFenceManager.GeoFenceManager(self)
# instantiate rewindManager
self.rewindManager = rewindManager.RewindManager(self.vehicle, self)
# Create a PyMata instance and initialize the object avoidance toggles
self.led_left_state = 0
self.led_right_state = 0
self.led_center_state = 0
self.led_beam_angle_state = 0
self.center_collision_state = 0
try:
self.arduinoBoard = PyMata("/dev/ttyACM0", verbose=True)
self.arduinoBoard.set_pin_mode(COLL_RIGHT, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(COLL_CENTER, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(COLL_LEFT, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(SCAN_BIT1, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(SCAN_BIT2, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(SCAN_BIT3, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
self.arduinoBoard.set_pin_mode(SCAN_BIT4, self.arduinoBoard.INPUT, self.arduinoBoard.DIGITAL)
except:
logger.log("[shotmanager]: Error in communication to Arduino")
### init APM stream rates ###
self.initStreamRates()
### register callbacks ###
self.registerCallbacks()
# Try to maintain a constant tick rate
self.timeOfLastTick = monotonic.monotonic()
# how many ticks have we performed since an RC update?
# register all connections (gopro manager communicates via appMgr's socket)
self.inputs = [self.rcMgr.server, self.appMgr.server]
self.outputs = []
#check if gimbal is present
if self.vehicle.gimbal.yaw is not None:
logger.log("[shot]: Gimbal detected.")
# Initialize gimbal to RC targeting
self.vehicle.gimbal.release()
else:
logger.log("[shot]: No gimbal detected.")
# mark first tick time
self.timeOfLastTick = monotonic.monotonic()
# check for In-Air start from Shotmanager crash
if self.vehicle.system_status == 'ACTIVE':
logger.log("[shot]: Restart in air.")
# load vehicle home
self.rewindManager.loadHomeLocation()
# not yet enabled until this check proves effective
#self.vehicle.mode = VehicleMode("RTL")
def Run(self):
while True:
try:
#print "in shotManager server loop"
# handle TCP/RC packets
# we set a timeout of UPDATE_TIME, so we roughly do this every UPDATE_TIME
rl, wl, el = select.select( self.inputs, self.outputs, [], UPDATE_TIME )
# handle reads
for s in rl:
if s is self.appMgr.server: # if read is a connection attempt
self.appMgr.connectClient()
elif s is self.appMgr.client: # if read is from app
self.appMgr.parse()
elif s is self.rcMgr.server: # if read is an RC packet
self.rcMgr.parse()
elif s is self.buttonManager.client: # if read is from buttons
self.buttonManager.parse()
# now handle writes (sololink.btn_msg handles all button writes)
for s in wl:
if s is self.appMgr.client: # if write is for app
self.appMgr.write()
# exceptions
for s in el:
if s is self.appMgr.client: # if its the app socket throwing an exception
self.appMgr.exception()
else:
# otherwise remove whichever socket is excepting
if s in self.inputs:
self.inputs.remove(s)
if s in self.outputs:
self.outputs.remove(s)
s.close()
self.buttonManager.checkButtonConnection()
# Check for obstacles when the lidar is active
if (self.buttonManager.scanactive == 1):
self.checkForObstacle()
# Check if copter is outside fence or will be
self.geoFenceManager.activateGeoFenceIfNecessary()
# call main control/planning loop at UPDATE_RATE
if time.time() - self.timeOfLastTick > UPDATE_TIME:
self.Tick()
except Exception as ex:
# reset any RC timeouts and stop any stick remapping
self.rcMgr.detach()
# try to put vehicle into LOITER
self.vehicle.mode = VehicleMode("LOITER")
exceptStr = traceback.format_exc()
print exceptStr
strlist = exceptStr.split('\n')
for i in strlist:
logger.log(i)
if self.appMgr.isAppConnected():
# send error to app
packet = struct.pack('<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out (for some reason
# setting client.setblocking(1) doesn't work)
time.sleep(0.4)
# cleanup
for socket in self.inputs:
socket.close()
os._exit(1)
def enterShot(self, shot):
if shot not in shots.SHOT_NAMES:
logger.log("[shot]: Shot not recognized. (%d)" % shot)
return
if shot == shots.APP_SHOT_NONE:
pass
# check our EKF - if it's bad and that we can't init the shot prior to EKF being OK, reject shot entry attempt
elif self.last_ekf_ok is False and shot not in shots.CAN_START_BEFORE_EKF:
logger.log('[shot]: Vehicle EKF quality is poor, shot entry into %s disallowed.' % shots.SHOT_NAMES[shot])
# set shot to APP_SHOT_NONE
shot = shots.APP_SHOT_NONE
# notify the app of shot entry failure
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_BAD_EKF)
self.appMgr.sendPacket(packet)
# check vehicle system status - if it's CRITICAL or EMERGENCY, reject shot entry attempt
elif self.vehicle.system_status in ['CRITICAL', 'EMERGENCY']:
logger.log('[shot]: Vehicle in %s, shot entry into %s disallowed.' % (self.vehicle.system_status, shots.SHOT_NAMES[shot]))
# set shot to APP_SHOT_NONE
shot = shots.APP_SHOT_NONE
# notify the app of shot entry failure
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_RTL)
self.appMgr.sendPacket(packet)
# check if vehicle is not armed or in STANDBY and that we can't init the shot prior to arm, reject shot entry attempt
elif (self.vehicle.armed is False or self.vehicle.system_status == 'STANDBY') and shot not in shots.CAN_START_BEFORE_ARMING:
logger.log('[shot]: Vehicle is unarmed, shot entry into %s disallowed.' % shots.SHOT_NAMES[shot])
self.vehicle.mode = VehicleMode("LOITER")
# set shot to APP_SHOT_NONE
shot = shots.APP_SHOT_NONE
# notify the app of shot entry failure
packet = struct.pack('<III', app_packet.SOLO_SHOT_ERROR, 4, app_packet.SHOT_ERROR_UNARMED)
self.appMgr.sendPacket(packet)
# OK fine, you get to start the shot.
if self.currentShot != shot:
logger.log('[shot]: Entering shot %s.' % shots.SHOT_NAMES[shot])
if self.currentShot == shots.APP_SHOT_REWIND:
# we are exiting Rewind
self.rewindManager.resetSpline()
# APP_SHOT_NONE
if shot == shots.APP_SHOT_NONE:
# mark curController for garbage collection
del self.curController
# set curController to None (should also mark for garbage collection)
self.curController = None
# re-enable manual gimbal controls (RC Targeting mode)
self.vehicle.gimbal.release()
# disable the stick re-mapper
self.rcMgr.enableRemapping( False )
# disable scanning
self.arduinoBoard.digital_write(6, 0)
# if the Rewind shot put us into RTL, lets stay there
if self.vehicle.mode.name == 'RTL':
logger.log("[shot]: Leaving vehicle in mode RTL")
# if vehicle mode is in another mode such as GUIDED or AUTO, then switch to LOITER
elif self.vehicle.mode.name in shots.SHOT_MODES:
logger.log("[shot]: Changing vehicle mode to LOITER.")
self.vehicle.mode = VehicleMode("LOITER")
else:
self.curController = ShotFactory.get_shot_obj(shot, self.vehicle, self)
# update currentShot
self.currentShot = shot
logger.log("[shot]: Successfully entered %s." % shots.SHOT_NAMES[shot])
# already in that shot
else:
logger.log('[shot]: Already in shot %s.' % shots.SHOT_NAMES[shot])
# let the world know
if self.appMgr.isAppConnected():
self.appMgr.broadcastShotToApp(shot)
# let Artoo know too
self.buttonManager.setArtooShot(shot)
# set new button mappings appropriately
self.buttonManager.setButtonMappings()
def mode_callback(self, vehicle, name, mode):
try:
if mode.name != self.lastMode:
logger.log("[callback]: Mode changed from %s -> %s"%(self.lastMode, mode.name))
if mode.name == 'RTL':
logger.log("[callback]: System entered RTL, we stay in there!")
# self.enterShot(shots.APP_SHOT_RTL)
elif self.currentShot != shots.APP_SHOT_NONE:
# looks like somebody switched us out of guided! Exit our current shot
if mode.name not in shots.SHOT_MODES:
logger.log("[callback]: Detected that we are not in the correct apm mode for this shot. Exiting shot!")
self.enterShot(shots.APP_SHOT_NONE)
self.lastMode = mode.name
# don't do the following for guided, since we're in a shot
if self.lastMode == 'GUIDED' or mode.name == 'RTL':
return
modeIndex = modes.getAPMModeIndexFromName( self.lastMode, self.vehicle)
if modeIndex < 0:
logger.log("couldn't find this mode index: %s" % self.lastMode)
return
if self.currentShot == shots.APP_SHOT_NONE:
self.buttonManager.setArtooShot( -1, modeIndex )
self.currentModeIndex = modeIndex
except Exception as e:
logger.log('[shot]: mode callback error, %s' % e)
def ekf_callback(self, vehicle, name, ekf_ok):
try:
if ekf_ok != self.last_ekf_ok:
self.last_ekf_ok = ekf_ok
logger.log("[callback]: EKF status changed to %d" % (ekf_ok))
self.buttonManager.setButtonMappings()
# if we regain EKF and are landing - just push us into fly
if ekf_ok and self.vehicle.mode.name == 'LAND':
self.enterShot(shots.APP_SHOT_NONE)
# only store home in the air when no home loc exists
if self.rewindManager:
if ekf_ok and self.last_armed and self.rewindManager.homeLocation is None:
self.rewindManager.loadHomeLocation()
except Exception as e:
logger.log('[callback]: ekf callback error, %s' % e)
def armed_callback(self, vehicle, name, armed):
try:
if armed != self.last_armed:
self.last_armed = armed
logger.log("[callback]: armed status changed to %d"%(armed))
self.buttonManager.setButtonMappings()
# clear Rewind manager cache
self.rewindManager.resetSpline()
if not armed and self.currentShot not in shots.CAN_START_BEFORE_ARMING:
self.enterShot(shots.APP_SHOT_NONE)
self.vehicle.mode = VehicleMode("LOITER")
# stop recording upon disarm (landing, hopefully)
if not armed:
self.goproManager.handleRecordCommand(self.goproManager.captureMode, RECORD_COMMAND_STOP)
# read home loc from vehicle
if armed:
self.rewindManager.loadHomeLocation()
except Exception as e:
logger.log('[callback]: armed callback error, %s' % e)
def camera_feedback_callback(self, vehicle, name, msg):
try:
if self.currentShot in shots.SITE_SCAN_SHOTS or self.vehicle.mode.name == 'AUTO':
# issue GoPro record command
self.goproManager.handleRecordCommand(CAPTURE_MODE_PHOTO, RECORD_COMMAND_START)
except Exception as e:
logger.log('[callback]: camera feedback callback error, %s.' % e)
def initStreamRates(self):
STREAM_RATES = {
mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS: 2,
mavutil.mavlink.MAV_DATA_STREAM_EXTRA1: 10,
mavutil.mavlink.MAV_DATA_STREAM_EXTRA2: 10,
mavutil.mavlink.MAV_DATA_STREAM_EXTRA3: 2,
mavutil.mavlink.MAV_DATA_STREAM_POSITION: 10,
mavutil.mavlink.MAV_DATA_STREAM_RAW_SENSORS: 2,
mavutil.mavlink.MAV_DATA_STREAM_RAW_CONTROLLER: 3,
mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS: 5,
}
for stream, rate in STREAM_RATES.items():
msg = self.vehicle.message_factory.request_data_stream_encode(
0, 1, # target system, target component
stream, # requested stream id
rate, # rate
1 # start it
)
self.vehicle.send_mavlink(msg)
def notifyPause(self, inShot=0):
'''notify the autopilot that we would like to pause'''
if inShot:
return
msg = self.vehicle.message_factory.command_long_encode(
0, # target system
1, # target component
mavutil.mavlink.MAV_CMD_SOLO_BTN_PAUSE_CLICK, # frame
0, # confirmation
int(inShot), # param 1: 1 if Solo is in a shot mode, 0 otherwise
0, 0, 0, 0, 0, 0) # params 2-7 (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
# This fetches and returns the value of the parameter matching the given name
# If the parameter is not found, returns the given default value instead
def getParam(self, name, default=None):
return self.vehicle.parameters.get(name, wait_ready=False) or default
# we call this at our UPDATE_RATE
# drives the shots as well as anything else timing-dependent
def Tick(self):
self.timeOfLastTick = time.time()
self.rcMgr.rcCheck()
# update rewind manager
if (self.currentShot == shots.APP_SHOT_REWIND or self.currentShot == shots.APP_SHOT_RTL or self.vehicle.mode.name == 'RTL') is False:
self.rewindManager.updateLocation()
# Always call remap
channels = self.rcMgr.remap()
if self.curController:
self.curController.handleRCs(channels)
def getHomeLocation(self):
if self.rewindManager.homeLocation is None or self.rewindManager.homeLocation.lat == 0:
return None
else:
return self.rewindManager.homeLocation
def enterFailsafe(self):
''' called when we loose RC link or have Batt FS event '''
# dont enter failsafe on the ground
if not self.vehicle.armed or self.vehicle.system_status != 'ACTIVE':
return
# dont enter failsafe if we are already rewinding home
if self.currentShot == shots.APP_SHOT_REWIND:
self.curController.exitToRTL = True
return
if self.currentShot == shots.APP_SHOT_RTL:
return
if self.last_ekf_ok is False:
# no GPS - force an emergency land
self.vehicle.mode = VehicleMode("LAND")
return
# ignore FS while in Auto mode
if self.vehicle.mode.name == 'AUTO' and self.rewindManager.fs_thr == 2:
return
if self.rewindManager.enabled:
self.enterShot(shots.APP_SHOT_REWIND)
self.curController.exitToRTL = True
#else:
# self.enterShot(shots.APP_SHOT_RTL)
def registerCallbacks(self):
self.last_ekf_ok = False
self.last_armed = False
self.lastMode = self.vehicle.mode.name
# MODE
self.vehicle.add_attribute_listener('mode', self.mode_callback) #register with vehicle class (dronekit)
# EKF
# call ekf back first
self.ekf_callback(self.vehicle, 'ekf_ok', self.vehicle.ekf_ok)
self.vehicle.add_attribute_listener('ekf_ok', self.ekf_callback) #register with vehicle class (dronekit)
# ARMED
self.vehicle.add_attribute_listener('armed', self.armed_callback) #register with vehicle class (dronekit)
# CAMERA FEEDBACK
self.vehicle.add_message_listener('CAMERA_FEEDBACK', self.camera_feedback_callback) #register with vehicle class (dronekit)
# gopro manager callbacks (defined in gopro manager)
self.vehicle.add_attribute_listener('gopro_status', self.goproManager.state_callback)
self.vehicle.add_attribute_listener('gopro_get_response', self.goproManager.get_response_callback)
self.vehicle.add_attribute_listener('gopro_set_response', self.goproManager.set_response_callback)
def checkForObstacle(self):
# check if beam will hit the ground at altitudes lower than 3 meters then disable scan
self.pitch_angle = abs(self.vehicle.attitude.pitch)
# use the lidar rangefinder for exact altitudes otherwise use the baro
if (self.vehicle.rangefinder.distance == None):
self.check_altitude = self.vehicle.location.global_relative_frame.alt
# logger.log("altitude Baro: %s" % self.check_altitude)
else:
self.check_altitude = self.vehicle.rangefinder.distance
# logger.log("altitude rangefinder: %s" % self.check_altitude)
if (self.vehicle.attitude.pitch < 0 and self.check_altitude < 3):
self.beamreach = (self.check_altitude / math.sin(self.pitch_angle))
else:
self.beamreach = 1000
# do the collision check only if above 1 meter and when it is ensured that the lidar beam will not hit the ground
if (self.beamreach > SCAN_BEAM_DISTANCE and self.check_altitude > 1):
if (self.led_beam_angle_state == 1):
self.led_beam_angle_state = 0
self.LEDrgb(2, 2, 0, 255, 0)
self.LEDrgb(3, 2, 255, 0, 0)
if (self.arduinoBoard.digital_read(COLL_RIGHT) == 1):
if (self.led_right_state == 0):
logger.log("[objavoid]: Obstacle on the right")
# send info to app
if self.appMgr.isAppConnected():
exceptStr = "Obstacle to the right"
packet = struct.pack('<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out
time.sleep(0.1)
# LED right_front set to strobe magenta
self.LEDrgb(3, 2, 255, 0, 0)
self.LEDrgb(2, 4, 255, 0, 255)
self.led_right_state = 1
self.led_left_state = 0
self.led_center_state = 0
self.led_beam_angle_state = 0
elif (self.arduinoBoard.digital_read(COLL_CENTER) == 1):
if (self.led_center_state == 0):
logger.log("[objavoid]: Obstacle in center")
# when we are not in a shot and flying higher than 1 meter
if (self.currentShot == shots.APP_SHOT_NONE and (self.vehicle.mode != "RTL" or self.vehicle.mode != "LAND")):
logger.log("[objavoid]: calling notifyPause now")
self.notifyPause(False) #trigger brake
# self.vehicle.mode = VehicleMode("BRAKE")
# send status to app
if self.appMgr.isAppConnected():
exceptStr = "Obstacle ahead in %.1f" % self.coll_distance() + " meters"
packet = struct.pack('<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out
time.sleep(0.1)
# both LED front set to strobe magenta
self.LEDrgb(2, 4, 255, 0, 255)
self.LEDrgb(3, 4, 255, 0, 255)
self.led_right_state = 0
self.led_left_state = 0
self.led_center_state = 1
self.led_beam_angle_state = 0
elif (self.arduinoBoard.digital_read(COLL_LEFT) == 1):
if (self.led_left_state == 0):
logger.log("[objavoid]: Obstacle on the left")
# send status to app
if self.appMgr.isAppConnected():
exceptStr = "Obstacle to the left"
packet = struct.pack('<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out
time.sleep(0.1)
# LED left_front set to strobe magenta
self.LEDrgb(2, 2, 0, 255, 0)
self.LEDrgb(3, 4, 255, 0, 255)
self.led_right_state = 0
self.led_left_state = 1
self.led_center_state = 0
self.led_beam_angle_state = 0
else:
# no obstacle in sight reset everything
if (self.led_left_state == 1 or self.led_right_state == 1 or self.led_center_state == 1):
logger.log("[objavoid]: no obstacle in sight reset LED")
self.LEDrgb(2, 2, 0, 255, 0)
self.LEDrgb(3, 2, 255, 0, 0)
self.led_right_state = 0
self.led_left_state = 0
self.led_center_state = 0
self.led_beam_angle_state = 0
else:
if (self.led_beam_angle_state == 0):
logger.log("[objavoid]: pitch/altitude too low - obstacle detection disabled")
self.LEDrgb(2, 4, 255, 100, 0)
self.LEDrgb(3, 4, 255, 100, 0)
self.led_beam_angle_state = 1
self.led_right_state = 0
self.led_left_state = 0
self.led_center_state = 0
# send status to app
if self.appMgr.isAppConnected():
exceptStr = "Altitude too low for scan"
packet = struct.pack('<II%ds' % (len(exceptStr)), app_packet.SOLO_MESSAGE_SHOTMANAGER_ERROR, len(exceptStr), exceptStr)
self.appMgr.client.send(packet)
# sleep to make sure the packet goes out
time.sleep(0.1)
def coll_distance(self):
x = str(self.arduinoBoard.digital_read(SCAN_BIT4)) + str(self.arduinoBoard.digital_read(SCAN_BIT3)) + str(self.arduinoBoard.digital_read(SCAN_BIT2)) + str(self.arduinoBoard.digital_read(SCAN_BIT1))
y = float(int(x, 2))/2
return y
def LEDpadArray(self, byteArray):
return byteArray + bytearray([0]) * (24 - len(byteArray))
# Set LED pattern and color (RGB value)
def LEDrgb(self, led, pattern, red, green, blue):
#print "rgb", led, pattern, red, green, blue
byteArray = bytearray(['R', 'G', 'B', '0', pattern, red, green, blue])
self.LEDsendMessage(led, 255, byteArray)
# reset LED to default behavior
def LEDreset(self, led):
#print "reset", led
self.LEDsendMessage(led, 0)
def LEDsendMessage(self, led, macro, byteArray = bytearray()):
#print "sendMessage", led, macro, ":".join(str(b) for b in byteArray)
msg = self.vehicle.message_factory.led_control_encode(0, 0, led, macro, len(byteArray), self.LEDpadArray(byteArray))
self.vehicle.send_mavlink(msg)
# Can't find a functional flush() operation, so wait instead
time.sleep(0.1)
# pymata_digitalRead.py
# PyMata digital read example
from PyMata.pymata import PyMata
import time
# Pin no.
pinNo = 2
# 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.DIGITAL)
while True:
pin = board.digital_read(pinNo)
print 'pin state: {}'.format(pin)
time.sleep(0.5)
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)
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
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:
board.digital_write(tasteport2, 1)
sleep(tasteduration2)
board.digital_write(tasteport2, 0)
print 'Completed trial number', trial
sleep(iti)
trial = trial+1
print('It\'s all ogre now') # Shrek is love, shrek is life
import signal
from PyMata.pymata import PyMata
ENCODER_A = 14
ENCODER_B = 15
prev_value = 0
# 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)
# configure the pins for the encoder
board.encoder_config(ENCODER_B, ENCODER_A)
while 1:
value = board.digital_read(ENCODER_B)
if value != prev_value:
prev_value = value
print(board.digital_read(ENCODER_B))
pass
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)
# control the servo - note that you don't need to set pin mode
# configure the servo
firmata.servo_config(SERVO_MOTOR)
# move the servo to 20 degrees
firmata.analog_write(SERVO_MOTOR, 20)
time.sleep(.5)
