/
moth.py
executable file
·116 lines (103 loc) · 4.74 KB
/
moth.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# Copyright 2020-2021 by Murray Altheim. All rights reserved. This file is part
# of the Robot Operating System project, released under the MIT License. Please
# see the LICENSE file included as part of this package.
#
# author: Murray Altheim
# created: 2021-02-12
# modified: 2021-02-12
#
from colorama import init, Fore, Style
init()
from lib.enums import Orientation
from lib.logger import Logger, Level
from lib.ioe import IoExpander
class Moth(object):
'''
A "moth" behaviour is where the robot moves towards the brightest light; an "anti-moth"
behaviour towards the darker area of its environment.
Captures the two analog outputs from the IO Expander used for moth and anti-moth sensing.
returning pairs of int and float values, an Orientation enum, or a bias ratio between
-max and +max. The int and float values are determined by the actual sensor outputs, with
the int values rounded up and multiplied by 100. So if the sensor output at maximum light
intensity is 3.3v the raw value would be 3.3 and the int value would be 330.
There is a hysteresis configuration that sets the range within which the port-starboard
bias is considered nil.
The actual sensor used can be any analog light sensor that outputs a floating value. On
the KD01/KR01 robots this happens to be a pair of Adafruit GA1A12S202 Log-scale Analog
Light Sensors, aiming forward, port and starboard at about 45 degrees.
:param config: the application configuration
:param ioe: the optional IO Expander. If not supplied one will be created from the
provided configuration.
:param level: the log level
'''
def __init__(self, config, ioe, level):
if config is None:
raise ValueError('no configuration provided.')
_config = config['ros'].get('moth')
self._hysteresis = _config.get('hysteresis')
self._log = Logger("moth", level)
if ioe:
self._ioe = ioe
else:
self._ioe = IoExpander(config, Level.INFO)
self._max_v = 3.3 # maximum sensor voltage expected
self._max_port_bias = 0.0
self._max_stbd_bias = 0.0
self._log.info('ready.')
# ..........................................................................
@property
def ioe(self):
'''
Return the IO Expander used by the Moth sensor.
'''
return self._ioe
def get_orientation(self):
'''
Processes the two sensor inputs, returning Orientation.PORT,
Orientation.STBD, or Orientation.NONE when there is no bias.
The hysteresis determining the range of "no bias" is set in
configuration.
'''
_values = self._ioe.get_moth_values()
if _values[0] > _values[1] + self._hysteresis:
self._log.info(Fore.RED + Style.BRIGHT + 'Orientation.PORT:\t'
+ Fore.RED + Style.NORMAL + '{:d}\t'.format(_values[0]) + Fore.GREEN + '{:d}'.format(_values[1]))
return Orientation.PORT
elif _values[0] < _values[1] - self._hysteresis:
self._log.info(Fore.GREEN + Style.BRIGHT + 'Orientation.STBD:\t'
+ Fore.RED + Style.NORMAL + '{:d}\t'.format(_values[0]) + Fore.GREEN + '{:d}'.format(_values[1]))
return Orientation.STBD
else:
self._log.info(Fore.YELLOW + Style.BRIGHT + 'Orientation.NONE:\t'
+ Fore.RED + Style.NORMAL + '{:d}\t'.format(_values[0]) + Fore.GREEN + '{:d}'.format(_values[1]))
return Orientation.NONE
def get_int_values(self):
'''
Return the values of the port and starboard moth sensors
(resp.) as a two-element array, in the range 0-MAX, where
MAX is likely 330.
'''
return self._ioe.get_moth_values()
def get_values(self):
'''
Return the values of the port and starboard moth sensors
(resp.) as a two-element array, in the range 0.0-MAX_V,
where MAX_V is likely 3.3v.
'''
return self._ioe.get_raw_moth_values()
def get_bias(self):
'''
Returns a two-element array containing a port and starboard value
between 0.0 and 1.0. There is no hysteresis in the result.
'''
_values = self._ioe.get_raw_moth_values()
_port_bias = _values[0] / self._max_v
self._max_port_bias = max(self._max_port_bias, _port_bias)
_stbd_bias = _values[1] / self._max_v
self._max_stbd_bias = max(self._max_stbd_bias, _stbd_bias)
# self._log.debug(Fore.RED + '{:5.2f}/{:5.2f}\t'.format(_port_bias, self._max_port_bias) + Fore.GREEN + '{:5.2f}/{:5.2f}'.format(_stbd_bias, self._max_stbd_bias))
return _port_bias, _stbd_bias
#EOF