84 lines
2.6 KiB
Python
84 lines
2.6 KiB
Python
|
import machine
|
||
|
|
import time
|
||
|
|
from i2c_handler import I2CSlaveHandler, Packet
|
||
|
|
|
||
|
|
class DistanceHandler(I2CSlaveHandler):
|
||
|
|
"""
|
||
|
|
I2C handler that responds to master requests by sending distance values
|
||
|
|
from an HC-SR04 ultrasonic sensor.
|
||
|
|
"""
|
||
|
|
|
||
|
|
def __init__(self, i2c_id, sda, scl, slave_addr, trig_pin, echo_pin):
|
||
|
|
super().__init__(i2c_id, sda, scl, slave_addr)
|
||
|
|
self.trig = machine.Pin(trig_pin, machine.Pin.OUT)
|
||
|
|
self.echo = machine.Pin(echo_pin, machine.Pin.IN)
|
||
|
|
|
||
|
|
def read_distance_mm(self):
|
||
|
|
"""
|
||
|
|
Measure distance using the HC-SR04 sensor.
|
||
|
|
Returns distance in millimeters as an integer.
|
||
|
|
"""
|
||
|
|
# Send a 10us pulse to trigger
|
||
|
|
self.trig.low()
|
||
|
|
time.sleep_us(2)
|
||
|
|
self.trig.high()
|
||
|
|
time.sleep_us(10)
|
||
|
|
self.trig.low()
|
||
|
|
|
||
|
|
# Wait for echo high (timeout after 25ms)
|
||
|
|
timeout = 25000
|
||
|
|
start = time.ticks_us()
|
||
|
|
while not self.echo.value():
|
||
|
|
if time.ticks_diff(time.ticks_us(), start) > timeout:
|
||
|
|
return 0 # sensor error
|
||
|
|
t1 = time.ticks_us()
|
||
|
|
while self.echo.value():
|
||
|
|
if time.ticks_diff(time.ticks_us(), t1) > timeout:
|
||
|
|
return 0 # sensor error
|
||
|
|
t2 = time.ticks_us()
|
||
|
|
|
||
|
|
# Calculate duration and distance
|
||
|
|
duration = time.ticks_diff(t2, t1)
|
||
|
|
distance_mm = (duration * 100) // 582
|
||
|
|
# HC-SR04: distance (mm) = duration (us) / 5.82
|
||
|
|
|
||
|
|
# Clamp to 0..65535
|
||
|
|
return max(0, min(distance_mm, 65535))
|
||
|
|
|
||
|
|
def process_request(self):
|
||
|
|
"""
|
||
|
|
Called when master requests data. Responds with a packet containing distance.
|
||
|
|
Packet format:
|
||
|
|
Byte 0: 0x01 (distance command)
|
||
|
|
Byte 1: reserved
|
||
|
|
Byte 2-3: distance (uint16, mm, little-endian)
|
||
|
|
Byte 4-6: reserved
|
||
|
|
Byte 7: checksum (xor of bytes 0-6)
|
||
|
|
"""
|
||
|
|
#dist = self.read_distance_mm()
|
||
|
|
dist = 155
|
||
|
|
data = [0] * Packet.LENGTH
|
||
|
|
data[0] = 0x01 # Command: distance
|
||
|
|
data[2] = dist & 0xFF # Low byte
|
||
|
|
data[3] = (dist >> 8) & 0xFF # High byte
|
||
|
|
|
||
|
|
# Checksum: XOR of bytes 0-6
|
||
|
|
cs = 0
|
||
|
|
for b in data[:-1]:
|
||
|
|
cs ^= b
|
||
|
|
data[7] = cs
|
||
|
|
|
||
|
|
print(Packet.from_bytes(data))
|
||
|
|
return Packet.from_bytes(data)
|
||
|
|
|
||
|
|
def main():
|
||
|
|
handler = DistanceHandler(i2c_id=0, sda=0, scl=1, slave_addr=0x41, trig_pin=3, echo_pin=2)
|
||
|
|
print("Distance handler I2C slave started")
|
||
|
|
try:
|
||
|
|
handler.handle()
|
||
|
|
except KeyboardInterrupt:
|
||
|
|
handler.deinit()
|
||
|
|
print("I2C slave stopped")
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main()
|