def set_prescale_value(psv: int):
"""Sets the prescale register to set the PWM frequency
PWM frequency approximately equal to ``6104 / (psv + 1)``
Args:
psv (int): The target prescale register value [*range 3-255*]
"""
global pwm_freq_hertz
if psv > 0xFF:
logging.warning(
"PWM prescale value out of range (%d), setting to 255" % psv)
psv = 0xFF
if psv < 3:
logging.warning("PWM prescale value out of range (%d), setting to 3" %
psv)
psv = 3
pwm_freq_hertz = 6103.516 / (psv + 1)
logging.debug("Prescale Value: %d Actual Frequency: %4.1f Hz" %
(psv, pwm_freq_hertz))
#Prescale can only be reset when SLEEP register is set to 1
if init_okay:
set_sleep_mode()
utils.i2c_lock()
pwm_bus.write_byte_data(s.PWM_ADDRESS, PRESCALE_REG, psv)
utils.i2c_unlock()
set_normal_mode()
def set_motor_speed(motor,speed):
"""Sets the motor to given speed
Args:
motor (int): The motor driver [*range 0-3*]
speed (float): The target speed for the motor [*range -1.0 to 1.0*]
"""
global brake_states,motor_speeds
speed=check_bounds(speed)
byte = 0
brake_states[motor]=False
motor_speeds[motor]=speed
if speed == 0:
logging_mode("Setting motor %d to coast" % motor)
else:
integer_speed = get_integer_speed(speed)
if speed < 0:
logging_mode("Setting motor {} speed to {} [{}V] backwards".format(motor,integer_speed,(0.08 * integer_speed)))
byte = ( integer_speed << 2 ) + 1
else:
logging_mode("Setting motor {} speed to {} [{}V] forwards".format(motor,integer_speed,(0.08 * integer_speed)))
byte = ( integer_speed << 2 ) + 2
if init_okay:
utils.i2c_lock()
i2c.write_byte_data(s.MOTOR_ADDRESSES[motor], 0, byte)
utils.i2c_unlock()
def init_display() -> bool :
"""A function to initialise and clear the display"""
if not s.HAS_DISPLAY: return False
utils.i2c_lock()
disp.begin()
utils.i2c_unlock()
clear()
return True
def set_normal_mode():
"""Disables sleep mode on PWM driver"""
logging.debug("Setting normal mode on PCA9685 PWM driver")
if init_okay:
utils.i2c_lock()
pwm_bus.write_byte_data(s.PWM_ADDRESS, MODE1_REG, 0x20)
utils.i2c_unlock()
def update_switched_gpio_outputs():
"""Sends i2c command to set the switched outputs based on stored variables"""
if init_okay:
utils.i2c_lock()
i2c.write_byte_data(
s.USER_GPIO_ADDRESS, 0x03, (switched_output_5V << 4) +
(switched_output_12V << 5) + (motor_fault_led << 6))
utils.i2c_unlock()
def read_input_registers():
"""A function to read the state of the switches at the bottom of the YRL040 PCB
Returns:
int: 11-bit value indicating switch states.
"""
if not init_okay:
logging.error("Call to read switch registered failed")
return 0
utils.i2c_lock()
ret_val = (i2c.read_word_data(s.SWITCH_GPIO_ADDRESS, 0x00) & 0x7FF)
utils.i2c_unlock()
return ret_val
def read_user_gpio():
"""A function to read the input registers of the user GPIO Expander
Returns:
int: 16-bit value indicating user GPIO states.
"""
if not init_okay:
logging.error("Call to read user GPIO failed")
return 0
utils.i2c_lock()
ret_val = i2c.read_word_data(s.USER_GPIO_ADDRESS, 0x00)
utils.i2c_unlock()
return ret_val
def update_switch_gpio_output(new_states):
"""A function to update the output pins on the PCA9555 GPIO Expander
Args:
new_states (int): A 1-byte value [MSB=1_7, Green LED].
"""
OUTPUT_PORT = 0x03
global switch_gpio_output_byte
switch_gpio_output_byte = new_states
if init_okay:
utils.i2c_lock()
i2c.write_byte_data(s.SWITCH_GPIO_ADDRESS, OUTPUT_PORT,
switch_gpio_output_byte)
utils.i2c_unlock()
def read_int_register(register):
try:
if not init_okay:
logging.warning("Error in initialisation of YRL039")
return 0
utils.i2c_lock()
i2c_bus.write_byte(s.YRL039_ADDRESS, register)
msg = i2c_msg.read(s.YRL039_ADDRESS, 2)
i2c_bus.i2c_rdwr(msg)
utils.i2c_unlock()
vals = list(msg)
return (vals[0] << 8) + vals[1]
except OSError:
logging.error(
"Error reading register from YRL039 (address %X bus %d)" %
(s.YRL039_ADDRESS, s.YRL039_BUS))
return 0
def setup_switch_gpio():
"""An initialisation function for the PCA9555 GPIO Expander controlling the switches
Sets pins IO0_0 : IO0_7 and IO1_0 : IO1_2 as inverted inputs [ie 0V = True]
Sets pins IO1_3 : IO1_7 as outputs. IO1_3 : IO1_6 are DIP LEDs, IO1_7 is green power LED
"""
#Set input 0.0-0.7 and 1.0-1.2 as inverted inputs, 1.3-1.7 as outputs [for LEDs]
if init_okay:
utils.i2c_lock()
i2c.write_byte_data(s.SWITCH_GPIO_ADDRESS, 0x04,
0xFF) #Polarity inversion
i2c.write_byte_data(s.SWITCH_GPIO_ADDRESS, 0x05, 0x07)
i2c.write_byte_data(s.SWITCH_GPIO_ADDRESS, 0x06, 0xFF) #IO State
i2c.write_byte_data(s.SWITCH_GPIO_ADDRESS, 0x07, 0x07)
utils.i2c_unlock()
def read_adc(channel):
"""A function that reads the raw 8-bit value [8-bit] from given channel of ADC
Args:
channel (int): The ADC channel to read from [0-7]. 6 is potentiometer on YRL040.
Returns:
int: ADC value [range 0-255]
"""
channel_values = [0x88, 0xC8, 0x98, 0xD8, 0xA8, 0xE8, 0xB8, 0xF8]
if not init_okay:
logging.warning("Call to read adc ignored")
return 0
utils.i2c_lock()
adc_bus.write_byte(s.ADC_ADDRESS, channel_values[channel])
ret_val = adc_bus.read_byte(s.ADC_ADDRESS)
utils.i2c_unlock()
return ret_val
def set_left_colour_pulse(index, speed=0x04):
"""Sets the left LED to pulse, at given speed and colour
Args:
index (int): The solid_colours entry to use [range 0-8]
speed (int): The speed of the pulse [range 0-15]
"""
if index < 0 or index > len(solid_colours):
logging.warning("Requested colour outside range; ignoring.")
else:
logging.debug("Setting left LED to pulse %s" % solid_colours[index][0])
if init_okay:
utils.i2c_lock()
body_bus.write_i2c_block_data(s.RGB_LED_ADDRESS, LED_REGISTER, [
0x00, solid_colours[index][1], solid_colours[index][1], speed,
speed << 1, speed, speed, speed,
get_brightness_int(), 0x0F, 0x11
])
utils.i2c_unlock()
def animation(index):
"""Displays the given animation from ``body_animations`` list
Args:
index (int): The ``body_animations`` entry to use [range 0-8]
"""
m_start = body_animations[index][1]
b_ness = [get_brightness_int(), 0x0F, 0x11]
message = m_start + b_ness
#.append(b_ness)
logging.debug("Playing animation index %d %s %s" %
(index, message, b_ness))
if init_okay:
utils.i2c_lock()
body_bus.write_i2c_block_data(s.RGB_LED_ADDRESS, LED_REGISTER, message)
utils.i2c_unlock()
def brake_motor(motor):
"""Turns on brake mode for given motor
Brake state effectively short-circuits the motor windings, resisting motion.
This is different to coast state [*high-impendance*] which is set by calling
``set_motor_speed(motor,0)``
Args:
motor (int): The motor driver [*range 0-3*]
"""
global brake_states
brake_states[motor]=True
byte = 0x03 # IN1 & IN2 both high = brake
logging_mode("Setting motor %d to brake" % motor)
if init_okay:
utils.i2c_lock()
i2c.write_byte_data(s.MOTOR_ADDRESSES[motor], 0, byte)
utils.i2c_unlock()
def set_colour_solid(index):
"""Sets both LEDs to solid colour
Args:
index (int): The solid_colours entry to use [range 0-8]
"""
if index < 0 or index > len(solid_colours):
logging.warning("Requested colour outside range; ignoring.")
else:
logging.debug("Setting LEDs to %s" % solid_colours[index][0])
if init_okay:
utils.i2c_lock()
body_bus.write_i2c_block_data(s.RGB_LED_ADDRESS, LED_REGISTER, [
solid_colours[index][1] + solid_colours[index][2], 0x00,
solid_colours[index][1] + solid_colours[index][2], 0x44, 0x44,
0x44, 0x44, 0x44,
get_brightness_int(), brightness, 0xFF
])
utils.i2c_unlock()
def set_duty_cycle_raw(output: int, dutycycle_raw: int):
"""Sets the raw on-period value for a given PWM output
Args:
output (int): The servo output to use [*range 0-15*]
dutycycle_raw (int): The on-time period [*range 0-4095*]
"""
if output < 0 or output > 15 or dutycycle_raw < 0 or dutycycle_raw > 4095:
logging.error("PWM request outside valid range (%d,%d)" %
(output, dutycycle_raw))
register_address = 6 + (output * 4)
bytes = [0, 0, dutycycle_raw % 256, dutycycle_raw >> 8]
logging.debug("Setting PWM bytes on channel %d to:" % output)
logging.debug(bytes)
if init_okay:
utils.i2c_lock()
pwm_bus.write_i2c_block_data(s.PWM_ADDRESS, register_address, bytes)
utils.i2c_unlock()
def read_all_values():
"""Reads and stores all voltage, current and temperature readings in a single i2c request"""
global v_batt, v_pi, v_aux, i_pi, i_aux, pcb_temp
try:
if init_okay:
utils.i2c_lock()
i2c_bus.write_byte(s.YRL039_ADDRESS, 8)
msg = i2c_msg.read(s.YRL039_ADDRESS, 8)
i2c_bus.i2c_rdwr(msg)
utils.i2c_unlock()
vals = list(msg)
v_batt = 0.0172 * ((vals[0] << 2) + (vals[1] >> 6))
v_pi = 0.00539 * (((vals[1] & 0x3F) << 4) + (vals[2] >> 4))
v_aux = 0.00539 * (((vals[2] & 0x0F) << 6) + (vals[3] >> 2))
i_pi = 0.0043 * ((vals[4] << 2) + (vals[5] >> 6))
i_aux = 0.0043 * (((vals[5] & 0x3F) << 4) + (vals[6] >> 4))
pcb_temp = ((((vals[6] & 0x0F) << 6) +
(vals[7] >> 2)) - 395) * 0.171875
except OSError:
logging.error(
"Error reading register 8 from YRL039 (address %X bus %d)" %
(s.YRL039_ADDRESS, s.YRL039_BUS))
def display_image(image):
"""Function to display a 128x32 pixel PIL image on the display
The display functions use the Python Image Library (Pillow) to store the image
bitmaps that are to be displayed. All other drawing functions generate a PIL
image then call this function to display the generated image.
Args:
image (PIL image): The 128x32 PIL image
"""
if(s.HAS_DISPLAY):
if(s.DISPLAY_ROTATED): image = image.transpose(Image.ROTATE_180)
try:
utils.i2c_lock()
disp.image(image)
disp.display()
utils.i2c_unlock()
except IOError:
logging.warning("IO Error writing to OLED display")
else: logging.debug("Call to display_image ignored; display disabled")
def setup_user_gpio():
"""An initialisation function for the PCA9555 GPIO Expander for the user GPIO and motor fault detection
Sets pins IO0_0 : IO0_7 based on values from settings.py
Sets pins IO1_0 : IO1_3 as inverted inputs for motor fault detection
Sets pins IO1_4 : IO1_6 as outputs [off] for switched outputs and motor fault LED
Sets pin IO1_7 as inverted input for kill switch
"""
#Set input 0.0-0.7 and 1.0-1.2 as inverted inputs, 1.3-1.7 as outputs [for LEDs]
if init_okay:
utils.i2c_lock()
i2c.write_byte_data(s.USER_GPIO_ADDRESS, 0x02,
s.USER_GPIO_OUTPUT_STATE) #Output register [if]
i2c.write_byte_data(s.USER_GPIO_ADDRESS, 0x03, 0x40)
i2c.write_byte_data(s.USER_GPIO_ADDRESS, 0x04,
s.USER_GPIO_INVERTED) #Polarity inversion
i2c.write_byte_data(s.USER_GPIO_ADDRESS, 0x05, 0x8F)
i2c.write_byte_data(s.USER_GPIO_ADDRESS, 0x06,
s.USER_GPIO_MODE) #Mode register []
i2c.write_byte_data(s.USER_GPIO_ADDRESS, 0x07, 0x8F)
utils.i2c_unlock()
def clear():
"""A function to clear the display"""
utils.i2c_lock()
disp.clear()
disp.display()
utils.i2c_unlock()
.. moduleauthor:: James Hilder <github.com/jah128>
"""
import time, logging, subprocess, Adafruit_SSD1306, os
import yrk.settings as s, yrk.utils as utils
from PIL import Image, ImageDraw, ImageFont
OLED_BUS = s.OLED_BUS # The display is attached to bus 5, which translates to RPi4 bus i2c_12
try:
utils.i2c_lock()
disp = Adafruit_SSD1306.SSD1306_128_32(rst=None,i2c_bus=OLED_BUS)
utils.i2c_unlock()
except FileNotFoundError:
s.HAS_DISPLAY=False
s.BUS_ERROR=True
logging.error("[display.py] : Cannot access /dev/i2c-%d" % (OLED_BUS))
#Fonts [filepaths in settings.py]
try:
mono_small = ImageFont.truetype(s.SMALL_MONO,8)
small_font = ImageFont.truetype(s.SMALL_FONT, 8)
small_font_bold = ImageFont.truetype(s.SMALL_BOLD, 8)
medium_font = ImageFont.truetype(s.LARGE_FONT,16)
large_font = ImageFont.truetype(s.LARGE_FONT,24)
except OSError:
logging.error("Can't read font files");
def write_message(register, message):
if init_okay:
utils.i2c_lock()
i2c_bus.write_i2c_block_data(s.YRL039_ADDRESS, register, message)
utils.i2c_unlock()
