def __init__(self, run_args):
self._log = log.getLogger('controller')
dmxl_intf = dmxl.USB2AX(run_args.intf_dev, debug=run_args.debug)
# check passed servos id
for _id in (run_args.left_id, run_args.right_id):
if not dmxl_intf.ping(dmxlid=_id):
raise ValueError('no servo found with id=%d' % _id)
if i2c and iopi:
i2cbus = i2c.SMBusI2CBus()
io_board = iopi.Board(i2cbus)
io_port_in = iopi.Port(io_board, iopi.EXPANDER_1, iopi.PORT_A)
io_port_out = iopi.Port(io_board, iopi.EXPANDER_1, iopi.PORT_B)
io_port_out.set_mode(iopi.DIR_OUTPUT)
self._log.info('using real GPIOs')
else:
i2cbus = io_board = io_port_in = io_port_out = None
self._log.warn('not running on a RasPi. GPIOs will be simulated.')
self._beacons = [
Beacon(
ident='L',
span=(0, 70),
dmxlid=run_args.left_id,
dmxlintf=dmxl_intf,
sensors_input=IO_Specs(io_port_in, (3, 4)),
sensors_enable=IO_Specs(io_port_out, (6, 5)),
laser_enable=IO_Specs(io_port_out, 3),
laser_off_delay=run_args.laser_off_delay,
auto_off_delay=run_args.scan_off_delay,
controller=self
),
Beacon(
ident='R',
span=(-70, 0),
dmxlid=run_args.right_id,
dmxlintf=dmxl_intf,
sensors_input=IO_Specs(io_port_in, (1, 2)),
sensors_enable=IO_Specs(io_port_out, (8, 7)),
laser_enable=IO_Specs(io_port_out, 4),
laser_off_delay=run_args.laser_off_delay,
auto_off_delay=run_args.scan_off_delay,
controller=self
)
]
# be sure sensors and lasers are off until we start scanning
for beacon in self._beacons:
beacon.enable_sensors(False)
beacon.enable_laser(False)
self._control_server = None
self._control_port = run_args.control_port
self._evtlistener_socket = None
self._notify_lock = threading.Lock()
def __init__(self, port, baudrate=57600, debug=False, simulate=False, trace=None):
""" Constructor:
Arguments:
port:
the serial port used for the serial link with it
baudrate:
serial link speed (default: 57600)
debug:
if True, activates the trace of serial data exchanges and various
other debugging help
simulate:
if True, simulate data exchanges
trace:
display format of the communication trace. None = no trace, 'd' = decimal, 'h' = hex
default : no trace
"""
self._serial = serial.Serial(port, baudrate=baudrate, timeout=0.5)
self._serial.flushInput()
self._serial.flushOutput()
# I/O serialization lock to be as much thread safe as possible
self._lock = threading.Lock()
self._debug = debug
self._trace = trace
if trace:
if not isinstance(trace, TraceOptions):
raise ValueError('trace options type mismatch')
self._trace_func = trace.trace_fmt
else:
self._trace_func = None
self._simulate = simulate
self._log = log.getLogger(type(self).__name__)
if self._debug:
self._log.setLevel(logging.DEBUG)
self._stream_listener = None
self._timer = None
# persistent status of the LEDs, so that it is possible to simplify the
# state change requests by specifying only the modifications
self._leds = None
# Persistent settings of low side drivers PWM duty cycle
# Initial state is supposed off (this will be set in constructor)
self._pwm = [0]*3
# events used to notify asynchronous operations
self._evt_packets = threading.Event()
self._evt_move = threading.Event()
def run(self):
""" Run the demo. """
self._logger.info("Initializing robot..")
robot = irobot.IRobotCreate(
port=self._args.port,
debug=self._args.debug,
trace=self._args.trace
)
robot.start(irobot.OI_MODE_FULL)
self._logger.info("Starting demo '%s'...", self._name)
self._meth(robot, log.getLogger(self._name))
self._logger.info('Demo complete.')
def __init__(self, name, args):
""" Constructor.
Parameters:
name:
the name of the demo, used to select the method which implements it
args:
the args to be passed to the method
"""
self._logger = log.getLogger('demo-create')
self._name = name
self._meth = getattr(self, self._name)
self._args = args
def __init__(self, name, _dspin, args):
""" Constructor.
Parameters:
name:
the name of the demo, used to select the method which implements it
_dspin:
the dSPIN interface object
args:
the command line arguments (containing various setting for the motor and the dSPIN)
"""
self._logger = log.getLogger('demo-dspin')
self._name = name
self._meth = getattr(self, self._name)
self._dspin = _dspin
self._args = args
def __init__(self,
ident,
span,
dmxlintf,
dmxlid,
sensors_input,
sensors_enable=None,
laser_enable=None,
laser_off_delay=30,
controller=None,
auto_off_delay=300
):
""" Constructor.
Parameters:
ident:
(str) symbolic identifier of the beacon
span:
(tuple) scanning angle span
dmxlintf:
instance of the DMXL bus interface used to
dialog with the beacon servo
dmxlid:
(int) id of the beacon AX12 servo
sensors_input:
(IO_Specs) IOPi port and IO ids of inputs from sensors
sensors_enable:
(IO_Specs) IOPi port and IO ids of outputs controlling the
activation of the sensors
laser_enable:
(IO_Specs) IOPi port and IO id of output controlling the
activation of the laser
laser_off_delay:
the delay (in secs) after which the laser is automatically
turned off
controller:
instance of the owning beacons controller managing
the beacon
auto_off_delay:
the delay (in secs) after which the beacon is automatically
turned off
"""
self._log = log.getLogger('beacon-%s' % ident)
self._ident = ident
self._dmxlintf = dmxlintf
self._dmxlid = dmxlid
if type(span) is tuple:
self._scan_limits = angle_to_pos(span)
else:
raise TypeError('span must be a tuple')
self._sensors_input = sensors_input
# compute and cache masks for faster processing while active
ios = sensors_input.ios
self._sensors_ios_masks = tuple(1 << (io - 1) for io in ios)
self._sensors_ios_gmask = reduce(lambda x, y : x | y, self._sensors_ios_masks)
self._sensors_ios_shift = min(ios) - 1
self._last_echos = 0
# setup the auto-off mechanism
self._auto_off_timer = None
self._auto_off_delay = auto_off_delay
self._sensors_enable = sensors_enable
if self._sensors_enable:
ios = sensors_enable.ios
self._sensors_enable_masks = tuple(1 << (io - 1) for io in ios)
self._laser_enable = laser_enable
if self._laser_enable:
io = laser_enable.ios
self._laser_enable_mask = 1 << (io - 1)
self._laser_off_delay = laser_off_delay
self._laser_off_timer = None
self._laser_state = False
self._scan_thread = None
self._active = False
self._scan_speed = 180 # deg/s
self._init_servo(dmxlid)
self._mode_method = self._mode_wipe
self._controller = controller
self._sim_echo_pos = None
self._position_errmax = 3
def server_activate(self):
""" Starts the control server."""
self._log = log.getLogger('server')
self._stop_request = threading.Event()
SocketServer.TCPServer.server_activate(self)
def sigterm_handler(signum, frame):
log.getLogger('sighandler').info('!! SIGTERM caught !!')
ControlServerHandler._shutdown_requested = True #pylint: disable=W0212
