intsys-car-v1 0.0.2__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- intsys_car_v1/__init__.py +4 -0
- intsys_car_v1/actors/__init__.py +2 -0
- intsys_car_v1/actors/motor.py +90 -0
- intsys_car_v1/actors/servo.py +138 -0
- intsys_car_v1/comm_lib/__init__.py +1 -0
- intsys_car_v1/comm_lib/serialization_socket.py +72 -0
- intsys_car_v1/robot_hat/__init__.py +6 -0
- intsys_car_v1/robot_hat/basic.py +61 -0
- intsys_car_v1/robot_hat/i2c.py +281 -0
- intsys_car_v1/robot_hat/pwm.py +216 -0
- intsys_car_v1/sensors/__init__.py +2 -0
- intsys_car_v1/sensors/camera/__init__.py +0 -0
- intsys_car_v1/sensors/line_detection_analog_grayscale.py +45 -0
- intsys_car_v1/sensors/ultrasonic_hc_sr04.py +129 -0
- intsys_car_v1/utils.py +21 -0
- intsys_car_v1-0.0.2.dist-info/METADATA +21 -0
- intsys_car_v1-0.0.2.dist-info/RECORD +19 -0
- intsys_car_v1-0.0.2.dist-info/WHEEL +4 -0
- intsys_car_v1-0.0.2.dist-info/licenses/LICENSE +674 -0
|
@@ -0,0 +1,90 @@
|
|
|
1
|
+
import time
|
|
2
|
+
|
|
3
|
+
import gpiozero
|
|
4
|
+
from gpiozero.pins.lgpio import LGPIOFactory
|
|
5
|
+
|
|
6
|
+
from robot_hat.pwm import PWM
|
|
7
|
+
|
|
8
|
+
# Motor Left PWM P13
|
|
9
|
+
# Motor Left Dir Pin: D4 - 23
|
|
10
|
+
# Motor Right PWM P12
|
|
11
|
+
# Motor Right Dir Pin: D5 - 24
|
|
12
|
+
|
|
13
|
+
class Motor:
|
|
14
|
+
"""Motor"""
|
|
15
|
+
PERIOD = 4095
|
|
16
|
+
PRESCALER = 10
|
|
17
|
+
DEFAULT_FREQ = 100 # Hz
|
|
18
|
+
_is_reversed = False
|
|
19
|
+
_is_in_reverse = False
|
|
20
|
+
_speed = 0
|
|
21
|
+
_min_speed_to_move = 0
|
|
22
|
+
|
|
23
|
+
def __init__(self, pwm_pin, dir_pin, factory, min_speed_to_move=0, is_reversed=False, freq=DEFAULT_FREQ):
|
|
24
|
+
"""
|
|
25
|
+
Initialize a motor
|
|
26
|
+
|
|
27
|
+
:param pwm: Motor speed control pwm pin
|
|
28
|
+
:type pwm: robot_hat.pwm.PWM
|
|
29
|
+
:param dir: Motor direction control pin
|
|
30
|
+
:type dir: robot_hat.pin.Pin
|
|
31
|
+
"""
|
|
32
|
+
|
|
33
|
+
self.dir = gpiozero.OutputDevice(dir_pin, pin_factory=factory)
|
|
34
|
+
self.freq = freq
|
|
35
|
+
self._is_reversed = is_reversed
|
|
36
|
+
self._min_speed_to_move = min_speed_to_move
|
|
37
|
+
self.pwm = PWM(pwm_pin)
|
|
38
|
+
self.pwm.period(4095)
|
|
39
|
+
self.pwm.prescaler(10)
|
|
40
|
+
self.pwm.freq(self.freq)
|
|
41
|
+
self.pwm.pulse_width_percent(0)
|
|
42
|
+
|
|
43
|
+
self._speed = 0
|
|
44
|
+
|
|
45
|
+
@property
|
|
46
|
+
def is_reversed(self):
|
|
47
|
+
return self._is_in_reverse
|
|
48
|
+
|
|
49
|
+
@is_reversed.setter
|
|
50
|
+
def is_reversed(self, value):
|
|
51
|
+
if self._is_reversed:
|
|
52
|
+
value = not value
|
|
53
|
+
if value:
|
|
54
|
+
self.dir.on()
|
|
55
|
+
else:
|
|
56
|
+
self.dir.off()
|
|
57
|
+
self._is_in_reverse = value
|
|
58
|
+
|
|
59
|
+
@property
|
|
60
|
+
def speed(self):
|
|
61
|
+
return self._speed
|
|
62
|
+
|
|
63
|
+
@speed.setter
|
|
64
|
+
def speed(self, value):
|
|
65
|
+
# Ensure speed to be between -100 and 100
|
|
66
|
+
value = max(min(100, value), -100)
|
|
67
|
+
|
|
68
|
+
self._speed = value
|
|
69
|
+
scaled_speed = self._min_speed_to_move + (value / 100) * (100 - self._min_speed_to_move) if value != 0 else 0
|
|
70
|
+
self.pwm.pulse_width_percent(abs(scaled_speed))
|
|
71
|
+
self.is_reversed = value < 0
|
|
72
|
+
|
|
73
|
+
def setSpeed(self, speed):
|
|
74
|
+
self.speed = speed
|
|
75
|
+
|
|
76
|
+
|
|
77
|
+
def main():
|
|
78
|
+
factory = LGPIOFactory()
|
|
79
|
+
motor_left = Motor("P13", 23, factory, min_speed_to_move=25)
|
|
80
|
+
motor_left.setSpeed(0)
|
|
81
|
+
motor_right = Motor("P12", 24, factory, is_reversed=True, min_speed_to_move=25)
|
|
82
|
+
motor_right.speed = 0
|
|
83
|
+
while True:
|
|
84
|
+
# Program needs to be running, if not the dir_pin is reset.
|
|
85
|
+
time.sleep(1)
|
|
86
|
+
|
|
87
|
+
|
|
88
|
+
|
|
89
|
+
if __name__ == '__main__':
|
|
90
|
+
main()
|
|
@@ -0,0 +1,138 @@
|
|
|
1
|
+
#!/usr/bin/env python3
|
|
2
|
+
from robot_hat import PWM
|
|
3
|
+
|
|
4
|
+
def mapping(x, in_min, in_max, out_min, out_max):
|
|
5
|
+
"""
|
|
6
|
+
Map value from one range to another range
|
|
7
|
+
|
|
8
|
+
:param x: value to map
|
|
9
|
+
:type x: float/int
|
|
10
|
+
:param in_min: input minimum
|
|
11
|
+
:type in_min: float/int
|
|
12
|
+
:param in_max: input maximum
|
|
13
|
+
:type in_max: float/int
|
|
14
|
+
:param out_min: output minimum
|
|
15
|
+
:type out_min: float/int
|
|
16
|
+
:param out_max: output maximum
|
|
17
|
+
:type out_max: float/int
|
|
18
|
+
:return: mapped value
|
|
19
|
+
:rtype: float/int
|
|
20
|
+
"""
|
|
21
|
+
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
|
|
22
|
+
|
|
23
|
+
class Servo(PWM):
|
|
24
|
+
"""Servo motor class"""
|
|
25
|
+
MAX_PW = 2500
|
|
26
|
+
MIN_PW = 500
|
|
27
|
+
FREQ = 50
|
|
28
|
+
PERIOD = 4095
|
|
29
|
+
_min = -90
|
|
30
|
+
_max = 90
|
|
31
|
+
_invert_angle = False
|
|
32
|
+
_compensate_angle = 0
|
|
33
|
+
|
|
34
|
+
@property
|
|
35
|
+
def min_angle(self):
|
|
36
|
+
return self._min
|
|
37
|
+
|
|
38
|
+
@min_angle.setter
|
|
39
|
+
def min_angle(self, value):
|
|
40
|
+
# Security check, servo range is max -90 to 90
|
|
41
|
+
self._min = max(value, -90)
|
|
42
|
+
|
|
43
|
+
@property
|
|
44
|
+
def max_angle(self):
|
|
45
|
+
return self._max
|
|
46
|
+
|
|
47
|
+
@max_angle.setter
|
|
48
|
+
def max_angle(self, value):
|
|
49
|
+
# Security check, servo range is max -90 to 90
|
|
50
|
+
self._max = min(value, 90)
|
|
51
|
+
|
|
52
|
+
@property
|
|
53
|
+
def invert_angle(self):
|
|
54
|
+
return self._invert_angle
|
|
55
|
+
|
|
56
|
+
@invert_angle.setter
|
|
57
|
+
def invert_angle(self, value):
|
|
58
|
+
self._invert_angle = value
|
|
59
|
+
|
|
60
|
+
@property
|
|
61
|
+
def compensate_angle(self):
|
|
62
|
+
return self._compensate_angle
|
|
63
|
+
|
|
64
|
+
@compensate_angle.setter
|
|
65
|
+
def compensate_angle(self, value):
|
|
66
|
+
self._compensate_angle = value
|
|
67
|
+
|
|
68
|
+
def __init__(self, name, channel, address=None, min_angle=-90, max_angle=90, compensate_angle=0, invert_angle=False, *args, **kwargs):
|
|
69
|
+
"""
|
|
70
|
+
Initialize the servo motor class
|
|
71
|
+
|
|
72
|
+
:param channel: PWM channel number(0-14/P0-P14)
|
|
73
|
+
:type channel: int/str
|
|
74
|
+
"""
|
|
75
|
+
print(f"Initializing servo motor {name} on {channel}")
|
|
76
|
+
super().__init__(channel, address, *args, **kwargs)
|
|
77
|
+
self.name = name
|
|
78
|
+
self.min_angle = min_angle
|
|
79
|
+
self.max_angle = max_angle
|
|
80
|
+
self.invert_angle = invert_angle
|
|
81
|
+
self.period(self.PERIOD)
|
|
82
|
+
self.compensate_angle = compensate_angle
|
|
83
|
+
prescaler = self.CLOCK / self.FREQ / self.PERIOD
|
|
84
|
+
self.prescaler(prescaler)
|
|
85
|
+
|
|
86
|
+
def angle(self, angle):
|
|
87
|
+
self.setAngle(angle)
|
|
88
|
+
|
|
89
|
+
def setAngle(self, angle):
|
|
90
|
+
"""
|
|
91
|
+
Set the angle of the servo motor
|
|
92
|
+
|
|
93
|
+
:param angle: angle(-90~90)
|
|
94
|
+
:type angle: float
|
|
95
|
+
"""
|
|
96
|
+
if not (isinstance(angle, int) or isinstance(angle, float)):
|
|
97
|
+
raise ValueError(
|
|
98
|
+
"Angle value should be int or float value, not %s" % type(angle))
|
|
99
|
+
angle += self.compensate_angle
|
|
100
|
+
if self.invert_angle:
|
|
101
|
+
angle = -angle
|
|
102
|
+
if angle < self.min_angle:
|
|
103
|
+
angle = self.min_angle
|
|
104
|
+
if angle > self.max_angle:
|
|
105
|
+
angle = self.max_angle
|
|
106
|
+
self._debug(f"Set angle to: {angle}")
|
|
107
|
+
pulse_width_time = mapping(angle, -90, 90, self.MIN_PW, self.MAX_PW)
|
|
108
|
+
self._debug(f"Pulse width: {pulse_width_time}")
|
|
109
|
+
self.pulse_width_time(pulse_width_time)
|
|
110
|
+
|
|
111
|
+
def pulse_width_time(self, pulse_width_time):
|
|
112
|
+
"""
|
|
113
|
+
Set the pulse width of the servo motor
|
|
114
|
+
|
|
115
|
+
:param pulse_width_time: pulse width time(500~2500)
|
|
116
|
+
:type pulse_width_time: float
|
|
117
|
+
"""
|
|
118
|
+
if pulse_width_time > self.MAX_PW:
|
|
119
|
+
pulse_width_time = self.MAX_PW
|
|
120
|
+
if pulse_width_time < self.MIN_PW:
|
|
121
|
+
pulse_width_time = self.MIN_PW
|
|
122
|
+
|
|
123
|
+
pwr = pulse_width_time / 20000
|
|
124
|
+
self._debug(f"pulse width rate: {pwr}")
|
|
125
|
+
value = int(pwr * self.PERIOD)
|
|
126
|
+
self._debug(f"pulse width value: {value}")
|
|
127
|
+
self.pulse_width(value)
|
|
128
|
+
|
|
129
|
+
def main():
|
|
130
|
+
cam_pan_servo = Servo("P0", invert_angle=True, compensate_angle=-6)
|
|
131
|
+
cam_tilt_servo = Servo("P1", min_angle=-65, max_angle=40)
|
|
132
|
+
steering_servo = Servo("P2", min_angle=-30, max_angle=30, compensate_angle=-10)
|
|
133
|
+
cam_pan_servo.angle(0)
|
|
134
|
+
cam_tilt_servo.angle(0)
|
|
135
|
+
steering_servo.angle(0)
|
|
136
|
+
|
|
137
|
+
if __name__ == '__main__':
|
|
138
|
+
main()
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
from .serialization_socket import SerializingSocket, SerializingContext
|
|
@@ -0,0 +1,72 @@
|
|
|
1
|
+
import zmq
|
|
2
|
+
import numpy as np
|
|
3
|
+
import cv2
|
|
4
|
+
|
|
5
|
+
class SerializingSocket(zmq.Socket):
|
|
6
|
+
"""Numpy array serialization methods.
|
|
7
|
+
|
|
8
|
+
Modelled on PyZMQ serialization examples.
|
|
9
|
+
|
|
10
|
+
Used for sending / receiving OpenCV images, which are Numpy arrays.
|
|
11
|
+
Also used for sending / receiving jpg compressed OpenCV images.
|
|
12
|
+
"""
|
|
13
|
+
|
|
14
|
+
def send_img(self, frame, msg):
|
|
15
|
+
ret_code, img = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, 90])
|
|
16
|
+
self.send_array(img, msg)
|
|
17
|
+
|
|
18
|
+
def recv_img(self):
|
|
19
|
+
msg, img = self.recv_array()
|
|
20
|
+
img = cv2.imdecode(img, -1)
|
|
21
|
+
return msg, img
|
|
22
|
+
|
|
23
|
+
def send_array(self, arr, msg=None, flags=0, copy=True, track=False):
|
|
24
|
+
"""Sends a numpy array with metadata and text message.
|
|
25
|
+
|
|
26
|
+
Sends a numpy array with the metadata necessary for reconstructing
|
|
27
|
+
the array (dtype,shape). Also sends a text msg, often the array or
|
|
28
|
+
image name.
|
|
29
|
+
|
|
30
|
+
Arguments:
|
|
31
|
+
arr: numpy array or OpenCV image.
|
|
32
|
+
msg: (optional) Json data.
|
|
33
|
+
flags: (optional) zmq flags.
|
|
34
|
+
copy: (optional) zmq copy flag.
|
|
35
|
+
track: (optional) zmq track flag.
|
|
36
|
+
"""
|
|
37
|
+
|
|
38
|
+
if msg is None:
|
|
39
|
+
msg = {}
|
|
40
|
+
md = dict(
|
|
41
|
+
msg=msg,
|
|
42
|
+
dtype=str(arr.dtype),
|
|
43
|
+
shape=arr.shape,
|
|
44
|
+
)
|
|
45
|
+
self.send_json(md, flags | zmq.SNDMORE)
|
|
46
|
+
return self.send(arr, flags, copy=copy, track=track)
|
|
47
|
+
|
|
48
|
+
def recv_array(self, flags=0, copy=True, track=False):
|
|
49
|
+
"""Receives a numpy array with metadata and text message.
|
|
50
|
+
|
|
51
|
+
Receives a numpy array with the metadata necessary
|
|
52
|
+
for reconstructing the array (dtype,shape).
|
|
53
|
+
Returns the array and a text msg, often the array or image name.
|
|
54
|
+
|
|
55
|
+
Arguments:
|
|
56
|
+
flags: (optional) zmq flags.
|
|
57
|
+
copy: (optional) zmq copy flag.
|
|
58
|
+
track: (optional) zmq track flag.
|
|
59
|
+
|
|
60
|
+
Returns:
|
|
61
|
+
msg: JSON Object with additional data.
|
|
62
|
+
A: numpy array or OpenCV image reconstructed with dtype and shape.
|
|
63
|
+
"""
|
|
64
|
+
|
|
65
|
+
md = self.recv_json(flags=flags)
|
|
66
|
+
msg = self.recv(flags=flags, copy=copy, track=track)
|
|
67
|
+
A = np.frombuffer(msg, dtype=md['dtype'])
|
|
68
|
+
return md['msg'], A.reshape(md['shape'])
|
|
69
|
+
|
|
70
|
+
|
|
71
|
+
class SerializingContext(zmq.Context):
|
|
72
|
+
_socket_class = SerializingSocket
|
|
@@ -0,0 +1,61 @@
|
|
|
1
|
+
# -*- coding: utf-8 -*-
|
|
2
|
+
#!/usr/bin/env python3
|
|
3
|
+
import logging
|
|
4
|
+
import time
|
|
5
|
+
|
|
6
|
+
|
|
7
|
+
class _Basic_class(object):
|
|
8
|
+
"""
|
|
9
|
+
Basic Class for all classes
|
|
10
|
+
|
|
11
|
+
with debug function
|
|
12
|
+
"""
|
|
13
|
+
_class_name = '_Basic_class'
|
|
14
|
+
DEBUG_LEVELS = {'debug': logging.DEBUG,
|
|
15
|
+
'info': logging.INFO,
|
|
16
|
+
'warning': logging.WARNING,
|
|
17
|
+
'error': logging.ERROR,
|
|
18
|
+
'critical': logging.CRITICAL,
|
|
19
|
+
}
|
|
20
|
+
"""Debug level"""
|
|
21
|
+
DEBUG_NAMES = ['critical', 'error', 'warning', 'info', 'debug']
|
|
22
|
+
"""Debug level names"""
|
|
23
|
+
|
|
24
|
+
def __init__(self, debug_level='warning'):
|
|
25
|
+
"""
|
|
26
|
+
Initialize the basic class
|
|
27
|
+
|
|
28
|
+
:param debug_level: debug level, 0(critical), 1(error), 2(warning), 3(info) or 4(debug)
|
|
29
|
+
:type debug_level: str/int
|
|
30
|
+
"""
|
|
31
|
+
self.logger = logging.getLogger(f"self._class_name-{time.time()}")
|
|
32
|
+
self.ch = logging.StreamHandler()
|
|
33
|
+
form = "%(asctime)s [%(levelname)s] %(message)s"
|
|
34
|
+
self.formatter = logging.Formatter(form)
|
|
35
|
+
self.ch.setFormatter(self.formatter)
|
|
36
|
+
self.logger.addHandler(self.ch)
|
|
37
|
+
self._debug = self.logger.debug
|
|
38
|
+
self._info = self.logger.info
|
|
39
|
+
self._warning = self.logger.warning
|
|
40
|
+
self._error = self.logger.error
|
|
41
|
+
self._critical = self.logger.critical
|
|
42
|
+
self.debug_level = debug_level
|
|
43
|
+
|
|
44
|
+
@property
|
|
45
|
+
def debug_level(self):
|
|
46
|
+
"""Debug level"""
|
|
47
|
+
return self._debug_level
|
|
48
|
+
|
|
49
|
+
@debug_level.setter
|
|
50
|
+
def debug_level(self, debug):
|
|
51
|
+
"""Debug level"""
|
|
52
|
+
if debug in range(5):
|
|
53
|
+
self._debug_level = self.DEBUG_NAMES[debug]
|
|
54
|
+
elif debug in self.DEBUG_NAMES:
|
|
55
|
+
self._debug_level = debug
|
|
56
|
+
else:
|
|
57
|
+
raise ValueError(
|
|
58
|
+
f'Debug value must be 0(critical), 1(error), 2(warning), 3(info) or 4(debug), not "{debug}".')
|
|
59
|
+
self.logger.setLevel(self.DEBUG_LEVELS[self._debug_level])
|
|
60
|
+
self.ch.setLevel(self.DEBUG_LEVELS[self._debug_level])
|
|
61
|
+
self._debug(f'Set logging level to [{self._debug_level}]')
|
|
@@ -0,0 +1,281 @@
|
|
|
1
|
+
#!/usr/bin/env python3
|
|
2
|
+
|
|
3
|
+
from smbus2 import SMBus
|
|
4
|
+
import multiprocessing
|
|
5
|
+
|
|
6
|
+
from robot_hat.basic import _Basic_class
|
|
7
|
+
|
|
8
|
+
def run_command(cmd):
|
|
9
|
+
"""
|
|
10
|
+
Run command and return status and output
|
|
11
|
+
|
|
12
|
+
:param cmd: command to run
|
|
13
|
+
:type cmd: str
|
|
14
|
+
:return: status, output
|
|
15
|
+
:rtype: tuple
|
|
16
|
+
"""
|
|
17
|
+
import subprocess
|
|
18
|
+
p = subprocess.Popen(
|
|
19
|
+
cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
|
|
20
|
+
result = p.stdout.read().decode('utf-8')
|
|
21
|
+
status = p.poll()
|
|
22
|
+
return status, result
|
|
23
|
+
|
|
24
|
+
def _retry_wrapper(func):
|
|
25
|
+
|
|
26
|
+
def wrapper(self, *arg, **kwargs):
|
|
27
|
+
for _ in range(self.RETRY):
|
|
28
|
+
try:
|
|
29
|
+
return func(self, *arg, **kwargs)
|
|
30
|
+
except OSError:
|
|
31
|
+
self._debug(f"OSError: {func.__name__}")
|
|
32
|
+
continue
|
|
33
|
+
else:
|
|
34
|
+
return False
|
|
35
|
+
|
|
36
|
+
return wrapper
|
|
37
|
+
|
|
38
|
+
|
|
39
|
+
class I2C(_Basic_class):
|
|
40
|
+
"""
|
|
41
|
+
I2C bus read/write functions
|
|
42
|
+
"""
|
|
43
|
+
RETRY = 5
|
|
44
|
+
|
|
45
|
+
# i2c_lock = multiprocessing.Value('i', 0)
|
|
46
|
+
|
|
47
|
+
def __init__(self, address=None, bus=1, *args, **kwargs):
|
|
48
|
+
"""
|
|
49
|
+
Initialize the I2C bus
|
|
50
|
+
|
|
51
|
+
:param address: I2C device address
|
|
52
|
+
:type address: int
|
|
53
|
+
:param bus: I2C bus number
|
|
54
|
+
:type bus: int
|
|
55
|
+
"""
|
|
56
|
+
super().__init__(*args, **kwargs)
|
|
57
|
+
self._bus = bus
|
|
58
|
+
self._smbus = SMBus(self._bus)
|
|
59
|
+
if isinstance(address, list):
|
|
60
|
+
connected_devices = self.scan()
|
|
61
|
+
for _addr in address:
|
|
62
|
+
if _addr in connected_devices:
|
|
63
|
+
self.address = _addr
|
|
64
|
+
break
|
|
65
|
+
else:
|
|
66
|
+
self.address = address[0]
|
|
67
|
+
else:
|
|
68
|
+
self.address = address
|
|
69
|
+
|
|
70
|
+
# print(f'address: 0x{self.address:02X}')
|
|
71
|
+
|
|
72
|
+
@_retry_wrapper
|
|
73
|
+
def _write_byte(self, data):
|
|
74
|
+
# with I2C.i2c_lock.get_lock():
|
|
75
|
+
self._debug(f"_write_byte: [0x{data:02X}]")
|
|
76
|
+
result = self._smbus.write_byte(self.address, data)
|
|
77
|
+
return result
|
|
78
|
+
|
|
79
|
+
@_retry_wrapper
|
|
80
|
+
def _write_byte_data(self, reg, data):
|
|
81
|
+
# with I2C.i2c_lock.get_lock():
|
|
82
|
+
self._debug(f"_write_byte_data: [0x{reg:02X}] [0x{data:02X}]")
|
|
83
|
+
return self._smbus.write_byte_data(self.address, reg, data)
|
|
84
|
+
|
|
85
|
+
@_retry_wrapper
|
|
86
|
+
def _write_word_data(self, reg, data):
|
|
87
|
+
# with I2C.i2c_lock.get_lock():
|
|
88
|
+
self._debug(f"_write_word_data: [0x{reg:02X}] [0x{data:04X}]")
|
|
89
|
+
return self._smbus.write_word_data(self.address, reg, data)
|
|
90
|
+
|
|
91
|
+
@_retry_wrapper
|
|
92
|
+
def _write_i2c_block_data(self, reg, data):
|
|
93
|
+
# with I2C.i2c_lock.get_lock():
|
|
94
|
+
self._debug(
|
|
95
|
+
f"_write_i2c_block_data: [0x{reg:02X}] {[f'0x{i:02X}' for i in data]}"
|
|
96
|
+
)
|
|
97
|
+
return self._smbus.write_i2c_block_data(self.address, reg, data)
|
|
98
|
+
|
|
99
|
+
@_retry_wrapper
|
|
100
|
+
def _read_byte(self):
|
|
101
|
+
# with I2C.i2c_lock.get_lock():
|
|
102
|
+
result = self._smbus.read_byte(self.address)
|
|
103
|
+
self._debug(f"_read_byte: [0x{result:02X}]")
|
|
104
|
+
return result
|
|
105
|
+
|
|
106
|
+
@_retry_wrapper
|
|
107
|
+
def _read_byte_data(self, reg):
|
|
108
|
+
# with I2C.i2c_lock.get_lock():
|
|
109
|
+
result = self._smbus.read_byte_data(self.address, reg)
|
|
110
|
+
self._debug(f"_read_byte_data: [0x{reg:02X}] [0x{result:02X}]")
|
|
111
|
+
return result
|
|
112
|
+
|
|
113
|
+
@_retry_wrapper
|
|
114
|
+
def _read_word_data(self, reg):
|
|
115
|
+
# with I2C.i2c_lock.get_lock():
|
|
116
|
+
result = self._smbus.read_word_data(self.address, reg)
|
|
117
|
+
result_list = [result & 0xFF, (result >> 8) & 0xFF]
|
|
118
|
+
self._debug(f"_read_word_data: [0x{reg:02X}] [0x{result:04X}]")
|
|
119
|
+
return result_list
|
|
120
|
+
|
|
121
|
+
@_retry_wrapper
|
|
122
|
+
def _read_i2c_block_data(self, reg, num):
|
|
123
|
+
# with I2C.i2c_lock.get_lock():
|
|
124
|
+
result = self._smbus.read_i2c_block_data(self.address, reg, num)
|
|
125
|
+
self._debug(
|
|
126
|
+
f"_read_i2c_block_data: [0x{reg:02X}] {[f'0x{i:02X}' for i in result]}"
|
|
127
|
+
)
|
|
128
|
+
return result
|
|
129
|
+
|
|
130
|
+
@_retry_wrapper
|
|
131
|
+
def is_ready(self):
|
|
132
|
+
"""Check if the I2C device is ready
|
|
133
|
+
|
|
134
|
+
:return: True if the I2C device is ready, False otherwise
|
|
135
|
+
:rtype: bool
|
|
136
|
+
"""
|
|
137
|
+
addresses = self.scan()
|
|
138
|
+
if self.address in addresses:
|
|
139
|
+
return True
|
|
140
|
+
else:
|
|
141
|
+
return False
|
|
142
|
+
|
|
143
|
+
def scan(self):
|
|
144
|
+
"""Scan the I2C bus for devices
|
|
145
|
+
|
|
146
|
+
:return: List of I2C addresses of devices found
|
|
147
|
+
:rtype: list
|
|
148
|
+
"""
|
|
149
|
+
cmd = f"i2cdetect -y {self._bus}"
|
|
150
|
+
# Run the i2cdetect command
|
|
151
|
+
_, output = run_command(cmd)
|
|
152
|
+
|
|
153
|
+
# Parse the output
|
|
154
|
+
outputs = output.split('\n')[1:]
|
|
155
|
+
addresses = []
|
|
156
|
+
addresses_str = []
|
|
157
|
+
for tmp_addresses in outputs:
|
|
158
|
+
if tmp_addresses == "":
|
|
159
|
+
continue
|
|
160
|
+
tmp_addresses = tmp_addresses.split(':')[1]
|
|
161
|
+
# Split the addresses into a list
|
|
162
|
+
tmp_addresses = tmp_addresses.strip().split(' ')
|
|
163
|
+
for address in tmp_addresses:
|
|
164
|
+
if address != '--':
|
|
165
|
+
addresses.append(int(address, 16))
|
|
166
|
+
addresses_str.append(f'0x{address}')
|
|
167
|
+
self._debug(f"Conneceted i2c device: {addresses_str}")
|
|
168
|
+
return addresses
|
|
169
|
+
|
|
170
|
+
def write(self, data):
|
|
171
|
+
"""Write data to the I2C device
|
|
172
|
+
|
|
173
|
+
:param data: Data to write
|
|
174
|
+
:type data: int/list/bytearray
|
|
175
|
+
:raises: ValueError if write is not an int, list or bytearray
|
|
176
|
+
"""
|
|
177
|
+
if isinstance(data, bytearray):
|
|
178
|
+
data_all = list(data)
|
|
179
|
+
elif isinstance(data, int):
|
|
180
|
+
if data == 0:
|
|
181
|
+
data_all = [0]
|
|
182
|
+
else:
|
|
183
|
+
data_all = []
|
|
184
|
+
while data > 0:
|
|
185
|
+
data_all.append(data & 0xFF)
|
|
186
|
+
data >>= 8
|
|
187
|
+
elif isinstance(data, list):
|
|
188
|
+
data_all = data
|
|
189
|
+
else:
|
|
190
|
+
raise ValueError(
|
|
191
|
+
f"write data must be int, list, or bytearray, not {type(data)}"
|
|
192
|
+
)
|
|
193
|
+
|
|
194
|
+
# Write data
|
|
195
|
+
if len(data_all) == 1:
|
|
196
|
+
data = data_all[0]
|
|
197
|
+
self._write_byte(data)
|
|
198
|
+
elif len(data_all) == 2:
|
|
199
|
+
reg = data_all[0]
|
|
200
|
+
data = data_all[1]
|
|
201
|
+
self._write_byte_data(reg, data)
|
|
202
|
+
elif len(data_all) == 3:
|
|
203
|
+
reg = data_all[0]
|
|
204
|
+
data = (data_all[2] << 8) + data_all[1]
|
|
205
|
+
self._write_word_data(reg, data)
|
|
206
|
+
else:
|
|
207
|
+
reg = data_all[0]
|
|
208
|
+
data = list(data_all[1:])
|
|
209
|
+
self._write_i2c_block_data(reg, data)
|
|
210
|
+
|
|
211
|
+
def read(self, length=1):
|
|
212
|
+
"""Read data from I2C device
|
|
213
|
+
|
|
214
|
+
:param length: Number of bytes to receive
|
|
215
|
+
:type length: int
|
|
216
|
+
:return: Received data
|
|
217
|
+
:rtype: list
|
|
218
|
+
"""
|
|
219
|
+
if not isinstance(length, int):
|
|
220
|
+
raise ValueError(f"length must be int, not {type(length)}")
|
|
221
|
+
|
|
222
|
+
result = []
|
|
223
|
+
for _ in range(length):
|
|
224
|
+
result.append(self._read_byte())
|
|
225
|
+
return result
|
|
226
|
+
|
|
227
|
+
def mem_write(self, data, memaddr):
|
|
228
|
+
"""Send data to specific register address
|
|
229
|
+
|
|
230
|
+
:param data: Data to send, int, list or bytearray
|
|
231
|
+
:type data: int/list/bytearray
|
|
232
|
+
:param memaddr: Register address
|
|
233
|
+
:type memaddr: int
|
|
234
|
+
:raise ValueError: If data is not int, list, or bytearray
|
|
235
|
+
"""
|
|
236
|
+
if isinstance(data, bytearray):
|
|
237
|
+
data_all = list(data)
|
|
238
|
+
elif isinstance(data, list):
|
|
239
|
+
data_all = data
|
|
240
|
+
elif isinstance(data, int):
|
|
241
|
+
data_all = []
|
|
242
|
+
if data == 0:
|
|
243
|
+
data_all = [0]
|
|
244
|
+
else:
|
|
245
|
+
while data > 0:
|
|
246
|
+
data_all.append(data & 0xFF)
|
|
247
|
+
data >>= 8
|
|
248
|
+
else:
|
|
249
|
+
raise ValueError(
|
|
250
|
+
"memery write require arguement of bytearray, list, int less than 0xFF"
|
|
251
|
+
)
|
|
252
|
+
self._write_i2c_block_data(memaddr, data_all)
|
|
253
|
+
|
|
254
|
+
def mem_read(self, length, memaddr):
|
|
255
|
+
"""Read data from specific register address
|
|
256
|
+
|
|
257
|
+
:param length: Number of bytes to receive
|
|
258
|
+
:type length: int
|
|
259
|
+
:param memaddr: Register address
|
|
260
|
+
:type memaddr: int
|
|
261
|
+
:return: Received bytearray data or False if error
|
|
262
|
+
:rtype: list/False
|
|
263
|
+
"""
|
|
264
|
+
result = self._read_i2c_block_data(memaddr, length)
|
|
265
|
+
return result
|
|
266
|
+
|
|
267
|
+
def is_avaliable(self):
|
|
268
|
+
"""
|
|
269
|
+
Check if the I2C device is avaliable
|
|
270
|
+
|
|
271
|
+
:return: True if the I2C device is avaliable, False otherwise
|
|
272
|
+
:rtype: bool
|
|
273
|
+
"""
|
|
274
|
+
return self.address in self.scan()
|
|
275
|
+
|
|
276
|
+
def __del__(self):
|
|
277
|
+
self._smbus.close()
|
|
278
|
+
self._smbus = None
|
|
279
|
+
|
|
280
|
+
if __name__ == "__main__":
|
|
281
|
+
i2c = I2C(address=[0x17, 0x15], debug_level='debug')
|