Create businessbot.py
This commit is contained in:
Hel Gibbons
parent
99672e2760
commit
3bdb313f3d
|
|
@ -0,0 +1,131 @@
|
|||
import pimoroni_i2c
|
||||
import breakout_vl53l5cx
|
||||
import time
|
||||
from ulab import numpy
|
||||
from motor import Motor, pico_motor_shim
|
||||
from pimoroni import NORMAL_DIR, REVERSED_DIR
|
||||
|
||||
# The VL53L5CX requires a firmware blob to start up.
|
||||
# Make sure you upload "vl53l5cx_firmware.bin" via Thonny to the root of your filesystem
|
||||
# You can find it here: https://github.com/ST-mirror/VL53L5CX_ULD_driver/blob/no-fw/lite/en/vl53l5cx_firmware.bin
|
||||
|
||||
# This example attempts to track a "bright" object (such as a white business card).
|
||||
# It uses reflectance to identify the target and compute the X/Y coordinates
|
||||
# of its "center of mass" in the sensors view.
|
||||
# We then use the object tracking to drive a little robot towards a business card onna stick!
|
||||
|
||||
# Motion indication only works at distances > 400mm so it's not
|
||||
# really useful as a method to reject data.
|
||||
|
||||
# Configure your distance and brightness thresholds to suit your object
|
||||
DISTANCE_THRESHOLD = 400 # Distance in mm
|
||||
REFLECTANCE_THRESHOLD = 60 # Estimated reflectance in %
|
||||
|
||||
PINS_BREAKOUT_GARDEN = {"sda": 4, "scl": 5}
|
||||
PINS_PICO_EXPLORER = {"sda": 20, "scl": 21}
|
||||
|
||||
# Sensor startup time is proportional to i2c baudrate
|
||||
# HOWEVER many sensors may not run at > 400KHz (400000)
|
||||
i2c = pimoroni_i2c.PimoroniI2C(**PINS_BREAKOUT_GARDEN, baudrate=2_000_000)
|
||||
|
||||
# Speed / distance constants for the robot - modify these to change driving behaviour!
|
||||
DRIVING_SPEED = 0.7 # The speed to drive the wheels at when going forward
|
||||
TURNING_SPEED = 0.2 # The speed to drive the wheels at when turning
|
||||
GOAL_DISTANCE = 100.0 # The distance in mm the robot will keep an object in front of it
|
||||
SPEED_RANGE = 50.0 # The distance an object is from the goal that will have the robot drive at full speed
|
||||
|
||||
# Setup the left and right motors - we're connecting our motors via a Motor SHIM for Pico.
|
||||
# Swap the directions if this is different to your setup
|
||||
left = Motor(pico_motor_shim.MOTOR_1, direction=REVERSED_DIR)
|
||||
right = Motor(pico_motor_shim.MOTOR_2, direction=NORMAL_DIR)
|
||||
|
||||
# Setup the VL53L5CX sensor
|
||||
print("Starting up sensor...")
|
||||
t_sta = time.ticks_ms()
|
||||
sensor = breakout_vl53l5cx.VL53L5CX(i2c)
|
||||
t_end = time.ticks_ms()
|
||||
print("Done in {}ms...".format(t_end - t_sta))
|
||||
|
||||
# Make sure to set resolution and other settings *before* you start ranging
|
||||
sensor.set_resolution(breakout_vl53l5cx.RESOLUTION_8X8)
|
||||
sensor.set_ranging_frequency_hz(15)
|
||||
sensor.start_ranging()
|
||||
|
||||
while True:
|
||||
time.sleep(1.0 / 60)
|
||||
if sensor.data_ready():
|
||||
# "data" is a namedtuple (attrtuple technically)
|
||||
# it includes average readings as "distance_avg" and "reflectance_avg"
|
||||
# plus a full 4x4 or 8x8 set of readings (as a 1d tuple) for both values.
|
||||
data = sensor.get_data()
|
||||
|
||||
reflectance = numpy.array(data.reflectance).reshape((8, 8))
|
||||
distance = numpy.array(data.distance).reshape((8, 8))
|
||||
|
||||
scalar = 0
|
||||
target_distance = 0
|
||||
n_distances = 0
|
||||
# Filter out unwanted reflectance values
|
||||
for ox in range(8):
|
||||
for oy in range(8):
|
||||
d = distance[ox][oy]
|
||||
r = reflectance[ox][oy]
|
||||
if d > DISTANCE_THRESHOLD or r < REFLECTANCE_THRESHOLD:
|
||||
reflectance[ox][oy] = 0
|
||||
else:
|
||||
scalar += r
|
||||
|
||||
# Get a total from all the distances within our accepted target
|
||||
for ox in range(8):
|
||||
for oy in range(8):
|
||||
d = distance[ox][oy]
|
||||
r = reflectance[ox][oy]
|
||||
if r > 0:
|
||||
target_distance += d
|
||||
n_distances += 1
|
||||
|
||||
# Average the target distance
|
||||
if n_distances > 0:
|
||||
target_distance /= n_distances
|
||||
else:
|
||||
target_distance = 0
|
||||
|
||||
# Flip reflectance now we've applied distance
|
||||
# both fields are upside-down!
|
||||
reflectance = numpy.flip(reflectance, axis=0)
|
||||
|
||||
# Calculate the center of mass along X and Y
|
||||
x = 0
|
||||
y = 0
|
||||
if scalar > 0:
|
||||
for ox in range(8):
|
||||
for oy in range(8):
|
||||
y += reflectance[ox][oy] * ox
|
||||
y /= scalar
|
||||
y /= 3.5
|
||||
y -= 1.0
|
||||
|
||||
for oy in range(8):
|
||||
for ox in range(8):
|
||||
x += reflectance[ox][oy] * oy
|
||||
x /= scalar
|
||||
x /= 3.5
|
||||
x -= 1.0
|
||||
|
||||
print("Object detected at x: {:.2f}, y: {:.2f}".format(x, y))
|
||||
|
||||
# Our robot will try and keep the object a goal distance in front of it.
|
||||
# If the object gets closer it will reverse, if the object gets further away it will drive forward.
|
||||
scale = (target_distance - GOAL_DISTANCE) / SPEED_RANGE
|
||||
spd = max(min(scale, 1.0), -1.0) * DRIVING_SPEED
|
||||
|
||||
print("Distance is {:.1f} mm. Speed is {:.2f}".format(target_distance, spd))
|
||||
|
||||
left.speed(spd - (x * TURNING_SPEED))
|
||||
right.speed(spd + (x * TURNING_SPEED))
|
||||
else:
|
||||
left.coast()
|
||||
right.coast()
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue