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)