def main():
#mp.set_start_method('spawn')
settings.setup_logger("core")
logging.info("York Robotics Kit Core Server [core.py] Started - Version %s" % (settings.VERSION_STRING))
logging.info("YRK core PID: %d " % (os.getpid()))
reset_all()
#Setup GPIO pins, interrupts
GPIO.setmode(GPIO.BCM)
GPIO.setup(settings.SWITCH_INTERRUPT_PIN,GPIO.IN,pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(settings.SWITCH_INTERRUPT_PIN , GPIO.FALLING, switch_interrupt_callback)
GPIO.setup(settings.GPIO_INTERRUPT_PIN,GPIO.IN,pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(settings.GPIO_INTERRUPT_PIN , GPIO.FALLING, gpio_interrupt_callback)
#Setup debug LED
GPIO.setup(settings.DEBUG_LED_PIN,GPIO.OUT)
#Setup audio thread
audio.setup_audio()
#Start update timer threads
start_threads()
core_loop()
logging.info("Core loop terminated")
#Set MODE1 register to 0x20 [default on state, auto-increment]
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()
#Test code
if __name__ == "__main__":
s.setup_logger("pwm")
logging.info("York Robotics Kit: Servo test code")
set_pwm_frequency(50)
#Code to sweep between 1.0ms and 2.0ms period [independant of pwm_freq] using calculate_nearest_duty_cycle_to_period and set_duty_cycle_raw
phase = 0
while (True):
for step in range(50):
if (phase == 0): dc_period = 1000 + (20 * step)
else: dc_period = 2000 - (20 * step)
if step == 49: phase = 1 - phase
raw_val = calculate_nearest_duty_cycle_to_period(0.000001 *
dc_period)
logging.info("Target period: %dms Raw:%d " %
(dc_period, raw_val))
set_duty_cycle_raw(0, raw_val)
ret_val = i2c.read_word_data(s.USER_GPIO_ADDRESS, 0x00)
utils.i2c_unlock()
return ret_val
def read_motor_fault():
return ((read_user_gpio() & 0x0F00) >> 8)
def init():
setup_user_gpio()
#Test code
if __name__ == "__main__":
s.setup_logger("gpio")
logging.info("York Robotics Kit: User GPIO test code")
try:
setup_user_gpio()
while (True):
logging.info("Motor fault LED on, 12V supply off")
set_motor_fault_led(True)
set_switched_output_12V(False)
time.sleep(0.5)
logging.info("5V supply on, motor fault LED off")
set_switched_output_5V(True)
set_motor_fault_led(False)
time.sleep(0.5)
logging.info("12V supply on, 5V supply off")
set_switched_output_12V(True)
set_switched_output_5V(False)
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
#Command line test prints out voltage, current and temperature readings [will run when yrl039.py is run directly]
if __name__ == "__main__":
s.setup_logger("power")
logging.info("York Robotics Kit: Power supply monitoring test code")
#This method sends all 6 values as a single 8-byte I2C message
read_all_values()
logging.info(
"Vin: %.2f Vpi: %.2f Vaux: %.2f Ipi: %.2f Iaux: %.2f PCB Temp: %.2f"
% (v_batt, v_pi, v_aux, i_pi, i_aux, pcb_temp))
#This method makes a 2 byte I2C request for each value
v_batt = read_battery_voltage()
v_pi = read_pi_voltage()
v_aux = read_aux_voltage()
i_pi = read_pi_current()
i_aux = read_aux_current()
pcb_temp = read_pcb_temperature()
play_audio_file(s.AUDIO_FILEPATH + "ip_address.wav")
for char in ip_string:
if char == '1': play_audio_file(s.AUDIO_FILEPATH + "1.wav")
elif char == '2': play_audio_file(s.AUDIO_FILEPATH + "2.wav")
elif char == '3': play_audio_file(s.AUDIO_FILEPATH + "3.wav")
elif char == '4': play_audio_file(s.AUDIO_FILEPATH + "4.wav")
elif char == '5': play_audio_file(s.AUDIO_FILEPATH + "5.wav")
elif char == '6': play_audio_file(s.AUDIO_FILEPATH + "6.wav")
elif char == '7': play_audio_file(s.AUDIO_FILEPATH + "7.wav")
elif char == '8': play_audio_file(s.AUDIO_FILEPATH + "8.wav")
elif char == '9': play_audio_file(s.AUDIO_FILEPATH + "9.wav")
elif char == '0': play_audio_file(s.AUDIO_FILEPATH + "0.wav")
elif char == '.': play_audio_file(s.AUDIO_FILEPATH + "dot.wav")
return IP.decode()
audio_thread = threading.Thread(target=audio_queue_thread)
audio_thread.daemon = True
#Command line test [will run when audio.py is run directly]
if __name__ == "__main__":
s.setup_logger("audio")
logging.info("YRK Audio Test")
setup_audio()
play_audio_file(s.AUDIO_FILEPATH + "york-robotics-kit.wav")
time.sleep(1.2)
say_ip()
time.sleep(10)
kill_audio()
os._exit(1)
"""A function that converts a raw ADC value to distance for Sharp IR sensor
Args:
raw_value (int): The 8-bit raw ADC value [eg from read_adc()]
sensor_model (str): The sensor model to use. '2y0a21' or '2y0a41' are
valid sensors; other options are 'voltage','raw' and 'inv_pct'
Returns:
float: Converted distance in mm based on look-up tables
"""
if (sensor_model == '2y0a21'): return DISTANCE_2Y0A21[raw_value]
if (sensor_model == '2y0a41'): return DISTANCE_2Y0A41[raw_value]
if (sensor_model == 'voltage'): return round(raw_value * 0.00977, 2)
if (sensor_model == 'raw'): return raw_value
if (sensor_model == 'inv_pct'):
return round((255 - raw_value) * 0.392157, 1)
return 0
#Test code
if __name__ == "__main__":
s.setup_logger("adc")
logging.info("York Robotics Kit: A\D Converter test code")
for channel in range(8):
raw_value = read_adc(channel)
logging.info(
f"Channel {channel} [{s.ADC_NAMES[channel]}]:{raw_value} [{s.ADC_MODELS[channel]}:{get_model_value(raw_value,s.ADC_MODELS[channel])}]"
)
os._exit(1)
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()
#Test code
if __name__ == "__main__":
s.setup_logger("motors")
logging.info("York Robotics Kit: Motor test code")
#EG: Set motor 0 to 50% for 3 seconds
set_motor_speed(0,0.5)
time.sleep(3)
stop_all_motors()
os._exit(1)
# H-Bridge Logic
#
# Motors use DRV8830 (Left = 0x62, Right = 0x60)
# Datasheet: http://www.ti.com/lit/ds/symlink/drv8830.pdf
#
# IN1 IN2 OUT1 OUT2 FUNCTION
# 0 0 Z Z Standby/coast
# 0 1 L H Reverse
# Version 0.21
# Self-test Python Script
# James Hilder, York Robotics Laboratory, Jan 2020
"""
.. module:: selftest
:synopsis: Self-test routine. Run at boot time in standard setup.
.. moduleauthor:: James Hilder <github.com/jah128>
"""
import logging, sys
import yrk.settings as settings
log_filename = "selftest"
settings.setup_logger(log_filename)
import RPi.GPIO as GPIO, subprocess, time, os
import yrk.display as display, yrk.led as led, yrk.audio as audio
import yrk.gpio as gpio, yrk.switch as switch, yrk.utils as utils
import yrk.motors as motors, yrk.pwm as pwm, yrk.adc as adc
import yrk.power as power
from subprocess import call
major_fail = False
dip_switch_state = 0
logging.info("York Robotics Kit Self Test - Ver %s" %
(settings.VERSION_STRING))
logging.info("HOSTNAME : %s" % (utils.get_hostname()))
logging.info("IP ADDRESS : %s" % (utils.get_ip()))
if settings.BUS_ERROR:
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()
#Command line test [will run when led.py is run directly]
if __name__ == "__main__":
s.setup_logger("led")
logging.info("York Robotics Kit: LED test code")
for i in range(16):
brightness = i
animation((i + 1) % 8)
time.sleep(0.5)
animation(0)
os._exit(1)
# LED Driver TI TCA6507
# http://www.ti.com/lit/ds/symlink/tca6507.pdf
# Port 0: LED_1_RED
# Port 1: LED_1_GREEN
# Port 2: LED_1_BLUE
# Port 3: LED_2_RED
# Port 4: LED_2_GREEN
Args:
state (bool): Enable or disable the LED
"""
update_switch_gpio_output((switch_gpio_output_byte & 0x78) + (state << 7))
def init():
setup_switch_gpio()
#Test code
if __name__ == "__main__":
s.setup_logger("switch")
logging.info("York Robotics Kit: Switch test code")
try:
# setup_switch_gpio()
# logging.info("Switch output:{0:016b}".format(read_input_registers()))
# set_dip_leds(read_dip_switch())
while (True):
setup_switch_gpio()
logging.info("Switch output:{0:011b}".format(
read_input_registers()))
set_dip_leds(read_dip_switch())
time.sleep(0.2)
set_power_green_led(True)
time.sleep(0.2)
set_power_green_led(False)
except IOError:
top = padding
bottom = height-padding
# Draw a black filled box to clear the image.
draw.rectangle((0,0,width,height), outline=0, fill=0)
draw.text((x, top), ("IP: %s" % utils.get_ip()), font=small_font, fill=255)
draw.text((x, top+8), ("Load: %2.1f%% MHz: %d" % (utils.get_cpu_load(), utils.get_arm_clockspeed())), font=small_font, fill=255)
draw.text((x, top+16), ("CPU: %2.1fC GPU:%2.1fC" % (utils.get_cpu_temp(),utils.get_gpu_temp())), font=small_font, fill=255)
mem = utils.get_mem_usage()
draw.text((x, top+24), ("Mem: %d/%dMB %2.2f%%" % (mem[0],mem[1],mem[2])), font=small_font, fill=255)
display_image(draw_image)
def get_ip():
"""Function to get IP address as a string using hostname system process"""
cmd = "hostname -I | cut -d\' \' -f1"
IP = subprocess.check_output(cmd, shell = True ).strip()
return IP.decode()
#Command line test [will run when display.py is run directly]
if __name__ == "__main__":
s.setup_logger("display")
logging.info("York Robotics Kit: Display Test")
init_display()
display_image_file("/home/pi/yrk/images/yrl-white.pbm")
time.sleep(0.5)
display_image_file("/home/pi/yrk/images/yrk-black.pbm")
time.sleep(0.5)
display_image_file("/home/pi/yrk/images/yrk-white.pbm")
time.sleep(4)
two_line_text_wrapped("IP Address:",get_ip())
os._exit(1)
