81 lines
2.6 KiB
Python
81 lines
2.6 KiB
Python
from compLib.IRSensor import IRSensor
|
|
|
|
import time
|
|
|
|
ir = IRSensor()
|
|
|
|
MIN_VALUE = 0.0
|
|
MAX_VALUE = 0.0
|
|
MOVING_AVERAGE_SIZE = 30.0
|
|
|
|
|
|
def approximate_rolling_average(avg, value):
|
|
avg -= avg / MOVING_AVERAGE_SIZE
|
|
avg += value / MOVING_AVERAGE_SIZE
|
|
return avg
|
|
|
|
|
|
class IRWrapper(object):
|
|
"""Wrapper around the IRSensor to enable calibration of the sensor
|
|
"""
|
|
|
|
@staticmethod
|
|
def calibrate():
|
|
"""Calibrate the black and white values of the IRSensors
|
|
This is done by putting the bot on black line with the middle sensor
|
|
Afterwards, all sensors are read for 2 seconds and filtered.
|
|
The minimum value is used for white, maximum is black.
|
|
"""
|
|
global MIN_VALUE
|
|
global MAX_VALUE
|
|
|
|
left_avg = ir.bottom_left_analog()
|
|
middle_avg = ir.bottom_middle_analog()
|
|
right_avg = ir.bottom_right_analog()
|
|
|
|
start_time = time.time()
|
|
while time.time() - start_time < 2:
|
|
left_avg = approximate_rolling_average(left_avg, ir.bottom_left_analog())
|
|
middle_avg = approximate_rolling_average(middle_avg, ir.bottom_middle_analog())
|
|
right_avg = approximate_rolling_average(right_avg, ir.bottom_right_analog())
|
|
time.sleep(0.01)
|
|
|
|
MIN_VALUE = min([left_avg, middle_avg, right_avg])
|
|
MAX_VALUE = max([left_avg, middle_avg, right_avg])
|
|
|
|
@staticmethod
|
|
def adjust_for_calibration(raw_value: float) -> float:
|
|
"""Adjust a raw sensor value to 0 to 1000
|
|
|
|
:return: Value between 0 and 1000, White and Black
|
|
:rtype: float
|
|
"""
|
|
x = (raw_value - MIN_VALUE) * 1000.0 / (MAX_VALUE - MIN_VALUE)
|
|
return max(min(1000.0, x), 0.0)
|
|
|
|
@staticmethod
|
|
def bottom_left_calibrated() -> float:
|
|
"""Returns calibrated value of the bottom left analog sensor
|
|
|
|
:return: Value between 0 and 1000, White and Black
|
|
:rtype: float
|
|
"""
|
|
return IRWrapper.adjust_for_calibration(ir.bottom_left_analog())
|
|
|
|
@staticmethod
|
|
def bottom_middle_calibrated() -> float:
|
|
"""Returns calibrated value of the bottom middle analog sensor
|
|
|
|
:return: Value between 0 and 1000, White and Black
|
|
:rtype: float
|
|
"""
|
|
return IRWrapper.adjust_for_calibration(ir.bottom_middle_analog())
|
|
|
|
@staticmethod
|
|
def bottom_right_calibrated() -> float:
|
|
"""Returns calibrated value of the bottom right analog sensor
|
|
|
|
:return: Value between 0 and 1000, White and Black
|
|
:rtype: float
|
|
"""
|
|
return IRWrapper.adjust_for_calibration(ir.bottom_right_analog())
|