def __init__(self, port = 13000, host = "localhost"): """ This function has two objects right now move will control the movement of the pibot com controls the communication. Basically the AI gets information from the sensors (Maybe thats a class aswell?) does its AI stuff, calls move to move the bot and calls com to update the laptop """ self.move = Move() self.room = Map([[0,0]]) self.com = Com(port = port, host = host)
def muda(task):
global i, aumenta
game.tesoura2(30 * i / 100)
game.haste2.setPos(i * 25 / 100, 0, 0)
haste1 = c.valores_esq()
game.join1.setHpr((haste1[1] - 416) * 130 / (1023 - 416), 0,
-5 + (haste1[0] - 74) * 110 / (220 - 74))
game.haste1.setP(-((haste1[2] - 16) * 270 / (1024 - 16) + haste1[4] * 20))
game.haste1.setPos(20 * haste1[3] / 392, 0, 0)
game.tesoura((haste1[5] - 71) * 30 / (113 - 71))
if aumenta == True:
i = i + 1
else:
i = i - 1
if i == 101:
aumenta = False
if i == 1:
aumenta = True
return task.again
import Com
print "Write the bianry you want to send!"
while True:
propperCommand = True
binary = raw_input(">>> ")
for x in binary:
if x == "1" or x == "0":
pass
else:
print "Please Only Type Bianary!"
propperCommand = False
break
if propperCommand and binary != "":
Com.Init()
Com.Binary(binary)
print "Sent Data"
class AI:
room = Map([])
movement = Move()
sensor = Sensor()
botAngle = 0
botPos = [0,0]
turnAngle = 15 # the angle the bot will turn to sense
'''
def mapRoom(self):
if
while not room.isMapped():
sense()
route = room.nextRoute()
for i in route:
break
break
return
'''
def __init__(self, port = 13000, host = "localhost"):
"""
This function has two objects right now
move will control the movement of the pibot
com controls the communication.
Basically the AI gets information from the sensors (Maybe thats a class aswell?)
does its AI stuff, calls move to move the bot and calls com to update the laptop
"""
self.move = Move()
self.room = Map([[0,0]])
self.com = Com(port = port, host = host)
def sense(self):
"""
This function is a temp function it just takes in input from the user
The user can send a message to the laptop or update the map (the map right now is
just a X Y coordination)
This entire function will be replaced its just for testing
"""
points = list()
for i in range(360/self.turnAngle):
if self.botAngle ==0:
points.insert(0,[self.sensor.getDistance(),self.botAngle])
elif self.botAngle == 90:
points.insert(1,[self.sensor.getDistance(),self.botAngle])
elif self.botAngle == 180:
points.insert(2,[self.sensor.getDistance(),self.botAngle])
elif self.botAngle == 270:
points.insert(3,[self.sensor.getDistance(),self.botAngle])
else:
points.append([self.sensor.getDistance(),self.botAngle])
self.movement.turn(self.turnAngle)
self.botAngle = self.botAngle+self.turnAngle
time.sleep(1)
if self.botAngle >= 360:
self.botAngle = self.botAngle-360
#self.movement.turn(self.turnAngle)
#self.botAngle = self.botAngle+self.turnAngle
#break
#if self.botAngle == 0:
# break
return points
def mapRoom90(self):
self.turnAngle = 90
while(not self.room.isMapped()):
points = self.sense()
self.botPos = self.room.updateMap(points)
self.room.fillMap90()
nextDir = self.room.nextRoute90()
if nextDir=="U":
if self.botAngle!=0:
self.movement.turn(360-self.botAngle)
self.botAngle=0
self.movement.move(MAXVIEW-20)
self.botPos[0]=self.botPos[0]-MAXVIEW+20
self.room.updatePos(self.botPos)
elif nextDir=="D":
if self.botAngle!=180:
self.movement.turn(180-self.botAngle)
self.botAngle=180
self.movement.move(MAXVIEW-20)
self.botPos[0]=self.botPos[0]+MAXVIEW-20
self.room.updatePos(self.botPos)
elif nextDir=="R":
if self.botAngle!=90:
self.movement.turn(90-self.botAngle)
self.botAngle=90
self.movement.move(MAXVIEW-20)
self.botPos[1]=self.botPos[1]+MAXVIEW-20
self.room.updatePos(self.botPos)
elif nextDir=="L":
if self.botAngle!=270:
self.movement.turn(270-self.botAngle)
self.botAngle=270
self.movement.move(MAXVIEW-20)
self.botPos[1]=self.botPos[1]-MAXVIEW+20
self.room.updatePos(self.botPos)
elif nextDir == None:
self.room.showRoom()
self.com.sendMessage("data")
time.sleep(1)
u=0
d=0
l=0
r=0
m = self.room.getMap()
for i in range(len(m)):
if m[i][self.botPos[1]]==1 and i<self.botPos[0]:
u=i
break
for i in range(len(m)):
if m[i][self.botPos[1]]==1 and i>self.botPos[0]:
d=i
break
for i in range(len(m[0])):
if m[self.botPos[0]][i]==1 and i<self.botPos[1]:
l = i
break
for i in range(len(m[0])):
if m[self.botPos[0]][i]==1 and i>self.botPos[1]:
r = i
break
print u,d,l,r
self.com.updateMap((l,r,u,d))
#self.com.updateMap(self.room.getMap())
time.sleep(5)
self.com.sendMessage("bot")
time.sleep(1)
self.com.sendBotLocation((self.botPos[0],self.botPos[1],self.botAngle))
time.sleep(1)
self.com.sendMessage("mes")
time.sleep(1)
self.com.sendMessage("Mapping Complete!")
break
#complete
'''
self.com.sendMessage("data")
time.sleep(1)
self.com.updateMap(self.room.getMap())
time.sleep(5)
self.com.sendMessage("bot")
time.sleep(1)
self.com.sendBotLocation((self.botPos[0],self.botPos[1],self.botAngle))
time.sleep(1)
'''
#print "Location: " + str(self.move.location)
'''
self.tesoura21.setH(90 + angulo / 2)
self.tesoura22.setH(90 - angulo / 2)
def muda(task):
global i, aumenta
game.tesoura2(30 * i / 100)
game.haste2.setPos(i * 25 / 100, 0, 0)
haste1 = c.valores_esq()
game.join1.setHpr((haste1[1] - 416) * 130 / (1023 - 416), 0,
-5 + (haste1[0] - 74) * 110 / (220 - 74))
game.haste1.setP(-((haste1[2] - 16) * 270 / (1024 - 16) + haste1[4] * 20))
game.haste1.setPos(20 * haste1[3] / 392, 0, 0)
game.tesoura((haste1[5] - 71) * 30 / (113 - 71))
if aumenta == True:
i = i + 1
else:
i = i - 1
if i == 101:
aumenta = False
if i == 1:
aumenta = True
return task.again
if op.com == "":
exit(1)
c.ini(op.com)
game = Window()
game.run()
oldLinesGood = lines
while True:
time.sleep(1)
# Change the location Not sure where
reading_file = open('newfile.txt', 'r')
lines = reading_file.readlines()
print lines
GoodLine = lines[len(lines) - 1]
if len(GoodLine) == 4 and len(lines) > len(oldLinesGood):
break
OldGood = GoodLine
oldLinesGood = lines
for x in GoodLine:
Expanded_Line.append(x)
#print Expanded_Line
Com.Init()
if Expanded_Line[0] + Expanded_Line[1] == "00":
DegB = "00"
print "90 Degrees"
elif Expanded_Line[0] + Expanded_Line[1] == "01":
DegB = "01"
print "180 Degrees"
elif Expanded_Line[0] + Expanded_Line[1] == "10":
DegB = "10"
print "270 Degrees"
elif Expanded_Line[0] + Expanded_Line[1] == "11":
DegB = "11"
print "360 Degrees"
else:
Code.append(0)
print "Empty"
def main():
isrec = 0
# muitmap=0
tello = Tello()
pydisplay = Pydisplay()
keyuser = Keyuser() # 键盘命令
ui = UID()
mapui = Mapui()
# if muitmap==0:
# mapui=Mapui()
# else:
# mapstack,mapsd = Pipe()
# #stack= Manager().list()
# mapshowing = Process(target=mapread, args=(mapstack,))
# mapshowing.start()
pose = Pose()
com = Com()
mapcom = Mapcom()
frame_skip = 300
# pidtuning
# if mapcom.tpid==1:
# pidimg=np.zeros((500, 512, 3), np.uint8)
# cv2.namedWindow('pidyaw')
# cv2.namedWindow('pidthro')
# cv2.namedWindow('pidpith')
# cv2.namedWindow('pidroll')
# cv2.createTrackbar('p', 'pidyaw', 0, 100, nothing)
# cv2.createTrackbar('i', 'pidyaw', 0, 100, nothing)
# cv2.createTrackbar('d', 'pidyaw', 0, 100, nothing)
# cv2.createTrackbar('down', 'pidyaw', 0, 100, nothing)
# cv2.createTrackbar('up', 'pidyaw', 0,100, nothing)
# cv2.createTrackbar('p', 'pidthro', 0, 100, nothing)
# cv2.createTrackbar('i', 'pidthro', 0, 100, nothing)
# cv2.createTrackbar('d', 'pidthro', 0, 100, nothing)
# cv2.createTrackbar('down', 'pidthro', 0, 100, nothing)
# cv2.createTrackbar('up', 'pidthro', 0,100, nothing)
# cv2.createTrackbar('p', 'pidpith', 0, 100, nothing)
# cv2.createTrackbar('i', 'pidpith', 0, 100, nothing)
# cv2.createTrackbar('d', 'pidpith', 0, 100, nothing)
# cv2.createTrackbar('down', 'pidpith', 0, 100, nothing)
# cv2.createTrackbar('up', 'pidpith', 0,100, nothing)
# cv2.createTrackbar('p', 'pidroll', 0, 100, nothing)
# cv2.createTrackbar('i', 'pidroll', 0, 100, nothing)
# cv2.createTrackbar('d', 'pidroll', 0, 100, nothing)
# cv2.createTrackbar('down', 'pidroll', 0, 100, nothing)
# cv2.createTrackbar('up', 'pidroll', 0,100, nothing)
# 录像功能
if isrec:
stack, sd = Pipe()
#stack= Manager().list()
pr = Process(target=read, args=(stack, ))
pr.start()
# try:
for frame in tello.container.decode(video=0): # 一定要用这个循环来获取才不会产生delay
if 0 < frame_skip:
frame_skip = frame_skip - 1
continue
start_time = time.time()
image2surface = numpy.array(frame.to_image()) # 做个拷贝给pygame
image = cv2.cvtColor(image2surface, cv2.COLOR_RGB2BGR)
key_list = pygame.key.get_pressed()
imageraw = image
image = cv2.resize(image, (640, 480)) # 这个太大会爆显存
userc = keyuser.usec(key_list) # 来自用户输入的命令
# userc[0 1 2 3 4 5 ]
# 是否使用openpose 四个通道 模式
if userc[4] == 0 or userc[4] == 1:
if userc[4] == 1: # 判断使用跟踪
kp, out = pose.get_kp(image)
else: # 不使用
kp = [[0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0],
[0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0],
[0, 0], [0, 0], [0, 0], [0, 0], [0, 0]]
comd = com.get_comd(kp, userc) # 接受两个数组进行判断
tello.send_comd(comd)
flight = tello.send_data() # 飞行数据
com.read_tello_data(flight) # 飞控获取数据用于判断指令
flightstate = com.get_state() # 命令状态
if userc[4] == 1: # 使用
rec = ui.show(out, kp, flightstate) # 显示并负责播放声音
else: # 不用
rec = ui.show(imageraw, 0, flightstate)
if isrec:
write(sd, rec)
# print(userc[4])
elif userc[4] == 2 or userc[4] == 3:
# if mapcom.tpid==1:
# cv2.imshow('pidyaw', pidimg)
# cv2.imshow('pidthro', pidimg)
# cv2.imshow('pidpith', pidimg)
# cv2.imshow('pidroll', pidimg)
# pid=[[cv2.getTrackbarPos('p', 'pidyaw')/10,cv2.getTrackbarPos('i', 'pidyaw')/100,cv2.getTrackbarPos('d', 'pidyaw')/10,-cv2.getTrackbarPos('down', 'pidyaw'),cv2.getTrackbarPos('up', 'pidyaw')],
# [cv2.getTrackbarPos('p', 'pidthro')/10,cv2.getTrackbarPos('i', 'pidthro')/100,cv2.getTrackbarPos('d', 'pidthro')/10,-cv2.getTrackbarPos('down', 'pidthro'),cv2.getTrackbarPos('up', 'pidthro')],
# [cv2.getTrackbarPos('p', 'pidpith')/10,cv2.getTrackbarPos('i', 'pidpith')/100,cv2.getTrackbarPos('d', 'pidpith')/10,-cv2.getTrackbarPos('down', 'pidpith'),cv2.getTrackbarPos('up', 'pidpith')],
# [cv2.getTrackbarPos('p', 'pidroll')/10,cv2.getTrackbarPos('i', 'pidroll')/100,cv2.getTrackbarPos('d', 'pidroll')/10,-cv2.getTrackbarPos('down', 'pidroll'),cv2.getTrackbarPos('up', 'pidroll')]]
data = tello.send_data()
mapcom.readflightdata(data)
# if mapcom.tpid==1:
# comd=mapcom.com(userc,pid)
# else:
comd = mapcom.com(userc)
flightstate = mapcom.send_flightdata()
tello.send_comd(comd)
checkoutmap = mapcom.checkalldone()
if checkoutmap == 1:
userc[4] = 0
keyuser.us[4] = 0
mapcom.checkdone = None
if isrec:
write(sd, imageraw)
mapui.mapshow(flightstate)
# if muitmap==0:
# mapui.mapshow(flightstate)
# else:
# mapsand(mapsd,flightstate)
pydisplay.display(image2surface, flightstate) # pygame飞行界面
# 目前对丢帧策略的理解，只要分母不要小于飞机发送回来的最大帧速率则不会产生延迟同时保证帧率
# 例子里的60是不合理的，会多丢弃一半的帧，浪费辽
if frame.time_base < 1.0 / 35:
if userc[4] == 1:
time_base = 1.0 / 35 # 使用pose稍微保守一点
else:
time_base = 1.0 / 35
else:
time_base = frame.time_base
frame_skip = int((time.time() - start_time) / time_base)
k = cv2.waitKey(1) & 0xff # 与pygame的键盘存在未知冲突
if k == 27:
pygame.display.quit()
tello.drone.quit() # 退出
break
# print(time.time()-start_time)
# except:
# print('连接超时或发生错误退出辽')
cv2.destroyAllWindows() # 关掉飞机直接退出程序
tello.drone.quit()
pygame.display.quit()
def Virage3t():
i = 0
j = 0
Angle = 0
global InvRota
global tps
global AngleCor
global CheckVirageD
global CheckVirageG
global recentrage
global SaveOneRange
degs = 0
print("Init Virage3t")
if (CheckVirageD == 1):
print("demitourdroite")
#DemitourDroite 3 temps avec gyro
#Prise de distance
while i < 50:
i = i + 1
Com.Avancer()
time.sleep(tps)
#Rotation a 90°
print("fin de prise de distance")
i = 0
while Angle < 90 + AngleCor:
Com.VirageD()
degs = Com.Gyro()
Angle = Angle + (degs)
i = i + 1
print("Angle : ", Angle)
time.sleep(tps)
print("fin virage : ", Angle)
AngleCor = 0
j = 0
Angle = 0
#Rotation Arriere a 40°
while Angle < 40:
Com.VirageArrierG()
degs = Com.Gyro()
Angle = Angle + (degs)
print("Angle : ", Angle)
time.sleep(tps)
print("fin virage : ", Angle)
CheckVirageD = 0
CheckVirageG = 1
recentrage = 1
InvRota = 1
SaveOneRange = -1
elif (CheckVirageG == 1):
print("demitourgauche")
#DemitourGauche 3 temps avec gyro
i = 0
#Prise de distance
while i < 50:
i = i + 1
Com.Avancer()
time.sleep(tps)
#Rotation a 90°
i = 0
while Angle < 90 + AngleCor:
Com.VirageG()
degs = Com.Gyro()
Angle = Angle + (degs)
i = i + 1
print("Angle : ", Angle)
time.sleep(tps)
print("fin virage : ", Angle)
Angle = 0
AngleCor = 0
#Rotation Arriere a 40°
while Angle < 40:
Com.VirageArrierD()
degs = Com.Gyro()
Angle = Angle + (degs)
i = i + 1
time.sleep(tps)
CheckVirageG = 0
CheckVirageD = 1
recentrage = 1
InvRota = 1
SaveOneRange = -1
else:
print("Erreur de Virage 3t")
def main():
i = 0
lstIndex = []
lstDistance = []
lstCalc = []
lstAlign1 = []
lstAlign2 = []
lsttemp = []
lsttemp2 = []
data = ""
datatemp = ""
Gps = ""
Gpstemp = ""
init = 1
distanceD = 0
distanceG = 0
existD = 0
existG = 0
degs = 0
j = 0
global InvRota
global CheckVirageD
global CheckVirageG
global tps
global DataDistanceD
global DataDistanceG
global AngleCor
global recentrage
global SaveOneRange
SaveOneRange = -1
DataDistanceD = 0
DataDistanceG = 0
tps = 0.02
while 1:
i = 0
existD = 0
existG = 0
#Gps = Com.Gps()
data = Com.Lidar()
#if(all((Value == 4000)for Value in data)):
#Virage3t()
if ((data != datatemp) and (init != 1)):
print("Obstacle Détecté")
del lstIndex[:], lstDistance[:], lstCalc[:], lstAlign1[:], lstAlign2[:], lsttemp[:], lsttemp2[:],
for index, val in enumerate(data):
if (data[index] != datatemp[index]):
#print(index,end=" data : ")
#print(data[index],end=' ')
#if (data[index] < 4000) :
lstIndex.append(index)
lstDistance.append(data[index])
for i, val in enumerate(lstIndex):
lstCalc.append(
round(
math.cos(math.radians(lstIndex[i])) * lstDistance[i],
-2))
#print("lstindex : ",lstIndex[i])
#print("lstdistance : ",lstDistance[i])
#print("lstcalc : ",lstCalc[i])
for j, val in enumerate(lstCalc):
for k, val in enumerate(lstCalc):
if ((lstCalc[j] == lstCalc[k]) and (j != k)
and (lstCalc[j] != 0) and (lstCalc[k] != 0)):
result = lstAlign1.index(
lstCalc[k]) if lstCalc[k] in lstAlign1 else -1
if (result == -1):
lstAlign1.append(lstCalc[k])
tmp = []
tmp.append(str(lstIndex[k]))
lstAlign2.append(tmp)
else:
result2 = lstAlign2[result].index(str(
lstIndex[k])) if str(
lstIndex[k]) in lstAlign2[result] else -1
if (result2 == -1):
lstAlign2[result].append(str(lstIndex[k]))
#print(lstCalc)
lstAlign = Com.remove_duplicates(lstAlign1)
size = 0
for j, val in enumerate(lstAlign2):
size += len(lstAlign2[j])
lsttemp.append(len(lstAlign2[j]))
if (len(lstAlign2) != 0):
average = size / len(lstAlign2)
#print ("Avg", average)
if (len(lsttemp) != 0):
Maxligne = max(lsttemp)
del lsttemp[:]
w = 1
distanceD = 0
distanceG = 0
while (w == 1):
w = 0
for j, val in enumerate(lstAlign2):
#print ("Size : ", len(lstAlign2[j]), ", IT : ", j)
if (len(lstAlign2[j]) < average):
lstAlign1.pop(j)
lstAlign2.pop(j)
w = 1
break
#print ("Del : ", )
#print("Liste Align1",lstAlign1)
#print("Liste Align2",lstAlign2)
#print("Max ligne : ",Maxligne)
lstAlign1.append(4000)
lstAlign1.append(-4000)
#Correction Trajectoire
for Value in lstAlign1:
if (Value < 0):
lsttemp.append(abs(Value))
existD = 1
elif (Value > 0):
lsttemp2.append(Value)
existG = 1
distanceD = min(lsttemp)
if (distanceD > 1000):
distanceD = 0
distanceG = min(lsttemp2)
if (distanceG > 1000):
distanceG = 0
if ((distanceD != 0) and (distanceG != 0)):
InvRota = 0
if ((distanceD == 0) and (distanceG > 0) and (distanceG < 700)):
print("NOLIGNEDROITE")
distanceD = distanceG
if (InvRota == 1):
InvRota = 2
if (SaveOneRange == -1):
SaveOneRange = distanceG
else:
distanceD = SaveOneRange
if ((distanceG == 0) and (distanceD > 0) and (distanceD < 700)):
print("NOLIGNEGAUCHE")
distanceG = distanceD
if (InvRota == 1):
InvRota = 2
if (SaveOneRange == -1):
SaveOneRange = distanceD
else:
distanceG = SaveOneRange
#print(distanceD)
#print(distanceG)
#Sequence d'instruction Motors
if ((recentrage == 1) and (Maxligne >= 12)):
recentrage = 0
if ((recentrage == 1) and (Maxligne < 12)):
print("Recentrage aprés virage")
if (CheckVirageD == 0):
i = 0
while i < 10:
Com.VirageD()
time.sleep(tps)
i += 1
elif (CheckVirageG == 0):
i = 0
while i < 10:
Com.VirageG()
time.sleep(tps)
i += 1
elif ((distanceD + distanceG) / 2 > distanceG):
print("Correction vers la droite")
i = 0
while i < 10:
Com.VirageD()
time.sleep(tps)
i += 1
AngleCor = AngleCor + 0.3
elif ((distanceD + distanceG) / 2 > distanceD):
print("Correction vers la gauche")
i = 0
while i < 10:
Com.VirageG()
time.sleep(tps)
i += 1
AngleCor = AngleCor - 0.3
elif (distanceD == DataDistanceD) and (distanceG == DataDistanceG):
print("Le robot semble au milieu")
else:
print("Probleme de correction de trajectoire")
DataDistanceD = distanceD
DataDistanceG = distanceG
print("Droite : ", distanceD)
print("Gauche : ", distanceG)
if ((distanceD == 0) and (distanceG == 0)):
print("Pas de ligne d'avancement detecté")
if (recentrage == 0):
#Inversion de la Rotation
if (InvRota == 2):
if ((CheckVirageD == 1) and (CheckVirageG == 0)):
CheckVirageD = 0
CheckVirageG = 1
elif ((CheckVirageG == 1) and (CheckVirageD == 0)):
CheckVirageD = 1
CheckVirageG = 0
#Rotation
Virage3t()
elif all((value > 90) for value in lstIndex):
print("Obstacle uniquement sur la droite")
Com.Avancer()
elif all((value < 90) for value in lstIndex):
print("Obstacle uniquement sur la gauche")
Com.Avancer()
elif all(((value > 60) and (value < 120)) for value in lstIndex):
print("Obstacle sur trajectoire")
else:
Com.Avancer()
Gpstemp = Gps
datatemp = data
#elif(Gps == Gpstemp):
#print("Robot embourbé, tentative de désembourbage")
#Com.Avancer()
#Gps = Com.Gps()
#if(Gps == Gpstemp):
#Com.Reculer()
#Gps = Com.Gps()
#if(Gps == Gpstemp):
#print("Echec du désembourbage, SOS SOS SOS")
else:
Com.Avancer()
print("Action Par Default")
init = 0
Gpstemp = Gps
datatemp = data
time.sleep(tps)
return 0
import Com
p = Com('https://habr.com/post/127584/')
