81 lines
2.5 KiB
Python
81 lines
2.5 KiB
Python
from compLib.LogstashLogging import Logging
|
|
from compLib.MetricsLogging import MetricsLogging
|
|
from compLib.Spi import Spi, Register
|
|
|
|
SENSOR_COUNT = 5
|
|
|
|
|
|
class IRSensor(object):
|
|
"""Access the different IR Sensors of the robot
|
|
"""
|
|
|
|
@staticmethod
|
|
def read(sensor: int) -> int:
|
|
"""Read one infrared sensor
|
|
|
|
:param sensor: Which sensor to read. Between 1 and 5
|
|
:raises: IndexError
|
|
:return: Sensor value. 10 bit accuracy
|
|
:rtype: int
|
|
"""
|
|
if sensor <= 0 or sensor > SENSOR_COUNT:
|
|
raise IndexError("Invalid sensor specified!")
|
|
|
|
result = 0
|
|
if sensor == 1:
|
|
result = Spi.read(Register.IR_1_H, 2)
|
|
elif sensor == 2:
|
|
result = Spi.read(Register.IR_2_H, 2)
|
|
elif sensor == 3:
|
|
result = Spi.read(Register.IR_3_H, 2)
|
|
elif sensor == 4:
|
|
result = Spi.read(Register.IR_4_H, 2)
|
|
elif sensor == 5:
|
|
result = Spi.read(Register.IR_5_H, 2)
|
|
|
|
MetricsLogging.put("Infrared", result, sensor)
|
|
return result
|
|
|
|
@staticmethod
|
|
def read_all():
|
|
"""Read all IR sensors at once.
|
|
This is faster than read as it only uses one SPI call for all sensors!
|
|
|
|
:return: Tuple of all current ir sensors
|
|
"""
|
|
|
|
sensors = Spi.read_array(Register.IR_1_H, 5 * 2)
|
|
|
|
sensor_1 = int.from_bytes(
|
|
sensors[0:2], byteorder='big', signed=False)
|
|
sensor_2 = int.from_bytes(
|
|
sensors[2:4], byteorder='big', signed=False)
|
|
sensor_3 = int.from_bytes(
|
|
sensors[4:6], byteorder='big', signed=False)
|
|
sensor_4 = int.from_bytes(
|
|
sensors[6:8], byteorder='big', signed=False)
|
|
sensor_5 = int.from_bytes(
|
|
sensors[8:10], byteorder='big', signed=False)
|
|
|
|
return (sensor_1, sensor_2, sensor_3, sensor_4, sensor_5)
|
|
|
|
@staticmethod
|
|
def set(sensor: int, on: bool):
|
|
"""Turn on / off a IR emitter
|
|
|
|
:param sensor: Which sensor to read. Between 1 and 5
|
|
:raises: IndexError
|
|
"""
|
|
if sensor <= 0 or sensor > SENSOR_COUNT:
|
|
raise IndexError("Invalid sensor specified!")
|
|
|
|
if sensor == 1:
|
|
Spi.write(Register.IR_1_LED, 1, on)
|
|
elif sensor == 2:
|
|
Spi.write(Register.IR_2_LED, 1, on)
|
|
elif sensor == 3:
|
|
Spi.write(Register.IR_3_LED, 1, on)
|
|
elif sensor == 4:
|
|
Spi.write(Register.IR_4_LED, 1, on)
|
|
elif sensor == 5:
|
|
Spi.write(Register.IR_5_LED, 1, on)
|