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.
@@ -0,0 +1,4 @@
1
+ from .utils import reset_mcu
2
+ from .comm_lib import SerializingSocket, SerializingContext
3
+ from .actors import Motor, Servo
4
+ from .sensors import HC_SR04, AnalogGrayscale, get_speed_of_sound
@@ -0,0 +1,2 @@
1
+ from .motor import Motor
2
+ from .servo import Servo
@@ -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,6 @@
1
+ #!/usr/bin/env python3
2
+ """
3
+ Robot Hat Library
4
+ """
5
+ from .i2c import I2C
6
+ from .pwm import PWM
@@ -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')