def __init__(self):
self._tof = qwiic.QwiicVL53L1X()
if self._tof.sensor_init() is None:
print("Sensor online!\n")
# set it to short range mode so it returns data faster
# but is limited to 1.3 meters which is enough for kitchen use
self._tof.set_distance_mode(1)
def __init__(self):
self.tof = qwiic.QwiicVL53L1X()
self.tof.sensor_init()
self.tof.start_ranging()
# Publishers
self.dist_pub = rospy.Publisher('/rpi_sensors/distance',
Float64, queue_size=1)
# Subscribers
rospy.Subscriber("/rpi_sensors/request_data",
String, self.request_handler)
def change_address(old_address: int, new_address):
# Connect to Mux
mux = qwiic.QwiicTCA9548A()
# Initially Disable All Channels
mux.disable_all()
print("Running VL53L1X I2C Address Change")
# Scans I2C addresses
avail_addresses = qwiic.scan()
# Check I2C addresses for Pi Servo pHat
while 0x40 in avail_addresses:
if 0x70 in avail_addresses:
pca = qwiic.QwiicPCA9685()
pca.set_addr_bit(0, 0)
avail_addresses = qwiic.scan()
# Remove Pi Servo pHat from avail_address list
try:
avail_addresses.remove(0x40)
except ValueError:
print(
"Addresses for Pi Servo pHat (0x40) not in scanned addresses.")
while 0x29 not in avail_addresses:
print(
"VL53L1X ToF sensor not detected on I2C bus at default address (0x29 or 41)."
)
mux.list_channels()
mux.enable_channels(4)
avail_addresses = qwiic.scan()
print("Initializing Device")
ToF = qwiic.QwiicVL53L1X(old_address)
ToF.SensorInit()
ToF.SetI2CAddress(new_address)
print("Address change to new address (", hex(new_address),
") is complete.")
def __init__(self):
super().__init__('vl53l1x_node')
self.sensor = qwiic.QwiicVL53L1X()
if (not self.init_sensor()):
self.get_logger().error(
"failed init_sensor() during construction, destroying node")
self.destroy_node()
self.error_count = 0
self.distance_msg = Range()
self.distance_msg.radiation_type = Range.INFRARED
self.distance_msg.field_of_view = 0.471 # 27 degrees
self.distance_msg.min_range = .04 # 40mm
self.distance_msg.max_range = 1.3 # 1.3m (short distance mode)
self.distance_pub = self.create_publisher(Range, 'tof_distance', 10)
timer_period = .125 # 125ms
self.timer = self.create_timer(timer_period, self.timer_callback)
# TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
# SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#=======================================================================
"""
Reading distance from the laser based VL53L1X
This example configures the sensor to short distance mode and then
prints the distance to an object. If you are getting weird readings,
be sure the vacuum tape has been removed from the sensor.
"""
import qwiic
import time
print("VL53L1X Qwiic Test\n")
ToF = qwiic.QwiicVL53L1X()
if (ToF.sensor_init() == None): # Begin returns 0 on a good init
print("Sensor online!\n")
ToF.set_distance_mode(1) # Sets Distance Mode Short (Long- Change value to 2)
while True:
try:
ToF.start_ranging(
) # Write configuration bytes to initiate measurement
time.sleep(.005)
distance = ToF.get_distance(
) # Get the result of the measurement from the sensor
time.sleep(.005)
ToF.stop_ranging()
import os, sys, time
import qwiic
# Connect to Mux
mux = qwiic.QwiicTCA9548A()
# Initially Disable All Channels
frontAddress = input("Select the front sensor address: ")
frontToF = qwiic.QwiicVL53L1X(frontAddress)
rearAddress = input("Select the rear sensor address: ")
rearToF = qwiic.QwiicVL53L1X(rearAddress)
try:
frontToF.SensorInit()
rearToF.SensorInit()
except Exception as e:
if e == OSError or e == IOError:
print("Issue connecting to device.")
print(e)
else:
print(e)
while True:
try:
frontToF.StartRanging() # Write configuration bytes to initiate measurement
rearToF.StartRanging()
time.sleep(.005)
frontDistance = frontToF.GetDistance() # Get the result of the measurement from the sensor
rearDistance = rearToF.GetDistance()
time.sleep(.005)
frontToF.StopRanging()
async def main():
"""
Runs the main control loop for this demo. Uses the KeyboardHelper class to read a keypress from the terminal.
W - Go forward. Press multiple times to increase speed.
A - Decrease heading by -10 degrees with each key press.
S - Go reverse. Press multiple times to increase speed.
D - Increase heading by +10 degrees with each key press.
Spacebar - Reset speed and flags to 0. RVR will coast to a stop
"""
global current_key_code
global speed
global heading
global flags
await rvr.wake()
await rvr.reset_yaw()
mux = qwiic.QwiicTCA9548A()
mux.disable_all()
mux.enable_channels([3, 4])
results = qwiic.list_devices()
avail_addresses = qwiic.scan()
print(avail_addresses)
for r in results:
print(r)
frontSensor = qwiic.QwiicVL53L1X(address=95)
rearSensor = qwiic.QwiicVL53L1X(address=64)
while True:
rearSensor.StartRanging()
frontSensor.StartRanging()
time.sleep(.005)
distance = frontSensor.GetDistance()
time.sleep(.005)
rDistance = rearSensor.GetDistance()
time.sleep(.005)
frontSensor.StopRanging()
rearSensor.StopRanging()
print("Front Distance: %s Rear Distance: %s" % (distance, rDistance))
if current_key_code == 119: # W
# if previously going reverse, reset speed back to 64
if flags == 1:
speed = 64
else:
# else increase speed
speed += 64
# go forward
flags = 0
elif current_key_code == 97: # A
heading -= 10
elif current_key_code == 115: # S
# if previously going forward, reset speed back to 64
if flags == 0:
speed = 64
else:
# else increase speed
speed += 64
# go reverse
flags = 1
elif current_key_code == 100: # D
heading += 10
elif current_key_code == 32: # SPACE
# reset speed and flags, but don't modify heading.
speed = 0
flags = 0
if distance < 50:
speed = 0
flags = 0
# check the speed value, and wrap as necessary.
if speed > 255:
speed = 255
elif speed < -255:
speed = -255
# check the heading value, and wrap as necessary.
if heading > 359:
heading = heading - 359
elif heading < 0:
heading = 359 + heading
# reset the key code every loop
current_key_code = -1
# issue the driving command
await rvr.drive_with_heading(speed, heading, flags)
# sleep the infinite loop for a 10th of a second to avoid flooding the serial port.
await asyncio.sleep(0.1)
if args.change_address:
import VL53L1X_change_address
VL53L1X_change_address.change_address(old_address=41, new_address=85)
# Connect to Mux
mux = qwiic.QwiicTCA9548A()
# Initially Disable All Channels
mux.disable_all()
pca = qwiic.QwiicPCA9685()
pca.set_addr_bit(0, 0)
mux.enable_channels(3)
mux.enable_channels(4)
ToF_front = qwiic.QwiicVL53L1X(41)
ToF_front.SensorInit()
print("Forward sensor initialized.")
ToF_rear = qwiic.QwiicVL53L1X(85)
ToF_rear.SensorInit()
print("Rear sensor initialized.")
# The callback for when the client receives a CONNACK response from the server.
def on_connect(client, userdata, flags, rc):
print("Connected with result code " + str(rc))
# The callback for when a PUBLISH message is received from the server.
def on_message(client, userdata, msg):
print(msg.topic + " " + str(msg.payload))
