200 lines
6.4 KiB
Python
200 lines
6.4 KiB
Python
"""
|
|
QTRSensors.h - Originally Arduino Library for using Pololu QTR reflectance sensors and reflectance sensor arrays
|
|
|
|
MIT Licence
|
|
Copyright (c) 2008-2012 waveshare Corporation. For more information, see
|
|
https://www.waveshare.com/wiki/AlphaBot2-Ar
|
|
|
|
Copyright (c) 2021 leomlr (Léo Meillier). For more information, see
|
|
https://github.com/vittascience/stm32-libraries
|
|
https://vittascience.com/stm32/
|
|
|
|
You may freely modify and share this code, as long as you keep this notice intact.
|
|
|
|
Disclaimer: To the extent permitted by law, waveshare provides this work
|
|
without any warranty. It might be defective, in which case you agree
|
|
to be responsible for all resulting costs and damages.
|
|
|
|
Author: Léo Meillier (leomlr)
|
|
Date: 07/2021
|
|
Note: library adapted in micropython for using 5 QTR sensors on Alphabot v2 robot controlled by STM32 board.
|
|
"""
|
|
|
|
import pyb
|
|
from micropython import const
|
|
import utime
|
|
|
|
PIN_CS = 'D10'
|
|
PIN_DOUT = 'D11'
|
|
PIN_ADDR = 'D12'
|
|
PIN_CLK = 'D13'
|
|
|
|
NUMSENSORS = const(5)
|
|
|
|
QTR_EMITTERS_OFF = const(0x00)
|
|
QTR_EMITTERS_ON = const(0x01)
|
|
QTR_EMITTERS_ON_AND_OFF = const(0x02)
|
|
|
|
QTR_NO_EMITTER_PIN = const(0xff)
|
|
|
|
QTR_MAX_SENSORS = const(16)
|
|
|
|
class TRSensors(object):
|
|
|
|
""" Base class data member initialization (called by derived class init()). """
|
|
def __init__(self, cs=PIN_CS, dout=PIN_DOUT, addr=PIN_ADDR, clk=PIN_CLK):
|
|
|
|
self._cs = pyb.Pin(cs, pyb.Pin.OUT)
|
|
self._dout = pyb.Pin(dout, pyb.Pin.IN)
|
|
self._addr = pyb.Pin(addr, pyb.Pin.OUT)
|
|
self._clk = pyb.Pin(clk, pyb.Pin.OUT)
|
|
|
|
self._numSensors = NUMSENSORS
|
|
|
|
self.calibratedMin = [0] * self._numSensors
|
|
self.calibratedMax = [1023] * self._numSensors
|
|
self.last_value = 0
|
|
|
|
""" Reads the sensor values using TLC1543 ADC chip into an array.
|
|
The values returned are a measure of the reflectance in abstract units,
|
|
with higher values corresponding to lower reflectance (e.g. a black
|
|
surface or a void). """
|
|
|
|
def analogRead(self):
|
|
value = [0]* (self._numSensors+1)
|
|
#Read Channel0~channel4 AD value
|
|
for j in range(0, self._numSensors+1):
|
|
self._cs.off()
|
|
for i in range(0,4):
|
|
#sent 4-bit Address
|
|
if (j >> (3 - i)) & 0x01:
|
|
self._addr.on()
|
|
else:
|
|
self._addr.off()
|
|
#read MSB 4-bit data
|
|
value[j] <<= 1
|
|
if self._dout.value():
|
|
value[j] |= 0x01
|
|
self._clk.on()
|
|
self._clk.off()
|
|
for i in range(0, self._numSensors+1):
|
|
#read LSB 8-bit data
|
|
value[j] <<= 1
|
|
if self._dout.value():
|
|
value[j] |= 0x01
|
|
self._clk.on()
|
|
self._clk.off()
|
|
#no mean ,just delay
|
|
#for i in range(0,6):
|
|
# self._clk.on()
|
|
# self._clk.off()
|
|
utime.sleep_us(100)
|
|
self._cs.on()
|
|
return value[1:]
|
|
|
|
""" Reads the sensors 10 times and uses the results for
|
|
calibration. The sensor values are not returned instead, the
|
|
maximum and minimum values found over time are stored internally
|
|
and used for the readCalibrated() method. """
|
|
|
|
def calibrate(self):
|
|
sensor_values = []
|
|
max_sensor_values = [0]*self._numSensors
|
|
min_sensor_values = [0]*self._numSensors
|
|
|
|
for j in range(0, 10):
|
|
sensor_values = self.analogRead()
|
|
for i in range(0, self._numSensors):
|
|
# set the max we found THIS time
|
|
if j == 0 or max_sensor_values[i] < sensor_values[i]:
|
|
max_sensor_values[i] = sensor_values[i]
|
|
# set the min we found THIS time
|
|
if j == 0 or min_sensor_values[i] > sensor_values[i]:
|
|
min_sensor_values[i] = sensor_values[i]
|
|
|
|
# record the min and max calibration values
|
|
for i in range(0, self._numSensors):
|
|
if min_sensor_values[i] > self.calibratedMax[i]:
|
|
self.calibratedMax[i] = min_sensor_values[i]
|
|
if max_sensor_values[i] < self.calibratedMin[i]:
|
|
self.calibratedMin[i] = max_sensor_values[i]
|
|
|
|
""" Returns values calibrated to a value between 0 and 1000, where
|
|
0 corresponds to the minimum value read by calibrate() and 1000
|
|
corresponds to the maximum value. Calibration values are
|
|
stored separately for each sensor, so that differences in the
|
|
sensors are accounted for automatically. """
|
|
|
|
def readCalibrated(self):
|
|
|
|
# read the needed values
|
|
sensor_values = self.analogRead()
|
|
|
|
for i in range(self._numSensors):
|
|
denominator = self.calibratedMax[i] - self.calibratedMin[i]
|
|
value = 0
|
|
if denominator is not 0:
|
|
value = (sensor_values[i] - self.calibratedMin[i]) * 1000 / denominator
|
|
if value < 0:
|
|
value = 0
|
|
elif value > 1000:
|
|
value = 1000
|
|
sensor_values[i] = value
|
|
|
|
return sensor_values
|
|
|
|
""" Operates the same as read calibrated, but also returns an
|
|
estimated position of the robot with respect to a line. The
|
|
estimate is made using a weighted average of the sensor indices
|
|
multiplied by 1000, so that a return value of 0 indicates that
|
|
the line is directly below sensor 0, a return value of 1000
|
|
indicates that the line is directly below sensor 1, 2000
|
|
indicates that it's below sensor 2000, etc. Intermediate
|
|
values indicate that the line is between two sensors. The
|
|
formula is:
|
|
|
|
0*value0 + 1000*value1 + 2000*value2 + ...
|
|
--------------------------------------------
|
|
value0 + value1 + value2 + ...
|
|
|
|
By default, this function assumes a dark line (high values)
|
|
surrounded by white (low values). If your line is light on
|
|
black, set the optional second argument white_line to true. In
|
|
this case, each sensor value will be replaced by (1000-value)
|
|
before the averaging. """
|
|
|
|
def readLine(self, white_line = 0):
|
|
|
|
sensor_values = self.readCalibrated()
|
|
avg = 0
|
|
sum = 0
|
|
on_line = 0
|
|
for i in range(0, self._numSensors):
|
|
value = sensor_values[i]
|
|
if white_line:
|
|
value = 1000-value
|
|
# keep track of whether we see the line at all
|
|
if value > 200:
|
|
on_line = 1
|
|
|
|
# only average in values that are above a noise threshold
|
|
if value > 50:
|
|
avg += value * (i * 1000) # this is for the weighted total,
|
|
sum += value # this is for the denominator
|
|
|
|
if on_line != 1:
|
|
# If it last read to the left of center, return 0.
|
|
if self.last_value < (self._numSensors - 1)*1000/2:
|
|
#print("left")
|
|
self.last_value = 0
|
|
|
|
# If it last read to the right of center, return the max.
|
|
else:
|
|
#print("right")
|
|
self.last_value = (self._numSensors - 1)*1000
|
|
|
|
else:
|
|
self.last_value = avg/sum
|
|
|
|
return self.last_value,sensor_values;
|