class ICAHSensor:
ping_dur = 0.00015 # s, same as measured for gpiozero
speed_of_sound = 343 # m/s at 101325 Pa, 293 K
def __init__(self, trig, echo):
self.trig = DigitalOutputDevice(trig)
self.echo = DigitalInputDevice(echo)
self.hist = []
def get_distance(self):
'''
This could be made non-blocking by using a background
thread that continually updates the current distance.
We should also reject outliers in the distance measurements
using the filter() function or similar.
'''
del (self.hist[:])
for i in range(10):
self.trig.on()
sleep(self.ping_dur)
self.trig.off()
self.echo.wait_for_active()
t0 = time()
self.echo.wait_for_inactive()
dt = time() - t0
dist = dt * ICAHSensor.speed_of_sound / 2
self.hist.append(dist)
return sum(self.hist) / len(self.hist)
def setup():
encoder_a = DigitalInputDevice(12)
while True:
encoder_a.wait_for_active()
print('ON!')
encoder_a.wait_for_inactive()
print('OFF!')
class RPIMotionSensor(MotionSensor):
def __init__(self):
super().__init__()
self.logger = logging.getLogger(
'rpiplatesrecognition_client.RPIMotionSensor')
self.input_device = DigitalInputDevice(self.current_gpio_pin_number,
pull_up=False,
bounce_time=1)
def wait_for_high_state(self, function):
self.input_device.wait_for_active()
self.logger.debug("Motion detected, invoking trigger photo")
function()
class Bumper():
'''
Bumper Task: reacts to bumper sensors.
Parameters:
pin: the GPIO pin used as an input
label: the label used in logging
level: the logging Level
activated_callback: a callback function called when the sensor is activated
deactivated_callback: a callback function called when the sensor is deactivated
Returns:
true for 1 second duration after a positive sensor reading.
Usage:
PIN = 25
LABEL = 'center'
LEVEL = Level.INFO
_activated_callback = self.activated_callback
_deactivated_callback = self.deactivated_callback
_bumper = Bumper(PIN, LABEL, LEVEL, _activated_callback, _deactivated_callback)
_bumper.enable()
value = _bumper.get()
'''
def __init__(self, pin, label, level, activated_callback,
deactivated_callback):
'''
The parameters include two optional callback functions, one when
the bumper is activated, another when deactivated.
'''
self._log = Logger("bumper:" + label, level)
self._log.debug('initialising bumper:{} on pin {}...'.format(
label, pin))
self._enabled = False
if activated_callback is None:
self._log.error("no activated_callback argument provided.")
if deactivated_callback is None:
self._log.error("no deactivated_callback argument provided.")
self._pin = pin
self._label = label
self._sensor = DigitalInputDevice(pin,
bounce_time=BOUNCE_TIME_SEC,
pull_up=True)
self._activated_callback = activated_callback
self._deactivated_callback = deactivated_callback
self._sensor.when_activated = self._activated
self._sensor.when_deactivated = self._deactivated
self._wait_for_inactive = True
self._log.info('bumper on pin {} ready.'.format(label, pin))
# ..........................................................................
def set_wait_for_inactive(self, state):
self._log.info('set wait for inactive for bumper:{} to: {}'.format(
self._label, state))
self._wait_for_inactive = state
# ..........................................................................
def enable(self):
self._enabled = True
self._log.info('enabled.')
# ..........................................................................
def disable(self):
self._enabled = False
self._log.info('disabled.')
# ..........................................................................
def poll(self):
_active = self._sensor.is_active
self._log.info('poll {} bumper: {}'.format(self._label, _active))
return _active
# ..........................................................................
def _activated(self):
'''
The default function called when the sensor is activated.
'''
self._log.info('>> activated bumper:{} on pin {}...'.format(
self._label, self._pin))
if self._enabled:
if self._activated_callback != None:
self._log.info('calling activated_callback...')
# Pause the script until the device is deactivated, or the timeout is reached.
# Parameters: timeout (float or None) – Number of seconds to wait before proceeding.
# If this is None (the default), then wait indefinitely until the device is inactive.
# self._log.info('wait for inactive: bumper:{} on pin {}.'.format(self._label,self._pin))
if self._wait_for_inactive:
self._sensor.wait_for_inactive(
timeout=ACTIVATE_TIMEOUT_SEC)
# self._sensor.wait_for_inactive(timeout=None)
self._activated_callback()
else:
self._log.warning('activated_callback is None!')
else:
self._log.warning(
'[DISABLED] activated bumper:{} on pin {}...'.format(
self._label, self._pin))
# ..........................................................................
def _deactivated(self):
'''
The default function called when the sensor is deactivated.
'''
self._log.debug('>> deactivated bumper:{} on pin {}...'.format(
self._label, self._pin))
if self._enabled:
if self._deactivated_callback != None:
self._log.debug('calling deactivated_callback...')
# Pause the script until the device is activated, or the timeout is reached.
# Parameters: timeout (float or None) – Number of seconds to wait before proceeding.
# If this is None (the default), then wait indefinitely until the device is active.
# self._log.info('wait for active: bumper:{} on pin {}.'.format(self._label,self._pin))
self._sensor.wait_for_active(timeout=DEACTIVATE_TIMEOUT_SEC)
# self._sensor.wait_for_active(timeout=None)
self._deactivated_callback()
else:
self._log.warning('deactivated_callback is None!')
else:
self._log.warning(
'[DISABLED] deactivated bumper:{} on pin {}...'.format(
self._label, self._pin))
# ..........................................................................
def close(self):
self._log.info('closed.')
class IQS5xx(object):
def __init__(self, resetPin, readyPin, address=IQS5xx_DEFAULT_ADDRESS):
self.address = address
self._resetPinNum = resetPin
self._readyPinNum = readyPin
self._resetPin = OutputDevice(pin=self._resetPinNum,
active_high=False,
initial_value=True)
self._readypin = DigitalInputDevice(pin=self._readyPinNum,
active_state=True,
pull_up=None)
def begin(self):
self.releaseReset()
time.sleep(0.01)
self.waitUntilReady()
self.acknowledgeReset()
time.sleep(0.01)
self.acknowledgeReset()
time.sleep(0.01)
self.endSession()
time.sleep(0.020)
@property
def address(self):
return self.__address
@address.setter
def address(self, value):
if (value < IQS5xx_DEFAULT_ADDRESS) or (value > IQS5xx_MAX_ADDRESS):
raise ValueError(
"Invalid I2C Address. Use something in the range [%x, %x]" %
(IQS5xx_DEFAULT_ADDRESS, IQS5xx_MAX_ADDRESS))
self.__address = value
self._device = i2c.get_i2c_device(value)
self._logger = logging.getLogger(
'IQS5xx.Address.{0:#0X}'.format(value))
def readUniqueID(self):
return bytesToHexString(self._device.readBytes_16BitAddress(
0xF000, 12))
def setupComplete(self):
self._device.writeByte_16BitAddress(SystemConfig0_adr, SETUP_COMPLETE,
SETUP_COMPLETE)
def setManualControl(self):
self._device.writeByte_16BitAddress(SystemConfig0_adr, MANUAL_CONTROL,
MANUAL_CONTROL)
self._device.writeByte_16BitAddress(SystemControl0_adr, 0x00,
0x07) # active mode
def setTXPinMappings(self, pinList):
assert isinstance(pinList,
list), "TX pinList must be a list of integers"
assert 0 <= len(
pinList) <= 15, "TX pinList must be between 0 and 15 long"
self._device.writeBytes_16BitAddress(TxMapping_adr, pinList)
self._device.writeByte_16BitAddress(TotalTx_adr, len(pinList))
def setRXPinMappings(self, pinList):
assert isinstance(pinList,
list), "RX pinList must be a list of integers"
assert 0 <= len(
pinList) <= 10, "RX pinList must be between 0 and 15 long"
self._device.writeBytes_16BitAddress(RxMapping_adr, pinList)
self._device.writeByte_16BitAddress(TotalRx_adr, len(pinList))
def enableChannel(self, txChannel, rxChannel, enabled):
assert 0 <= txChannel < 15, "txChannel must be less than 15"
assert 0 <= rxChannel < 10, "rxChannel must be less than 10"
registerAddy = ActiveChannels_adr + (txChannel * 2)
if rxChannel >= 8:
mask = 1 << (rxChannel - 8)
else:
registerAddy += 1
mask = 1 << rxChannel
value = mask if enabled else 0x00
self._device.writeByte_16BitAddress(registerAddy, value, mask)
def setTXRXChannelCount(self, tx_count, rx_count):
assert 0 <= txChannel <= 15, "tx_count must be less or equal tp 15"
assert 0 <= rxChannel <= 10, "rx_count must be less than or equal to 10"
self._device.writeByte_16BitAddress(TotalTx_adr, txChannel)
self._device.writeByte_16BitAddress(TotalRx_adr, rxChannel)
def swapXY(self, swapped):
value = SWITCH_XY_AXIS if swapped else 0x00
self._device.writeByte_16BitAddress(XYConfig0_adr, value,
SWITCH_XY_AXIS)
def setAtiGlobalC(self, globalC):
self._device.writeByte_16BitAddress(GlobalATIC_adr, globalC)
def setChannel_ATI_C_Adjustment(self, txChannel, rxChannel, adjustment):
assert 0 <= txChannel < 15, "txChannel must be less than 15"
assert 0 <= rxChannel < 10, "rxChannel must be less than 10"
registerAddy = ATICAdjust_adr + (txChannel * 10) + rxChannel
self._device.writeByte_16BitAddress(registerAddy, adjustment)
def setTouchMultipliers(self, set, clear):
self._device.writeByte_16BitAddress(GlobalTouchSet_adr, set)
self._device.writeByte_16BitAddress(GlobalTouchClear_adr, clear)
def rxFloat(self, floatWhenInactive):
value = RX_FLOAT if floatWhenInactive else 0x00
self._device.writeByte_16BitAddress(HardwareSettingsA_adr, value,
RX_FLOAT)
def runAtiAlgorithm(self):
self._device.writeByte_16BitAddress(SystemControl0_adr, AUTO_ATI,
AUTO_ATI)
def acknowledgeReset(self):
self._device.writeByte_16BitAddress(SystemControl0_adr, ACK_RESET,
ACK_RESET)
def atiErrorDetected(self):
reg = self._device.readByte_16BitAddress(SystemInfo0_adr)
return bool(reg & ATI_ERROR)
def reseed(self):
self._device.writeByte_16BitAddress(SystemControl0_adr, RESEED, RESEED)
def endSession(self):
self._device.writeByte_16BitAddress(EndWindow_adr, 0x00)
time.sleep(0.001)
def readVersionNumbers(self):
bytes = self._device.readBytes_16BitAddress(ProductNumber_adr, 6)
fields = struct.unpack(">HHBB", bytes)
return {
"product": fields[0],
"project": fields[1],
"major": fields[2],
"minor": fields[3]
}
def bootloaderAvailable(self):
BOOTLOADER_AVAILABLE = 0xA5
NO_BOOTLOADER = 0xEE
result = self._device.readByte_16BitAddress(BLStatus_adr)
# result = ord(result)
if result == BOOTLOADER_AVAILABLE:
return True
elif result == NO_BOOTLOADER:
return False
else:
raise ValueError(
"Unexpected value returned for bootloader status: {0:#0X}".
format(result))
def holdReset(self, millis=None):
self._resetPin.on()
if millis != None:
time.sleep(millis / 1000.0)
self.releaseReset()
def releaseReset(self):
self._resetPin.off()
def isReady(self):
return self._readypin.is_active
def waitUntilReady(self, timeout=None):
self._readypin.wait_for_active(timeout)
def updateFirmware(self, hexFilePath, newDeviceAddress=None):
hexFile = IntelHex(source=hexFilePath)
hexFile.padding = FLASH_PADDING
appBinary = hexFile.tobinarray(start=APP_START_ADDRESS,
end=NV_SETTINGS_END)
crcBinary = hexFile.tobinarray(start=CHECKSUM_DESCRIPTOR_START,
end=CHECKSUM_DESCRIPTOR_END)
if newDeviceAddress:
self._logger.debug(
"Modifying the last byte in NV settings to change Device I2C Addrress to {0:#0X}"
.format(newDeviceAddress))
if (newDeviceAddress < IQS5xx_DEFAULT_ADDRESS) or (
newDeviceAddress > IQS5xx_MAX_ADDRESS):
raise ValueError(
"Invalid I2C Address. Use something in the range [%x, %x]"
% (IQS5xx_DEFAULT_ADDRESS, IQS5xx_MAX_ADDRESS))
appBinary[-1] = newDeviceAddress
# Step 1 - Enter Bootloader
self._logger.debug("Entering Bootloader")
bootloaderAddress = 0x40 ^ self.address
bootloaderDevice = i2c.get_i2c_device(bootloaderAddress)
self.holdReset(100)
bootloaderEntered = False
for i in range(10):
try:
version = bootloaderDevice.readU16(BL_CMD_READ_VERSION,
little_endian=False)
bootloaderEntered = True
except:
pass
if not bootloaderEntered:
raise IOError("Timeout while trying to enter bootlaoder")
self._logger.debug("Bootloader entered successfully")
# Step 2 - Read and verify the bootloader version number
self._logger.debug("Reading Bootloader version")
if version != BL_VERSION:
raise Exception(
"Incompatible bootloader version detected: {0:#0X}".format(
version))
self._logger.debug("Bootloader version is compatible: 0x%02X", version)
# Step 3 - Write the new application firmware and settings
self._logger.debug("Starting to write Application and NV settings")
for blockNum in range(APP_SIZE_BLOCKS + NV_SETTINGS_SIZE_BLOCKS):
blockAddress = APP_START_ADDRESS + (blockNum * BLOCK_SIZE)
self._logger.debug(
'Writing 64-byte block {0}/{1} at address {2:#0X}'.format(
blockNum + 1, APP_SIZE_BLOCKS + NV_SETTINGS_SIZE_BLOCKS,
blockAddress))
data = bytearray(BLOCK_SIZE + 2)
data[0] = (blockAddress >> 8) & 0xFF
data[1] = blockAddress & 0xFF
data[2:] = appBinary[blockNum * BLOCK_SIZE:(blockNum + 1) *
BLOCK_SIZE]
bootloaderDevice.writeBytes(data)
time.sleep(.010) # give the device time to write to flash
# Step 4 - Write the checksum descriptor section
self._logger.debug("Writing CRC section")
blockAddress = CHECKSUM_DESCRIPTOR_START
data = bytearray(BLOCK_SIZE + 2)
data[0] = (blockAddress >> 8) & 0xFF
data[1] = blockAddress & 0xFF
data[2:] = crcBinary[0:]
bootloaderDevice.writeBytes(data)
time.sleep(0.010) # give the device time to write to flash
# Step 5 - Perform CRC and read back settins section
time.sleep(0.1)
self._logger.debug("Performing CRC calculation")
bootloaderDevice.writeRaw8(BL_CMD_RUN_CRC)
time.sleep(0.2)
crcStatus = bootloaderDevice.readRaw8()
if crcStatus != BL_CRC_PASS:
raise Exception("CRC Failure")
self._logger.debug("CRC Success")
self._logger.debug("Reading back NV settings and comparing")
for blockNum in range(NV_SETTINGS_SIZE_BLOCKS):
blockAddress = NV_SETTINGS_START + (blockNum * BLOCK_SIZE)
self._logger.debug(
'Reading 64-byte block {0}/{1} at address {2:#0X}'.format(
blockNum + 1, NV_SETTINGS_SIZE_BLOCKS, blockAddress))
data = bytearray(3)
data[0] = BL_CMD_READ_64_BYTES
data[1] = (blockAddress >> 8) & 0xFF
data[2] = blockAddress & 0xFF
reply = bootloaderDevice.writeRawListReadRawList(data, BLOCK_SIZE)
expectedReply = appBinary[(APP_SIZE_BLOCKS + blockNum) *
BLOCK_SIZE:(APP_SIZE_BLOCKS + blockNum +
1) * BLOCK_SIZE].tostring()
if reply != expectedReply:
raise Exception(
"Unexpected values while reading back NV Setting: {0} \nExpected values: {1}"
.format(bytesToHexString(reply),
bytesToHexString(expectedReply)))
self._logger.debug("NV Settings match expected values")
# Step 6 - Execute application
self._logger.debug("Execute Application")
bootloaderDevice.writeRaw8(BL_CMD_EXECUTE_APP)
if newDeviceAddress:
self.address = newDeviceAddress
class ArduinoCommClass:
"""Class for arduino IO & communication"""
def __init__(self, pin1, pin2, pin3, pin4):
"""pins for the arduino communication"""
# TODO: make the pins a function in arduino_comm which are passed from app, setup function or class
self.pin1 = pin1
self.pin2 = pin2
self.pin3 = pin3
self.pin4 = pin4
self.pin_nums = [self.pin1, self.pin2, self.pin3, self.pin4]
"""
# Direction pins
self.x_axis_positive = PWM(self.pin_nums[0])
self.x_axis_negative = PWM(self.pin_nums[1])
# Speed pins
self.y_axis_positive = PWM(self.pin_nums[2])
self.y_axis_negative = PWM(self.pin_nums[3])
"""
# TODO: make this test the actual class defined communication pins on the arduino, redundant otherwise
def comm_verify(self, pin5: int) -> None:
"""Verifies communication between arduino and computer"""
from time import sleep
# declare the receiving pin an a GPIO input device
self.recv_pin = IN_PIN(pin5)
try:
for i in self.pin_nums:
# TODO: send ping & receive ping on each of the 4 pins
print("see RoboticsCV.Image-Proc.class_test TODOs")
pin = OUT_PIN(i)
pin.on()
self.recv_pin.wait_for_active(3)
raise IOError if not self.recv_pin.value else print(
f'pin{i} communication verified')
except IOError:
print('pin communication failure')
def positional_data(self, roi: list, dim: tuple):
"""Sets the 'x' and 'depth' positions and baselines"""
"""roi is face data, baseline is dim from .shape"""
self.frame_x_position = roi[0][
0] # will update from roi, position of roi in camera frame
self.frame_depth_position = (
roi[1][0] +
roi[1][1]) / 2 # will update from roi, dimensions of roi
self.frame_x_baseline = dim[0] / 2 # 1920/2
self.frame_depth_baseline = dim[1] / 3 # 1080/3
# convert image data to motor controls
# print('you\'ve gone right') if face_data['loc']['x'] > width*3/5:
# print('you\'ve gone left') if face_data['loc']['x'] < width*2/5:
# print('you\'ve gone far away') if (face_data['dim']['w'] + face_data['dim']['h'])/2 < width/3:
# https://www.gpiozero.readthedocs.io/en/stable/source_values.html
def turning(self):
"""sends turn commands to arduino"""
"""if object is un-centered, turn left or right"""
x_delta = self.frame_x_position - self.frame_x_baseline
x_value = 0
# TODO: change these functions to have x_position_delta be turn_delta and have speed based off it
if x_delta > 50:
# turn right
print(
"write increasing x_positive_axis values to object based on passed frame value"
)
self.x_axis_positive.source(x_value) # map between 0 and 1
if x_delta < -50:
# turn left
print(
"write increasing x_negative_axis values to object based on passed frame value"
)
self.x_axis_negative.source(x_value)
def movement(self):
"""send movement commands to arduino"""
"""if frame get distance away, increase speed, vice-versa"""
depth_delta = self.frame_depth_position - self.frame_depth_baseline
depth_value = 0
if depth_delta > 50:
# go forward
print(
"write increasing y_positive_axis values to object based on passed frame size"
)
self.y_axis_positive.source(depth_value)
if depth_delta < -50:
# go backwards
print(
"write increasing y_positive_axis values to object based on passed frame size"
)
self.y_axis_negative.source(depth_value)
from gpiozero import LED, DigitalInputDevice
from time import sleep
ledLight = LED(21)
igitionInput = DigitalInputDevice(20)
sleepTime = 3.5
while True:
igitionInput.wait_for_active()
ledLight.on()
