def point_turn(direction, speed, term):
'''포인트 턴
direction이 True이면 left, False이면 right으로 point turn
'''
car.engine(not direction, direction, speed, speed)
sleep(term)
def swing_turn(direction, speed, term):
'''스윙 턴
direction이 True이면 left, False이면 right으로 swing turn
'''
car.engine(True, True, speed * (not direction), speed * direction)
sleep(term)
def turn(count):
direction = False
if count == 1: #우회전
direction = True
elif count == 0:
return 0
time.sleep(0.2)
car.engine(direction, not direction, 35, 38) # 39, 39)
time.sleep(0.5)
timer = 0
while not (trackingModule.navigator() & 4):
car.engine(direction, not direction, 35, 38) # 39, 39)
time.sleep(0.05)
#timer+=1
#if timer==6000:
#car.engine(direction, not direction,55,55)
#time.sleep(0.1)
#timer=0
car.engine(True, True, 0, 0)
time.sleep(0.3)
while not (trackingModule.navigator() & 4):
car.engine(not direction, direction, 35, 38) #39, 39)
time.sleep(0.0075)
car.engine(True, True, 0, 0)
time.sleep(0.3)
def lineTracking(direction):
'''선을 따라 주행 comment 입력 필요
r은 내부 l는 외부주행
내부 주행 외부 주행은 서로 요구하는 led방향이 반대
reverse 여부는 바퀴 출력 순서 반대
평가 요구사함
바깥은 좌회전으로
내부는 우회전으로
즉 서로 led와 바퀴 출력 순서가 반대
둘다 안들어온다면 지그재그 주행
'''
#when input l
lSpeed = 25
rSpeed = 30
li = trackingmodule.navigator()
if direction == 'l' or direction == 'r':
if li[0] == False and li[1] == False:
lSpeed = 15
rSpeed = 35
elif li[1] == False and li[2] == False:
pass
#lSpeed=25
#rSpeed=30
elif li[2] == False and li[3] == False:
pass
#lSpeed=25
#rSpeed=30
elif li[3] == False and li[4] == False:
lSpeed = 30
rSpeed = 25
elif li[0] == False:
lSpeed = 15
rSpeed = 30
elif li[1] == False:
lSpeed = 20
rSpeed = 30
elif li[2] == False:
pass
#lSpeed=25
#rSpeed=35
elif li[3] == False:
lSpeed = 30
rSpeed = 25
elif li[4] == False:
lSpeed = 30
rSpeed = 25
if direction == 'l':
car.engine(True, True, lSpeed, rSpeed)
else:
car.engine(True, True, lSpeed, rSpeed + 1)
return True
def avoid(direction):
'''장애물 회피 comment 입력 필요'''
#dummy code
stop()
sleep(1)
#return 0
#sample code, 아마 도착시 대각선으로 닿아 있는 편이 좋을 듯
stop()
sleep(0.1)
#회전
car.engine(True, False, 30, 30)
while ultraModule.getDistance() < 25:
pass
stop()
sleep(0.1)
car.engine(True, True, 30, 30)
sleep(0.6)
stop()
sleep(0.1)
#전진
car.engine(True, True, 20, 40)
#sleep(1)
while not trackingModule.bit2():
pass
stop()
sleep(0.3)
car.engine(True, True, 30, 30)
def avoid(direction):
'''장애물 회피 comment 입력 필요'''
#dummy code
stop()
sleep(1)
#return 0
#sample code, 아마 도착시 대각선으로 닿아 있는 편이 좋을 듯
if direction == 'l':
stop()
sleep(0.1)
#회전
car.engine(True, False, 0, 30)
sleep(1)
car.engine(False, False, 10, 50) #휘어서 후진? 그냥 후진?
while not trackingModule.bitCount() >= 3:
pass
stop()
sleep(1)
car.engine(False, False, 0, 30)
while ultraModule.getDistance() < 30:
pass
sleep(0.5) #쪼끔만 더 돌자
stop()
sleep(0.1)
#전진
car.engine(True, True, 20, 60)
#sleep(1)
while not trackingModule.bit16():
pass
stop()
sleep(1)
def avoid(direction):
'''장애물 회피 comment 입력 필요'''
#dummy code
stop()
sleep(1)
return 0
#sample code, 아마 도착시 대각선으로 닿아 있는 편이 좋을 듯
turn_way = True
if direction == 'l':
turn_way = False
car.engine(True, False, 30, 30)
sleep(1)
car.engine(True, True, 30, 60)
while not trackingModule.bit8():
pass
stop()
sleep(0.1)
def lineTracking(speed):
'''선을 따라 주행 comment 입력 필요
r은 내부 l는 외부주행
내부 주행 외부 주행은 서로 요구하는 led방향이 반대
reverse 여부는 바퀴 출력 순서 반대
평가 요구사함
바깥은 좌회전으로
내부는 우회전으로
즉 서로 led와 바퀴 출력 순서가 반대
둘다 안들어온다면 지그재그 주행
'''
bit = trackingModule.navigator()
print(bit)
if bit == 0 or bit == 1 or bit == 3 or bit == 24 or bit == 16:
stop()
sleep(0.1)
whereToBack = True
if trackingModule.getBeforeBit() > 6:
whereToBack = False
#trackingModule.navigator()
point_turn(whereToBack, speed)
print(whereToBack)
counter = 0
#trackingModule.navigator()
while not trackingModule.bit4():
counter += 1
print(counter)
if counter % 1500 == 0:
point_turn(whereToBack, speed)
elif counter > 60000:
car.engine(True, True, speed, speed)
counter = 0
stop()
sleep(0.1)
car.engine(True, True, speed, speed)
#trackingModule.navigator()
return True
def lineTracking(direction):
'''선을 따라 주행 comment 입력 필요
r은 내부 l는 외부주행
내부 주행 외부 주행은 서로 요구하는 led방향이 반대
reverse 여부는 바퀴 출력 순서 반대
평가 요구사함
바깥은 좌회전으로
내부는 우회전으로
즉 서로 led와 바퀴 출력 순서가 반대
둘다 안들어온다면 지그재그 주행
'''
#일단 바깥주행먼저, 알고리즘을 수정해서 커브 bit, correct bit, 나머지는
#간단히 2번 3번 비트는 무시한다.
lSpeed = 30
rSpeed = 30
bit = not lightTrack.navigator()
if bit & 1 == True:
#print("?")
car.engine(False, True, lSpeed, rSpeed)
#sleep(0.3)
#while lightTrack.navigator()>1:
# pass
#print("??")
#stop()
#sleep(0.1)
elif 1 < bit <= 6:
#right motor faster22
car.engine(True, True, lSpeed, rSpeed + 5)
#sleep(0.5)
elif 6 < bit <= 24:
car.engine(True, True, lSpeed + 5, rSpeed)
#sleep(0.5)
else:
car.engine(True, True, lSpeed, rSpeed)
#sleep(1)
'''if bit & 1 == False:
car.engine(True, True, 0, rSpeed)
elif bit>>1 & 1 ==False:
car.engine(True, True, lSpeed, rSpeed)
elif bit>>2 & 1 ==False:
if bit>>3 & 1:
car.engine(True, True, lSpeed, rSpeed)
else:
car.engine(True, True, rSpeed, lSpeed)
elif bit>>3 & 1==False or bit>>4 & 1 ==False:
car.engine(True, True, rSpeed, lSpeed)'''
return True
def turn(count):
direction = False
if count == 1: #우회전
direction = True
elif count == 0:
return 0
time.sleep(0.5)
car.engine(direction, not direction, 36, 39)
time.sleep(0.3)
while not (trackingModule.navigator() & 4):
car.engine(direction, not direction, 36, 39)
time.sleep(0.001)
car.engine(True, True, 0, 0)
time.sleep(0.3)
while not (trackingModule.navigator() & 4):
car.engine(not direction, direction, 36, 39)
time.sleep(0.0001)
car.engine(True, True, 0, 0)
time.sleep(0.3)
def avoid(direction):
'''장애물 회피 comment 입력 필요'''
#dummy code
stop()
sleep(1)
#car.engine(True, True, 30, 30)
#return 0
#sample code, 아마 도착시 대각선으로 닿아 있는 편이 좋을 듯
turn_way = True
if direction == 'r':
turn_way = False
#swing_turn(turn_way, 40, 3)
#go_forward(20)
#sleep(1)
#swing_turn(not turn_way, 20, 3)
#go_forward(20)
#sleep(1)
#swing_turn(not turn_way, 20, 3)
#go_forward(20)
car.engine(True, False, 30, 30)
sleep(1)
car.engine(True, True, 30, 60)
while not trackingModule.bit8():
pass
stop()
sleep(0.1)
car.engine(False, True, 30, 30)
sleep(1)
def turn(count):
#time.sleep(0.5)
########
direction = False
if count == 1: #우회전
direction = True
time.sleep(0.5)
while not (trackingModule.navigator() & 4):
car.engine(direction, not direction, 36, 39)
time.sleep(0.001)
car.engine(True, True, 0, 0)
time.sleep(0.5)
while not (trackingModule.navigator() & 4):
car.engine(not direction, direction, 36, 39)
time.sleep(0.0001)
car.engine(True, True, 0, 0)
time.sleep(0.5)
########
'''while count!=0:
def go_backward(speed):
'''후진
양 모터의 방향을 False, 속도를 speed로 설정해줍니다.
'''
car.engine(False, False, speed, speed)
def go_forward(speed):
'''전진
양 모터의 방향을 True, 속도를 speed로 설정해줍니다.
'''
car.engine(True, True, speed, speed)
def lineTracking(direction):
'''선을 따라 주행 comment 입력 필요
r은 내부 l는 외부주행
내부 주행 외부 주행은 서로 요구하는 led방향이 반대
reverse 여부는 바퀴 출력 순서 반대
평가 요구사함
바깥은 좌회전으로
내부는 우회전으로
즉 서로 led와 바퀴 출력 순서가 반대
둘다 안들어온다면 지그재그 주행
'''
lSpeed = 30
rSpeed = 30
bit = trackingModule.navigator()
print(bit)
if direction == 'l':
#바깥주행
if bit == 3 or bit < 2:
car.engine(False, True, lSpeed, rSpeed)
count = 0
while not trackingModule.bit2():
count += 1
print(count)
#차의 성능이 아니라 컴퓨터의 성능에 따라 갈리기 때문에 좋은 파트는 아닌듯
if count % 1300 == 0:
car.engine(False, True, lSpeed + 10,
rSpeed) #배터리 딸려서 턴 제대로 안되면
elif count > 60000:
car.engine(True, True, lSpeed, rSpeed)
sleep(0.01)
count = 0
pass
stop()
sleep(0.1)
car.engine(True, True, lSpeed, rSpeed + 5)
elif bit == 8 or bit > 15:
car.engine(True, False, lSpeed, rSpeed)
count = 0
while not (trackingModule.bit4()):
count += 1
print(count)
if count % 3000 == 0:
car.engine(True, False, lSpeed.rSpeed) ##
if count > 60000:
car.engine(True, True, lSpeed, rSpeed) ##
sleep(0.01)
count = 0
pass
stop()
sleep(0.1)
car.engine(True, True, lSpeed, rSpeed + 5)
elif direction == 'r':
#안쪽 주행
if bit == 24 or bit == 16 or bit == 0:
car.engine(True, False, lSpeed, rSpeed)
count = 0
while not trackingModule.bit8():
count += 1
print(count)
if count % 1400 == 0:
car.engine(True, False, lSpeed, rSpeed)
elif count > 60000:
car.engine(True, True, lSpeed, rSpeed)
sleep(0.01)
count = 0
pass
stop()
sleep(0.1) #해줘야 딱 특정 비트에 닿았을 때 라는 컨디션을 조절 가능
car.engine(True, True, lSpeed + 2, rSpeed)
elif 1 <= bit <= 3:
car.engine(False, True, lSpeed, rSpeed)
count = 0
while not (trackingModule.bit4()):
count += 1
print(count)
if count % 3000 == 0:
car.engine(False, True, lSpeed, rSpeed)
elif count > 60000:
car.engine(True, True, lSpeed, rSpeed)
sleep(0.01)
count = 0
pass
stop()
sleep(0.1)
car.engine(True, True, lSpeed + 2, rSpeed)
return True
stop()
sleep(0.1)
car.engine(True, True, lSpeed + 2, rSpeed)
return True
if __name__ == "__main__":
try:
Tmode = input("choose 'l or 'r'")
dis = 13 ###수정해도 됨
#차 시동을 건다.
car.startUp()
endline = True
if Tmode == 'l':
car.engine(True, True, 30, 35)
else:
car.engine(True, True, 35, 30)
sleep(0.1)
while endline:
distance = ultraModule.getDistance() ###초음파센서
if distance < dis:
print(distance)
avoid(Tmode)
else:
endline = lineTracking(Tmode)
#차 시동을 끈다.
car.turnOff()
#ctrl + c 키로 종료한 경우
def lineTracking(direction):
'''선을 따라 주행 comment 입력 필요
r은 내부 l는 외부주행
내부 주행 외부 주행은 서로 요구하는 led방향이 반대
reverse 여부는 바퀴 출력 순서 반대
평가 요구사함
바깥은 좌회전으로
내부는 우회전으로
즉 서로 led와 바퀴 출력 순서가 반대
둘다 안들어온다면 지그재그 주행
'''
#when input l
lSpeed = 25
rSpeed = 20
li = trackingmodule.navigator()
if direction == 'l' or direction == 'r':
if li[0] == False:
lSpeed = 20
rSpeed = 25
elif li[1] == False:
lSpeed = 20
rSpeed = 25
elif li[2] == False:
if li[1] == False:
lSpeed = 20
rSpeed = 25
else:
lSpeed = 25
rSpeed = 20
elif li[3] == False:
lSpeed = 25
rSpeed = 20
#elif li[4]==False:
# lSpeed=25
# rSpeed=0
if direction == 'l':
car.engine(True, True, lSpeed, rSpeed)
else:
car.engine(True, True, lSpeed, rSpeed + 1)
return True
else:
lSpeed = 15
rSpeed = 15
counter = 0
while not li[3] == True:
li = trackingmodule.navigator()
car.engine(True, True, 15 + counter, 0)
sleep(0.1)
print(counter)
if counter < 5:
counter = counter + 1
counter = 0
while not li[4] == True:
li = trackingmodule.navigator()
car.engine(True, True, 15 + counter, 0)
sleep(0.1)
print(counter)
if counter < 10:
counter = counter + 1
counter = 0
while not li[0] == True:
li = trackingmodule.navigator()
car.engine(True, True, 0, 15 + counter)
sleep(0.1)
print(counter)
if counter < 5:
counter = counter + 1
counter = 0
while not li[1] == True:
li = trackingmodule.navigator()
car.engine(True, True, 0, 15 + counter)
sleep(0.1)
print(counter)
if counter < 10:
counter = counter + 1
if li[0] and li[1] and li[2] and li[3] and li[4]:
car.engine(True, True, 15, 15)
sleep(0.01)
return True
def go_forward(speed): car.engine(True, True, speed, speed+0.7)
return True
if __name__ == "__main__":
try:
Tmode = input("choose 'l or 'r'")
dis = 13 ###수정해도 됨
#차 시동을 건다.
car.startUp()
endline = True
#car.engine(True, True, 30, 30)
#sleep(0.1)
lSpeed = 30
rSpeed = 35
while endline:
car.engine(True, True, lSpeed, rSpeed)
while changed:
distance = ultraModule.getDistance() ###초음파센서
if distance <= dis:
avoid(Tmode)
else:
endline = lineTracking(Tmode) ###dummy로 무한루프
#차 시동을 끈다.
car.turnOff()
#ctrl + c 키로 종료한 경우
except KeyboardInterrupt:
print("강제종료 하셨습니다.")
#오류로 인한 종료 시 시동끄기 함수 호출. 없다면 오류로 프로그램이 종료된 이후에도 바퀴가 돌 수 있다.
finally:
car.turnOff()
def avoid(direction): '''장애물 회피 comment 입력 필요''' #dummy code stop() sleep(1) #return 0 #sample code, 아마 도착시 대각선으로 닿아 있는 편이 좋을 듯 stop() sleep(0.1) #회전 car.engine(True, False, 30, 30) while ultraModule.getDistance()<25: pass stop() sleep(0.1) car.engine(True, True, 30, 30) sleep(0.6) stop() sleep(0.1) #전진 car.engine(True, True, 20, 40) #sleep(1) while not trackingModule.bit8(): pass stop() sleep(0.3) count=0 car.engine(True, True, 25, 0) while not trackingModule.bit2(): count+=1 print(count) if count%1500==0: car.engine(True, True, 25, 0) elif count>60000: car.engine(True, True, 30, 30) pass stop() sleep(0.3) car.engine(True, True, 30, 30)
def lineTracking():
print("자동주행을 시작합니다.")
#전진후 좌우 교정과 판단을 동시에
while True:
count = 0
turn_finish = False
car.engine(True, True, 42, 42)
time.sleep(0.001)
bit = 2
while True:
bit = trackingModule.bitCount()
if bit > 2 or bit == 0:
while True:
if bit == 1 or bit == 0:
break
bit = trackingModule.bitCount()
break
li = trackingModule.li[len(trackingModule.li) - 1]
if li[2] == False:
car.engine(True, True, 0, 0)
time.sleep(0.05)
if li[1] == True:
while trackingModule.bit4() != True:
car.engine(False, True, 35, 35)
time.sleep(0.001)
if trackingModule.bitCount() == 0:
break
else:
while trackingModule.bit4() != True:
car.engine(True, False, 35, 35)
time.sleep(0.001)
if trackingModule.bitCount() == 0:
break
car.engine(True, True, 0, 0)
time.sleep(0.05)
car.engine(True, True, 42, 42)
print(bit, "cdcd")
#decision
count = trackingModule.where_to_go()
print(count)
for a in reversed(trackingModule.li):
print(a)
print()
turn(count)
trackingModule.li_clear()
print("자동주행을 마칩니다.")
def point_turn(direction, speed, term): car.engine(not direction, direction, speed, speed) sleep(term)
def lineTracking():
print("자동주행을 시작합니다.")
#전진후 좌우 교정과 판단을 동시에
u_check = 0
while True:
count = 0
turn_finish = False
car.engine(True, True, 42, 37)
time.sleep(0.001)
bit = 2
is_break = True
while is_break:
bit = trackingModule.bitCount()
if bit > 2 or bit == 0:
bit = trackingModule.bitCount()
while True:
bit = trackingModule.bitCount()
if bit < 3: #bit==1 or bit==0:
time.sleep(0.1)
trackingModule.bitCount()
break
break
li = trackingModule.li[len(trackingModule.li) - 1]
if li[2] == False:
car.engine(True, True, 0, 0)
time.sleep(0.05)
if li[1] == True:
while trackingModule.bit4() != True:
car.engine(False, True, 35, 35)
time.sleep(0.005)
if trackingModule.bitCount() == 0:
is_break = False
break
else:
while trackingModule.bit4() != True:
car.engine(True, False, 35, 35)
time.sleep(0.005)
if trackingModule.bitCount() == 0:
is_break = False
break
car.engine(True, True, 0, 0)
time.sleep(0.05)
car.engine(True, True, 42, 38)
#print(bit, "bit_is")
#decision
count = trackingModule.where_to_go() #u_check)
#print(count)
u_check += 1
for a in reversed(trackingModule.li):
print(a)
print()
turn(count)
trackingModule.li_clear()
#print("after clear ", trackingModule.li)
print("자동주행을 마칩니다.")
def go_backward(speed, term):
car.engine(False, False, speed, speed - 4.8)
sleep(term)
def stop():
car.engine(True, True, 0, 0)
def stop():
'''정지
차를 정지합니다. car.turnOff()를 부르면 아예 GPIO를 clean시키기 때문에 일시정지에 사용합니다.
'''
car.engine(True, True, 0, 0)
def go_backward(speed): car.engine(False, False, speed, speed-4.8)
def go_forward(speed, term):
car.engine(True, True, speed, speed - 0.4)
sleep(term)
def lineTracking():
print("자동주행을 시작합니다.")
#전진후 좌우 교정과 판단을 동시에
while True:
count = 0
turn_finish = False
car.engine(True, True, 42, 42)
time.sleep(0.001)
bit = 2
count = 0
while True:
bit = trackingModule.bitCount()
#if bit>2:
# print(bit)
if trackingModule.where_to_go() != 0:
break
'''if bit==5:
print("CC")
break
elif bit==0:
print("jj")
break
elif bit>2:
print("more go", bit)
while bit!=0:
bit=trackingModule.bitCount()
break'''
li = trackingModule.li[len(trackingModule.li) - 1]
if li[2] == False:
car.engine(True, True, 0, 0)
time.sleep(0.05)
if li[1] == True:
while trackingModule.bit4() != True:
car.engine(False, True, 35, 35)
if trackingModule.bitCount() == 0:
break
else:
while trackingModule.bit4() != True:
car.engine(True, False, 35, 35)
if trackingModule.bitCount() == 0:
break
car.engine(True, True, 0, 0)
time.sleep(0.05)
car.engine(True, True, 42, 42)
#decision
count = trackingModule.where_to_go()
print(count)
for a in reversed(trackingModule.li):
print(a)
print()
turn(count)
trackingModule.li_clear()
#once=False
print("자동주행을 마칩니다.")
import car
import trackingModule
import ultraModule
from time import sleep
if __name__ == "__main__":
try:
Tmode = input("choose 'l or 'r'")
dis = 15 ###수정해도 됨
#차 시동을 건다.
car.startUp()
if Tmode == 'l':
while True: #기능 테스트 과연 car.engine의 구조는 옳은가
#속도변환
car.engine(True, True, 30, 30)
car.engine(True, True, 30, 0)
car.engine(True, True, 0, 30)
#방향전환
car.engine(False, False, 30, 30)
car.engine(False, True, 30, 30)
car.engine(True, False, 30, 30)
elif Tmode == 'r': #engine처럼 일일이 변환할 필요 없는 True, False값이나 변화 없는 것까지 변화 하지 않는다면,
while True: #그냥 차의 속도와 방향을 따로 조절한다면?
car.setDirection(True, True)
car.setSpeed(30, 30)
car.setSpeed(30, 0)
car.setSpeed(0, 30)
car.setDirection(False, True)
