#Import memory mapping and Overlay
from pynq import MMIO
from pynq import Overlay
from time import sleep
#Load the specific overlay
overlay = Overlay('/home/xilinx/pynq/overlays/PoC/PoC_Overlay_200.bit')
#Get the memory address of the different axi intarfaces
motors_addr=(overlay.ip_dict['motor_axi_0']['phys_addr'])
neo0_addr = (overlay.ip_dict['neo_pixel_axi_0']['phys_addr'])
neo1_addr = (overlay.ip_dict['neo_pixel_axi_1']['phys_addr'])
neo2_addr = (overlay.ip_dict['neo_pixel_axi_2']['phys_addr'])
neo3_addr = (overlay.ip_dict['neo_pixel_axi_3']['phys_addr'])
uart_addr = (overlay.ip_dict['uart_axi_0']['phys_addr'])
#Create memory objects
motors = MMIO(motors_addr,0x800)
neo0=MMIO(neo0_addr,0x800)
neo1=MMIO(neo1_addr,0x800)
neo2=MMIO(neo2_addr,0x800)
neo3=MMIO(neo3_addr,0x800)
neo=[neo0,neo1,neo2,neo3]
uart = MMIO(uart_addr,0xC)
#Define motors memory offsets
MOTOR_DATA = 0x0
MOTOR_START = 0x24
#Define Neo Pixel memory offsets
NEO_NUMBER = 0x0
NEO_1 = 0x4
NEO_2 = 0x8
NEO_3 = 0xC
NEO_4 = 0x10
NEO_5 = 0x14
NEO_6 = 0x18
NEO_7 = 0x1C
NEO_8 = 0x20
NEO_9 = 0x24
NEO_10 = 0x28
NEO_11 = 0x2C
NEO_12 = 0x30
NEO_13 = 0x34
NEO_14 = 0x38
NEO_15 = 0x3C
#Define tailtales strips
TELLTALES_LEFT = [0,1]
TELLTALES_RIGHT = [2,3]
TELLTALES_UPPER = [0,2]
#Define UART memory of offsets
UART_RX = 0x0
UART_TX = 0x4
UART_STAT = 0x8
UART_CTRL = 0xC
#Define motors Data
MOTORS_START_READ = 0x100
#Define Neo Pixel color (in hex GGRRBB)
NEO_RED = 0x00FF00
NEO_BLUE = 0x0000FF
NEO_GREEN =0xFF0000
NEO_MAGENTA = 0x00FFFF
NEO_AMBER = 0x25FF00#0x45FF00
NEO_INDIGO = 0x004B82
NEO_WHITE = 0xFFFFFF
NEO_OFF = 0x000000
#Define UART status mask
UART_FIFO_RX_VALID = 0b1<<0
UART_CLEAR_RX_FIFO = 0b1<<1
#Define neo pixel strips
neopixel_colors = {
0:{
"green": [1, 15],
"backlight" : [2,3,4,5,6,7,8,9,10,11,12],
"blue": [13],
"amber": [14]
},
1:{
"amber": [1,2,3,4,5,10,11,12,13,14],
"red": [6,7,8,9]
},
2:{
"backlight": [1,2,4,5,6,7,8],
"red": [3],
"green": [9,10],
"amber":[11]
},
3:{
"amber": [1,2,3],
"backlight": [4,5,6,7,8,9,10]
}
}
num_neopixel_strips = len(neopixel_colors)
neopixels_in_strip = [0,0,0,0]
for strip in range(num_neopixel_strips):
neopixels_in_strip[strip] = sum(len(neopixels) for neopixels in neopixel_colors[strip].values())
motors_f = ['3 -3 ']#['1 +165 ','2 -90 ','3 +227 ','4 -90 ']
motors_b = ['1 -165 ','2 +90 ','3 -227 ','4 +90 ']
sweeps = 0
sleeps =[1.8,1,3,1]
num_motors = len(motors_f)
while(sweeps < 1):
for motor_f in range(num_motors):
for char in motors_f[motor_f]:
motors.write(MOTOR_DATA,ord(char))
sleep(0.1)
motors.write(MOTOR_START,MOTORS_START_READ)
sleep(0.1)
sleep(sleeps[motor_f])
# sleep(0.25)
# for motor_b in range(num_motors):
# for char in motors_b[motor_b]:
# # print(motors_b[motor][char])
# motors.write(MOTOR_DATA,ord(char))
# sleep(0.1)
# motors.write(MOTOR_START,MOTORS_START_READ)
# sleep(0.1)
# sleep(sleeps[motor_b])
# print(sweeps)
sweeps+=1
# sleep(0.25)
blink = 0
while blink < 12:
if(neo[0].read(NEO_15) == NEO_OFF):
neo[0].write(NEO_15, NEO_GREEN)
# print('green')
else:
neo[0].write(NEO_15, NEO_OFF)
sleep(0.5)
blink+=1
blink = 0
while blink < 12:
if(neo[2].read(NEO_10) == NEO_OFF):
neo[2].write(NEO_10, NEO_GREEN)
else:
neo[2].write(NEO_10, NEO_OFF)
sleep(0.5)
blink+=1
#Define functions
########################################
# Pegar programa motores aqui
def motors_3_sweeps():
motors_f = ['1 +165 ','2 -90 ','3 +227 ','4 -90 ']
motors_b = ['1 -165 ','2 +90 ','3 -227 ','4 +90 ']
sweeps = 0
sleeps =[1.15,0.75,1.5,0]
num_motors = len(motors_f)
while(sweeps < 2):
for motor_f in range(num_motors):
for char in motors_f[motor_f]:
motors.write(MOTOR_DATA,ord(char))
motors.write(MOTOR_START,MOTORS_START_READ)
sleep(sleeps[motor_f])
sleep(0.75)
for motor_b in range(num_motors):
for char in motors_b[motor_b]:
motors.write(MOTOR_DATA,ord(char))
motors.write(MOTOR_START,MOTORS_START_READ)
sleep(sleeps[motor_b])
sleep(0.75)
sweeps+=1
################################
return 0
def motors_drive_demo():
motor_init = ['']
return 0
def red_backlight():
global backlight_color
backlight_color = NEO_RED
for strip in range(num_neopixel_strips):
for NeoPixel in neopixel_colors[strip].get('backlight',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, backlight_color)
# print("red")
return 0
def green_backlight():
global backlight_color
backlight_color = NEO_GREEN
for strip in range(num_neopixel_strips):
for NeoPixel in neopixel_colors[strip].get('backlight',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, backlight_color)
# print("green")
return 0
def blue_backlight():
global backlight_color
backlight_color = NEO_BLUE
for strip in range(num_neopixel_strips):
for NeoPixel in neopixel_colors[strip].get('backlight',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, backlight_color)
# print("blue")
return 0
def magenta_backlight():
global backlight_color
backlight_color = NEO_MAGENTA
for strip in range(num_neopixel_strips):
for NeoPixel in neopixel_colors[strip].get('backlight',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, backlight_color)
# print("magenta")
return 0
def amber_backlight():
global backlight_color
backlight_color = NEO_AMBER
for strip in range(num_neopixel_strips):
for NeoPixel in neopixel_colors[strip].get('backlight',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, backlight_color)
# print("amber")
return 0
def indigo_backlight():
global backlight_color
backlight_color = NEO_INDIGO
for strip in range(num_neopixel_strips):
for NeoPixel in neopixel_colors[strip].get('backlight',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, backlight_color)
# print("indigo")
return 0
def white_backlight():
global backlight_color
backlight_color = NEO_WHITE
for strip in range(num_neopixel_strips):
for NeoPixel in neopixel_colors[strip].get('backlight',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, backlight_color)
# print("white")
return 0
def turn_off_all_leds():
for strip in range(num_neopixel_strips):
for NeoPixel in range (1,neopixels_in_strip[strip]+1):
neo[strip].write(NeoPixel*4, NEO_OFF)
return 0
def turn_on_all_leds():
global backlight_color
for strip in range(num_neopixel_strips):
for NeoPixel in neopixel_colors[strip].get('backlight',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, backlight_color)
for NeoPixel in neopixel_colors[strip].get('amber',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, NEO_AMBER)
for NeoPixel in neopixel_colors[strip].get('red',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, NEO_RED)
for NeoPixel in neopixel_colors[strip].get('green',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, NEO_GREEN)
for NeoPixel in neopixel_colors[strip].get('blue',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, NEO_BLUE)
return 0
def left_6_blink():
blink = 0
while blink < 12:
if(neo[0].read(NEO_15) == NEO_OFF):
neo[0].write(NEO_15, NEO_GREEN)
else:
neo[0].write(NEO_15, NEO_OFF)
sleep(0.5)
blink+=1
return 0
def right_6_blink():
blink = 0
while blink < 12:
if(neo[2].read(NEO_10) == NEO_OFF):
neo[2].write(NEO_10, NEO_GREEN)
else:
neo[2].write(NEO_10, NEO_OFF)
sleep(0.5)
blink+=1
return 0
def toggle_left_telltales():
for strip in TELLTALES_LEFT:
if(neo[strip].read(NEO_1)!=0x0):
for neocolor, neopixels in neopixel_colors[strip].items():
for NeoPixel in neopixels:
if ((strip == 0 and NeoPixel < 13 and neocolor != 'backlight') or strip == 1):
neo[strip].write(NeoPixel*4,NEO_OFF)
else:
for neocolor, neopixels in neopixel_colors[strip].items():
for NeoPixel in neopixels:
if neocolor == 'amber'and strip == 1:
neo[strip].write(NeoPixel*4,NEO_AMBER)
elif neocolor == 'red':
neo[strip].write(NeoPixel*4,NEO_RED)
elif (strip == 0 and NeoPixel < 13 and neocolor == 'green'):
neo[strip].write(NeoPixel*4,NEO_GREEN)
return 0
def toggle_right_telltales():
for strip in TELLTALES_RIGHT:
if((strip==2 and neo[strip].read(NEO_3)!=0x0) or (strip==3 and neo[strip].read(NEO_1)!=0x0)):
for neocolor, neopixels in neopixel_colors[strip].items():
for NeoPixel in neopixels:
if ((strip == 2 and NeoPixel < 9 and neocolor != 'backlight') or (strip == 3 and NeoPixel < 4)):
neo[strip].write(NeoPixel*4,NEO_OFF)
else:
for neocolor, neopixels in neopixel_colors[strip].items():
for NeoPixel in neopixels:
if neocolor == 'amber' and strip == 3:
neo[strip].write(NeoPixel*4,NEO_AMBER)
elif neocolor == 'red':
neo[strip].write(NeoPixel*4,NEO_RED)
return 0
def toggle_upper_telltales():
for strip in TELLTALES_UPPER:
if((strip==0 and neo[strip].read(NEO_13)!=0x0) or (strip==2 and neo[strip].read(NEO_9)!=0x0)):
for neocolor, neopixels in neopixel_colors[strip].items():
for NeoPixel in neopixels:
if ((strip == 0 and NeoPixel > 12 and neocolor != 'backlight') or (strip == 2 and NeoPixel > 8 and neocolor != 'backlight')):
neo[strip].write(NeoPixel*4,NEO_OFF)
else:
for neocolor, neopixels in neopixel_colors[strip].items():
for NeoPixel in neopixels:
if neocolor == 'amber':
neo[strip].write(NeoPixel*4,NEO_AMBER)
elif neocolor == 'blue':
neo[strip].write(NeoPixel*4,NEO_BLUE)
elif neocolor == 'green' and (strip == 2 or (strip == 0 and NeoPixel > 12)):
neo[strip].write(NeoPixel*4,NEO_GREEN)
return 0
#Define case
def Cluster_action(argument):
switcher = {
ord('0'): motors_3_sweeps,
ord('1'): motors_drive_demo,
ord('2'): red_backlight,
ord('3'): green_backlight,
ord('4'): blue_backlight,
ord('5'): magenta_backlight,
ord('6'): amber_backlight,
ord('7'): indigo_backlight,
ord('8'): white_backlight,
ord('9'): turn_off_all_leds,
ord(':'): turn_on_all_leds,
ord('A'): left_6_blink,
ord('B'): right_6_blink,
ord('C'): toggle_left_telltales,
ord('D'): toggle_right_telltales,
ord('E'): toggle_upper_telltales
}
# Get the function from switcher dictionary
func = switcher.get(argument, lambda: "Error")
# Execute the function
func()
# Turn on backlight
backlight_color = NEO_WHITE
for strip in range(num_neopixel_strips):
for NeoPixel in neopixel_colors[strip].get('backlight',[-1]):
if(NeoPixel != -1):
neo[strip].write(NeoPixel*4, backlight_color)
for strip in range(num_neopixel_strips):
neo[strip].write(NEO_NUMBER,neopixels_in_strip[strip])
while True:
uart_data_valid = uart.read(UART_STAT) & UART_FIFO_RX_VALID
while(not uart_data_valid):
uart_data_valid = uart.read(UART_STAT) & UART_FIFO_RX_VALID
uart_data = uart.read(UART_RX)
if(uart_data == ord(';')):
uart.write(UART_CTRL,UART_CLEAR_RX_FIFO)
for strip in range(num_neopixel_strips):
for NeoPixel in range (1,neopixels_in_strip[strip]+1):
neo[strip].write(NeoPixel*4, NEO_OFF)
break
else:
Cluster_action(uart_data)
Comments
Please log in or sign up to comment.