157 lines
4 KiB
Python
157 lines
4 KiB
Python
import pigpio
|
|
|
|
from compLib.ADC import ADC
|
|
from compLib.LogstashLogging import Logging
|
|
import spidev
|
|
import threading
|
|
import time
|
|
|
|
TOP_LEFT_CHANNEL = 0
|
|
TOP_RIGHT_CHANNEL = 1
|
|
|
|
TOP_IR_MIN_VOLTAGE = 0.0
|
|
TOP_IR_MAX_VOLTAGE = 3.3
|
|
|
|
BOTTOM_LEFT_PIN = 14
|
|
BOTTOM_MIDDLE_PIN = 15
|
|
BOTTOM_RIGHT_PIN = 23
|
|
|
|
BOTTOM_LEFT_CHANNEL = 2
|
|
BOTTOM_MIDDLE_CHANNEL = 1
|
|
BOTTOM_RIGHT_CHANNEL = 0
|
|
|
|
adc = ADC()
|
|
spi = spidev.SpiDev()
|
|
spi.open(0, 0)
|
|
spi.max_speed_hz = 1000000
|
|
|
|
# GPIO.setmode(GPIO.BCM)
|
|
|
|
GPIO = pigpio.pi()
|
|
|
|
GPIO.set_mode(BOTTOM_LEFT_PIN, pigpio.INPUT)
|
|
GPIO.set_mode(BOTTOM_MIDDLE_PIN, pigpio.INPUT)
|
|
GPIO.set_mode(BOTTOM_RIGHT_PIN, pigpio.INPUT)
|
|
|
|
states = {
|
|
str(BOTTOM_LEFT_PIN): GPIO.read(BOTTOM_LEFT_PIN),
|
|
str(BOTTOM_MIDDLE_PIN): GPIO.read(BOTTOM_MIDDLE_PIN),
|
|
str(BOTTOM_RIGHT_PIN): GPIO.read(BOTTOM_RIGHT_PIN)}
|
|
|
|
analog_states = {
|
|
str(BOTTOM_LEFT_CHANNEL): 0,
|
|
str(BOTTOM_MIDDLE_CHANNEL): 0,
|
|
str(BOTTOM_RIGHT_CHANNEL): 0}
|
|
|
|
|
|
def gpio_callback(gpio, level, tick):
|
|
states[str(gpio)] = level
|
|
|
|
|
|
GPIO.callback(BOTTOM_LEFT_PIN, pigpio.EITHER_EDGE, gpio_callback)
|
|
GPIO.callback(BOTTOM_MIDDLE_PIN, pigpio.EITHER_EDGE, gpio_callback)
|
|
GPIO.callback(BOTTOM_RIGHT_PIN, pigpio.EITHER_EDGE, gpio_callback)
|
|
|
|
|
|
def read_analog_channel(channel):
|
|
spi.writebytes([channel << 3, 0])
|
|
|
|
time.sleep(0.0033) # 300HZ
|
|
|
|
bytes = spi.readbytes(2)
|
|
return (bytes[0] * 256 + bytes[1]) >> 2
|
|
|
|
|
|
def sensor_thread():
|
|
while True:
|
|
analog_states[str(BOTTOM_LEFT_CHANNEL)] = read_analog_channel(BOTTOM_LEFT_CHANNEL)
|
|
analog_states[str(BOTTOM_MIDDLE_CHANNEL)] = read_analog_channel(BOTTOM_MIDDLE_CHANNEL)
|
|
analog_states[str(BOTTOM_RIGHT_CHANNEL)] = read_analog_channel(BOTTOM_RIGHT_CHANNEL)
|
|
|
|
|
|
analog_thread = threading.Thread(target=sensor_thread, daemon=True)
|
|
analog_thread.start()
|
|
time.sleep(0.25)
|
|
|
|
|
|
class IRSensor(object):
|
|
"""Access the different IR Sensors at top / bottom of the robot
|
|
"""
|
|
|
|
@staticmethod
|
|
def top_left_percent() -> int:
|
|
"""Get top left infrared sensor percentage
|
|
|
|
:return: Percentage between 0 and 100
|
|
:rtype: int
|
|
"""
|
|
voltage = adc.read(TOP_LEFT_CHANNEL)
|
|
return int((voltage - TOP_IR_MIN_VOLTAGE) / TOP_IR_MAX_VOLTAGE * 100)
|
|
|
|
@staticmethod
|
|
def top_right_percent() -> int:
|
|
"""Get top right infrared sensor percentage
|
|
|
|
:return: Percentage between 0 and 100
|
|
:rtype: int
|
|
"""
|
|
voltage = adc.read(TOP_RIGHT_CHANNEL)
|
|
return int((voltage - TOP_IR_MIN_VOLTAGE) / TOP_IR_MAX_VOLTAGE * 100)
|
|
|
|
@staticmethod
|
|
def bottom_left() -> bool:
|
|
"""Get bottom left infrared sensor status
|
|
|
|
:return: True, if sensor detects IR signals
|
|
:rtype: bool
|
|
"""
|
|
return states[str(BOTTOM_LEFT_PIN)]
|
|
# return GPIO.read(BOTTOM_LEFT_PIN)
|
|
|
|
@staticmethod
|
|
def bottom_middle() -> bool:
|
|
"""Get bottom middle infrared sensor status
|
|
|
|
:return: True, if sensor detects IR signals
|
|
:rtype: bool
|
|
"""
|
|
return states[str(BOTTOM_MIDDLE_PIN)]
|
|
# return GPIO.read(BOTTOM_MIDDLE_PIN)
|
|
|
|
@staticmethod
|
|
def bottom_right() -> bool:
|
|
"""Get bottom right infrared sensor status
|
|
|
|
:return: True, if sensor detects IR signals
|
|
:rtype: bool
|
|
"""
|
|
return states[str(BOTTOM_RIGHT_PIN)]
|
|
# return GPIO.read(BOTTOM_RIGHT_PIN)
|
|
|
|
@staticmethod
|
|
def bottom_left_analog() -> int:
|
|
"""Get bottom left infrared sensor value: ranges from 0 to 1023
|
|
|
|
:return: 10-bit brightness value
|
|
:rtype: int
|
|
"""
|
|
return analog_states[str(BOTTOM_LEFT_CHANNEL)]
|
|
|
|
@staticmethod
|
|
def bottom_middle_analog() -> int:
|
|
"""Get bottom middle infrared sensor value: ranges from 0 to 1023
|
|
|
|
:return: 10-bit brightness value
|
|
:rtype: int
|
|
"""
|
|
return analog_states[str(BOTTOM_MIDDLE_CHANNEL)]
|
|
|
|
@staticmethod
|
|
def bottom_right_analog() -> int:
|
|
"""Get bottom right infrared sensor value: ranges from 0 to 1023
|
|
|
|
:return: 10-bit brightness value
|
|
:rtype: int
|
|
"""
|
|
return analog_states[str(BOTTOM_RIGHT_CHANNEL)]
|
|
|