# Untitled - By: filiplagerqvist - Sat May 16 2020
## ::::::::: alligned :::::::::
# ------- ------#
def alligned():
print("alligned started")
error = 0
while(1):
img = sensor.snapshot()
K = img.find_line_segments((initX,initY, intiWidth, initHeight),merge_distance = 2, max_theta_diff = 30)
img.draw_line((319,0,319,480), color = (205, 205, 205), thickness = 2)
for t in K:
if not(t.theta() > 70 and t.theta() < 110):
img.draw_line(t.line(), color = (205, 205, 205), thickness = 2)
error = (t.x1()+t.x2())/2-320
if(error < 1 and error > -1):
return(True)
else:
print("Error: ", error)
## ::::::::: findVertL :::::::::
# ------- ------#
def findVertL():
while(1):
print("findVertL started")
img = sensor.snapshot()
HorLineList = getHorLine(img)
if(len(HorLineList) >= 3 and HorLineList[0].y1() > 280):
return(HorLineList)
## ::::::::: controlFindVertL :::::::::
# ------- ------#
def controlFindVertL(HorLineListF):
img = sensor.snapshot()
while(len(HorLineListF) > 0):
img = sensor.snapshot()
for l in HorLineListF:
img.draw_line(l.line(), color = (255, 255, 255), thickness = 2)
# ------------------------------
# --------- Hitta vertikala linjer ---------
def getHorLine(img):
print("getHorLine found")
alrd_in_list = False
N = 14
first = True
list =[]
for i in range(N):
if(i < 10):
maxd_vertical = 20
elif(i>= 10):
maxd_vertical = 7
K = img.find_line_segments((320-70, 440-i*20, 140, 40),merge_distance = 5, max_theta_diff = 30)
for t in K:
if(t.theta() < 100 and t.theta() > 70 and t.length() > 30):
if(first):
list.append(t)
first = False
else:
for p in list:
if (t.y1() > p.y1()-maxd_vertical) and (t.y1() < p.y1()+maxd_vertical):
alrd_in_list = True
if (not alrd_in_list):
list.append(t)
alrd_in_list = False
return list
def findCoordinat(XF,YF, linPXLF, offsetF):
print("findCoordinat started")
[Xi, Yi] = initCoord(linPXLF)
#contolinitCoord(Xi, Yi)
if(XF > 0):
Typ = "Right"
elif(XF<0):
Typ = "Left"
else:
return(Xi, Yi)
while(1):
N = 6
[line1A, line2A] = sideSearch(N, Typ, Xi, Yi, offsetF, YF)
if not(line1A == False and line2A == False):
#coontrolSideSearch(line1A, line2A)
lineC = [line1A, line2A]
break
else:
print("Aj då")
[x_Found, y_Found] =cross(lineC)
print(x_Found, y_Found)
drawCircle(x_Found, y_Found)
return(False)
def coontrolSideSearch(line1AF, line2AF):
while(True):
img = sensor.snapshot()
img.draw_line(line1AF.line(), color = (255, 255, 255), thickness = 2)
img.draw_line(line2AF.line(), color = (255, 255, 255), thickness = 2)
def sideSearch(NF, typF, XiF, YiF, offsetFF, YFF):
print("sideSearch started")
img = sensor.snapshot()
for q in range(NF):
#q=1
addedFLine = False
width= [40, 40, 40]
height = [35, 35, 20]
if(typF == "Right"):
K = img.find_line_segments((XiF+width[YFF-1]*(q+1), YiF-offsetFF*q, width[YFF-1], height[YFF-1]),merge_distance = 20, max_theta_diff = 30)
elif(typF == "Left"):
K = img.find_line_segments((XiF-width[YFF-1]*(q+2), YiF-offsetFF*q, width[YFF-1], height[YFF-1]),merge_distance = 20, max_theta_diff = 30)
#img = sensor.snapshot()
#print("New image", q)
#while(True):
# img = sensor.snapshot()
# print("new Image")
# for p in K:
# print(p)
# img.draw_line(p.line(), color = (255, 255, 255), thickness = 2)
for p in K:
#img.draw_line(p.line(), color = (255, 255, 255), thickness = 2)
print(p)
if not(addedFLine):
line1 = p
addedFLine = True
elif(line1.theta() < 160):
if(p.theta() > line1.theta()+25) or p.theta() < line1.theta()-25:
line2 = p
return(line1, line2)
elif(line1.theta() > 160):
if(p.theta() > 20 and p.theta() < line1.theta()-25):
line2 = p
return(line1, line2)
return(False, False)
def initCoord(linPXLF):
print("initCoord started")
iC_width = 30
iC_height = 20
Y_search = int(linPXLF - iC_height/2)
X_search = int(320 - iC_width/2)
while(1):
img = sensor.snapshot()
for i in range(3):
iC = img.find_line_segments((X_search+iC_width*i, Y_search, iC_width, iC_height),merge_distance = 5, max_theta_diff = 30)
if(len(iC) >= 2):
break
iC = img.find_line_segments((X_search-iC_width*i, Y_search, iC_width, iC_height),merge_distance = 5, max_theta_diff = 30)
if(len(iC) >= 2):
break
[x_found, y_found] = cross(iC)
return(x_found, y_found)
return(False, False)
def cross(iCF):
print("cross started")
if(iCF[0].x2()-iCF[0].x1() == 0 or\
iCF[1].x2()-iCF[1].x1() == 0):
x_cross = int((iCF[0].x1() + iCF[0].x2()+iCF[1].x1() + iCF[1].x2())/4)
y_cross = int((iCF[0].y1() + iCF[0].y2()+iCF[1].y1() + iCF[1].y2())/4)
else:
k_zero = (iCF[0].y1()-iCF[0].y2()) / (iCF[0].x1()-iCF[0].x2())
k_one = (iCF[1].y1()-iCF[1].y2()) / (iCF[1].x1()-iCF[1].x2())
if(k_one-k_zero == 0):
x_cross = int((iCF[0].x1() + iCF[0].x2()+iCF[1].x1() + iCF[1].x2())/4)
y_cross = int((iCF[0].y1() + iCF[0].y2()+iCF[1].y1() + iCF[1].y2())/4)
else:
m_zero = iCF[0].y1()-iCF[0].x1()*k_zero
m_one = iCF[0].y1()-iCF[0].x1()*k_one
x_cross = int((m_zero-m_one)/(k_one-k_zero))
y_cross = int(iCF[0].x1()*k_zero+m_zero)
return(x_cross, y_cross)
def drawCircle(X,Y):
print("drawCircle started")
while(1):
img = sensor.snapshot()
img.draw_circle((X,Y,20), color = (255, 255, 255), thickness = 2)
def contolinitCoord(XiF,YiF):
drawCircle(XiF,YiF)
#### -------------- Main ----------- ####
print("main started")
## :::::::: Setup ::::::::
import sensor, image, time
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.VGA)
sensor.skip_frames(time = 2000)
## :::::::: Bools ::::::::
centered = False
## :::::::: Numbers ::::::::
error = 0
intiWidth = 300
initHeight = 30
offsetHorSearch = [10, 7, 5]
## :::::::: Etapp 0 ::::::::
initX = int(320-intiWidth/2)
initY = int(400-initHeight/2)
## :::::::: Function calls ::::::::git
img = sensor.snapshot()
alligned()
HorList = findVertL()
#controlFindVertL(HorList)
# Pixel som ska hittas finnns på pixel linPXL i y-led
x_find = -1
y_find = 3
lineNMB = y_find-1
linPXL = HorList[lineNMB].y1()
offsetF = offsetHorSearch[lineNMB]
[X_goal, Y_goal] = findCoordinat(x_find,y_find, linPXL, offsetF)
drawCircle(X_goal, Y_goal)
print("Program executed")
# UART - By: carlbjörk - Sat May 16 2020
#Skickar data över UART till STM32
from pyb import UART
#Port P4 och P5.
uart = UART(3,19200)
#Tar in en float och skickar som en string
def send(data):
sData = str(data)
uart.write("K" + sData + "E")
#Tar emot data, under konstruktion
def get():
return uart.read()