def __init__(self, carName): try: # ================================================================ # ULTRASONIC MODULE DRIVER INITIALIZE # ================================================================ self.distance_detector = Supersonic_Sensor.Supersonic_Sensor(35) # ================================================================ # TRACKING MODULE DRIVER INITIALIZE # ================================================================ self.line_detector = Tracking_Sensor.SEN040134_Tracking([16, 18, 22, 40, 32]) # ================================================================ # RGB MODULE DRIVER INITIALIZE # ================================================================ # self.color_getter = RGB_Sensor.TCS34725() # if self.color_getter.get_exception_occur(): # print("[ERRNO-101] There is a problem with RGB_Sensor(TCS34725)") # ================================================================ # DISABLE RGB MODULE INTERRUPTION # ================================================================ # self.color_getter.set_interrupt(False) # ================================================================ # PCA9685(PWM 16-ch Extension Board) MODULE WAKEUP # ================================================================ self.carEngine = PWM_Controller.PWM() self.carEngine.startup() # ================================================================ # FRONT WHEEL DRIVER SETUP # ================================================================ self.steering = front_wheels.Front_Wheels(db='config') self.steering.ready() # ================================================================ # REAR WHEEL DRIVER SETUP # ==================================================6============== self.accelerator = rear_wheels.Rear_Wheels(db='config') self.accelerator.ready() # ================================================================ # SET LIMIT OF TURNING DEGREE # =============================================================== self.steering.turning_max = 35 # ================================================================ # SET FRONT WHEEL CENTOR ALLIGNMENT # ================================================================ self.steering.center_alignment() # ================================================================ # SET APPOINTED OF CAR NAME # ================================================================ self.car_name = carName except Exception as e: print("CONTACT TO Kookmin Univ. Teaching Assistant") print("Learn more : " + e)
def car_startup(self): # implement the assignment code here distance_detector = Supersonic_Sensor.Supersonic_Sensor(35) Tracker = Tracking_Sensor.SEN040134_Tracking([16, 18, 22, 40, 32]) steering = front_wheels.Front_Wheels(db='config') steering.center_alignment() accelerator = rear_wheels.Rear_Wheels(db='config') current = 0 previous = 0 lineValue = [[1,0,0,0,0], [1,1,0,0,0], [0,1,0,0,0], [0,1,1,0,0], [0,0,1,0,0,], [0,0,1,1,0], [0,0,0,1,0], [0,0,0,1,1], [0,0,0,0,1]] lineAngle = [-35, -25, -15, -5, 0, 5, 15, 25, 35] obstacle = 0 avoidance = 0 reverse = 0 count = 0 accelerator.ready() accelerator.go_forward(60) while True: if reverse == 1: revrese = 0 accelerator.stop() steering.turn(90 + lineAngle[0]) accelerator.go_forward(60) line = Tracker.read_digital() if avoidance == 0:
def car_startup(self): # implement the assignment code here distance_detector = Supersonic_Sensor.Supersonic_Sensor(35) Tracker = Tracking_Sensor.SEN040134_Tracking([16, 18, 22, 40, 32]) steering = front_wheels.Front_Wheels(db='config') steering.center_alignment() accelerator = rear_wheels.Rear_Wheels(db='config') current = 0 previous = 0 line_case = [[1, 0, 0, 0, 0], [1, 1, 0, 0, 0], [0, 1, 0, 0, 0], [0, 1, 1, 0, 0], [ 0, 0, 1, 0, 0, ], [0, 0, 1, 1, 0], [0, 0, 0, 1, 0], [0, 0, 0, 1, 1], [0, 0, 0, 0, 1]] line_angle = [-35, -25, -15, -5, 0, 5, 15, 25, 35] obstacle = 0 accelerator.ready() accelerator.go_forward(60) while True: distance = distance_detector.get_distance() current = distance current, previous == previous, current print("Distance : ", distance) time.sleep(0.0001) if current < (previous - 30) or current > (previous + 30): current == previous if current <= 40: steering.turn_left(90 + lineAngle[0]) if Tracker.read_digital == lineValue[0]: steering.turn_right(90 + lineAngle[8]) for i in range(len(lineValue)): if Tracker.read_digital() == lineValue[i]: steering.turn_left(90 + lineAngle[i]) print("turn : {}".format(lineAngle[i])) time.sleep(0.0001) if Tracker.read_digital() == [1, 1, 1, 1, 1]: break if Tracker.read_digital() == [1, 1, 1, 1, 1]: accelerator.stop() steering.center_alignment() print("rascar stop") accelerator.power_down()
def __init__(self, car_name): #self.car = Car(car_name) self.line_detector = Tracking_Sensor.SEN040134_Tracking( [16, 18, 22, 40, 32]) self.carEngine = PWM_Controller.PWM() self.carEngine.startup() self.steering = front_wheels.Front_Wheels(db='config') self.steering.ready() self.accelerator = rear_wheels.Rear_Wheels(db='config') self.accelerator.ready() self.steering.turning_max = 35 self.steering.center_alignment()
def moduleInitialize(self): try: # ================================================================ # ULTRASONIC MODULE DRIVER INITIALIZE # ================================================================ self.distance_detector = Ultrasonic_Sensor.Ultrasonic_Avoidance(35) # ================================================================ # TRACKING MODULE DRIVER INITIALIZE # ================================================================ self.line_detector = Tracking_Sensor.SEN040134_Tracking( [16, 18, 22, 40, 32]) # ================================================================ # RGB MODULE DRIVER INITIALIZE # ================================================================ self.color_getter = RGB_Sensor.TCS34725() # ================================================================ # FRONT WHEEL DRIVER SETUP # ================================================================ self.front_steering = front_wheels.Front_Wheels(db='config') self.front_steering.ready() # ================================================================ # REAR WHEEL DRIVER SETUP # ================================================================ self.rear_wheels_drive = rear_wheels.Rear_Wheels(db='config') self.rear_wheels_drive.ready() # ================================================================ # SET LIMIT OF TURNING DEGREE # =============================================================== self.front_steering.turning_max = 35 # ================================================================ # SET FRONT WHEEL CENTOR ALLIGNMENT # ================================================================ self.front_steering.turn_straight() # ================================================================ # DISABLE RGB MODULE INTERRUPTION # ================================================================ self.color_getter.set_interrupt(False) except: print("MODULE INITIALIZE ERROR") print("CONTACT TO Kookmin Univ. Teaching Assistant")
def car_startup(self): # implement the assignment code here Tracker = Tracking_Sensor.SEN040134_Tracking([16, 18, 22, 40, 32]) steering = front_wheels.Front_Wheels(db='config') steering.center_alignment() accelerator = rear_wheels.Rear_Wheels(db='config') accelerator.ready() accelerator.go_forward(80) while Tracker.is_in_line() != False: print("line detected") time.sleep(0.0001) if Tracker.read_digital()[1] == 1: steering.turn_left(80) print("turn left : 10") time.sleep(0.0001) elif Tracker.read_digital()[3] == 1: steering.turn_right(100) print("turn right : 10") time.sleep(0.0001) elif Tracker.read_digital()[0] == 1: steering.turn_left(60) print("turn left : 30") time.sleep(0.0001) elif Tracker.read_digital()[4] == 1: steering.turn_right(120) print("turn right : 30") time.sleep(0.0001) if Tracker.read_digital() == [0, 0, 0, 0, 0]: break if Tracker.is_in_line() == False: accelerator.stop() steering.center_alignment() print("rascar stop") accelerator.power_down()
def moduleInitialize(self): try: # ================================================================ # ULTRASONIC MODULE DRIVER INITIALIZE # ================================================================ self.distance = Ultrasonic_Sensor.Ultrasonic_Avoidance(35) # ================================================================ # TRACKING MODULE DRIVER INITIALIZE # ================================================================ self.line = ts.SEN040134_Tracking([16, 18, 22, 40, 32]) # ================================================================ # FRONT WHEEL DRIVER SETUP # ================================================================ self.direction = front_wheels.Front_Wheels(db='config') self.direction.ready() # ================================================================ # REAR WHEEL DRIVER SETUP # ================================================================ self.drive = rear_wheels.Rear_Wheels(db='config') self.drive.ready() # ================================================================ # SET LIMIT OF TURNING DEGREE # =============================================================== self.direction.turning_max = 35 # ================================================================ # SET FRONT WHEEL CENTOR ALLIGNMENT # ================================================================ self.direction.turn_straight() self.getLineStatus = False except: print("MODULE INITIALIZE ERROR") print("CONTACT TO Kookmin Univ. Teaching Assistant")
def car_startup(self): # implement the assignment code here distance_detector = Supersonic_Sensor.Supersonic_Sensor(35) Tracker = Tracking_Sensor.SEN040134_Tracking([16, 18, 22, 40, 32]) steering = front_wheels.Front_Wheels(db='config') steering.center_alignment() accelerator = rear_wheels.Rear_Wheels(db='config') current_line = Tracker.read_digital() line_case = [[1,0,0,0,0], [1,1,0,0,0], [0,1,0,0,0], [0,1,1,0,0], [0,0,1,0,0], [0,0,1,1,0], [0,0,0,1,0], [0,0,0,1,1], [0,0,0,0,1]] line_angle = [-35, -25, -15, -5, 0, 5, 15, 25, 35] previous_line = [0,0,0,0,0] obstacle = 0 count = 0 accelerator.ready() accelerator.go_forward(60) while True: print(current_line) time.sleep(0.0001) distance = distance_detector.get_distance() print("Distance : ", distance) time.sleep(0.0001) if 0 < distance <= 35 and distance != -1: count += 1 if count >= 10: accelerator.stop() steering.turn(90 + line_angle[0]) accelerator.go_forward(60) time.sleep(1) accelerator.stop() time.sleep(0.01) steering.turn(90 + line_angle[8]) accelerator.go_forward(60) time.sleep(1.9) obstacle += 1 print("count obstacle : ", obstacle) obstacle = 0 while True: if current_line != [0,0,0,0,0]: break if current_line == line_case[0]: steering.turn(90 + line_angle[0]) accelerator.go_forward(60) elif current_line == line_case[1]: steering.turn(90 + line_angle[1]) accelerator.go_forward(60) elif current_line == line_case[2]: steering.turn(90 + line_angle[2]) accelerator.go_forward(60) elif current_line == line_case[3]: steering.turn(90 + line_angle[3]) accelerator.go_forward(60) elif current_line == line_case[4]: steering.turn(90 + line_angle[4]) accelerator.go_forward(60) elif current_line == line_case[5]: steering.turn(90 + line_angle[5]) accelerator.go_forward(60) elif current_line == line_case[6]: steering.turn(90 + line_angle[6]) accelerator.go_forward(60) elif current_line == line_case[7]: steering.turn(90 + line_angle[7]) accelerator.go_forward(60) elif current_line == line_case[8]: steering.turn(90 + line_angle[8]) accelerator.go_forward(60) if current_line == [0,0,0,0,0]: steering.turn_right(100) accelerator.go_backward(60) time.sleep(0.33) accelerator.stop() elif current_line == [1,1,1,1,1]: if obstacle == 2: accelerator.stop() break else: continue #accelerator.stop() #steering.center_alignment() print("rascar stop") accelerator.power_down()
from SEN040134 import SEN040134_Tracking as Tracking_Sensor import time import RPi.GPIO as GPIO if __name__ == "__main__": Tracker = Tracking_Sensor.SEN040134_Tracking([16, 18, 22, 40, 32]) try: while True: print("Current detection result: :", Tracker.read_digital()) print("The result of the driver detecting the line : ", Tracker.is_in_line()) print("The result of detecting the center of the line :", Tracker.is_center()) time.sleep(1) except KeyboardInterrupt: GPIO.cleanup()
def destroy(): rear_wheels.pwm_low() FR.turn(90) if __name__ == "__main__": try: # ======================================================================= # setup and initilaize the left motor and right motor # ======================================================================= # ULTRASONIC MODULE DRIVER INITIALIZE UA = SR02_Ultrasonic.Ultrasonic_Avoidance(35) # TRACKING MODULE DRIVER INITIALIZE LF = SEN040134_Tracking.SEN040134_Tracking([16, 18, 22, 40, 32]) # RGB MODULE DRIVER INITIALIZE RM = TCS34725_RGB.TCS34725() # FRONT WHEEL DRIVER SETUP FR = front_wheels.Front_Wheels(db='config') FR.ready() # REAR WHEEL DRIVER SETUP rear_wheels.setup(1) # IS LINE FOLLOWER FR VAR SETUP FR.turning_max = 35 # Front wheel Calibration
from SEN040134 import SEN040134_Tracking as Tracking_Sensor import rear_wheels import front_wheels # import Tracking sensor module(time module included) import time import RPi.GPIO as GPIO if __name__ == "__main__": Tracker = Tracking_Sensor.SEN040134_Tracking() steering = front_wheels() accelerator = rear_wheels() try: while True: data =Tracker.read_digital() if data == [0,0,0,0,0]: time.sleep(1) afterdata =Tracker.read_digital() if afterdata == [0,0,0,0,0]: accelerator.stop() break if Tracker.is_in_line(data) == True: accelerator.go_forward(30) if Tracker.in_center(data) == True: continue else: if data == [0,0,1,0,0]: steering.center_alignment() elif data == [1,0,0,0,0]: steering.turn_left(35)
def car_startup(self): Tracker = Tracking_Sensor.SEN040134_Tracking([16, 18, 22, 40, 32]) accelerator = rear_wheels.Rear_Wheels(db='config') accelerator.ready() steering = front_wheels.Front_Wheels(db='config') steering.ready() accelerator.forward_with_speed(20) while (True): print(" read digital : ", Tracker.read_digital()) time.sleep(0.2) if ((Tracker.read_digital() == [1, 1, 1, 0, 0])): steering.turn_left(75) time.sleep(0.2) elif (Tracker.read_digital() == [1, 0, 0, 0, 0]): steering.turn_left(55) time.sleep(0.2) elif ((Tracker.read_digital() == [1, 1, 0, 0, 0])): steering.turn_left(60) time.sleep(0.2) elif ((Tracker.read_digital() == [0, 1, 0, 0, 0])): steering.turn_left(80) time.sleep(0.2) elif ((Tracker.read_digital() == [0, 1, 1, 0, 0])): steering.turn_left(85) time.sleep(0.2) elif ((Tracker.read_digital() == [0, 0, 1, 0, 0]) or (Tracker.read_digital() == [0, 1, 0, 1, 0]) or (Tracker.read_digital() == [0, 1, 1, 1, 0])): steering.turn(90) time.sleep(0.2) elif ((Tracker.read_digital() == [0, 0, 1, 1, 0])): steering.turn_right(95) time.sleep(0.2) elif ((Tracker.read_digital() == [0, 0, 0, 1, 0])): steering.turn_right(100) time.sleep(0.2) elif ((Tracker.read_digital() == [0, 0, 0, 1, 1])): steering.turn_right(120) time.sleep(0.2) elif ((Tracker.read_digital() == [0, 0, 0, 0, 1])): steering.turn_right(125) time.sleep(0.2) elif ((Tracker.read_digital() == [0, 0, 1, 1, 1])): steering.turn_right(110) time.sleep(0.2) elif ((Tracker.read_digital() == [0, 0, 0, 0, 0])): while (True): print(Tracker.is_in_line) time.sleep(0.2) accelerator.backward_with_speed(20) # 라인벗어났을시 뒤로간다. if (Tracker.is_in_line == True): break accelerator.stop() time.sleep(0.2) accelerator.forward_with_speed(20) # 다시 앞으로 가면서실 else: steering.turn(90) time.sleep(0.2) # implement the assignment code here pass
def car_startup(self): # implement the assignment code here distance_detector = Supersonic_Sensor.Supersonic_Sensor(35) Tracker = Tracking_Sensor.SEN040134_Tracking([16, 18, 22, 40, 32]) steering = front_wheels.Front_Wheels(db='config') steering.center_alignment() accelerator = rear_wheels.Rear_Wheels(db='config') current_line = Tracker.read_digital() line_case = [[1,0,0,0,0], [1,1,0,0,0], [0,1,0,0,0], [0,1,1,0,0], [0,0,1,0,0], [0,0,1,1,0], [0,0,0,1,0], [0,0,0,1,1], [0,0,0,0,1]] line_angle = [-35, -25, -15, -5, 0, 5, 15, 25, 35] previous_line = [0,0,0,0,0] avoidance = 0 # 각 동작들의 진행 상황을 알리는 지표 obstacle = 0 # 장애물 35cm 이내로 감지 시 감지했음을 알림 goBack = 0 # 코너 도는 중 라인트래커 센서가 라인 이탈 시 후진하도록 지정 count = 0 # 지금까지 돈 바퀴 수 angle = 0 current = 0 previous = 0 accelerator.ready() accelerator.go_forward(60) while True: if goBack == 1:# 커브 회전 중 라인트래커 이탈한 경우에 필요 goBack = 0 accelerator.stop() steering.turn(90 + line_angle[0]) accelerator.go_forward(60) if avoidance == 0: distance = distance_detector.get_distance() # 초음파 센서의 값이 튀는 것에 대한 에러처리 current = distance current, previous == previous, current print("Distance : ", distance) time.sleep(0.0001) if current < (previous - 30) or current > (previous + 30): # 초음파 센서의 값이 튀는 것에 대한 에러처리 current == previous if 0 < current <= 35: obstacle = 1 print("Obstacle detected") else: pass if obstacle == 0: # 라인 주행에 필요 for i in range(len(line_case)): if current_line == line_case[i]: steering.turn(90 + line_angle[i]) previous_line = current_line if avoidance == 1: avoidance = 2 print("avoidance 1 -> 2") elif avoidance == 3: avoidance = 0 print("avoidance 3 -> 0") break elif obstacle == 1: # 장애물 회피에 필요 print("obstacle 1") steering.turn(90 + line_angle[2]) print("turn") avoidance = 1 obstacle = 0 while True: if current_line == [0,0,0,0,0]: print("Exit main track") break else: pass while True: if Tracker.is_in_line() == True: print("Line detected") break else: pass elif avoidance == 1: pass
def car_startup(self): count = 0 Tracker = Tracking_Sensor.SEN040134_Tracking([16, 18, 22, 40, 32]) accelerator = rear_wheels.Rear_Wheels(db='config') distance_detector = Supersonic_Sensor.Supersonic_Sensor(35) distance = distance_detector.get_distance() accelerator.ready() steering = front_wheels.Front_Wheels(db='config') steering.ready() accelerator.forward_with_speed(30) while(True): print(count) distance = distance_detector.get_distance() time.sleep(0.01) if((distance < 15) and (distance > 5) and (distance != 6) and (distance != 7)): print("distance", distance) accelerator.stop() time.sleep(0.1) steering.turn(90) time.sleep(0.1) accelerator.backward_with_speed(30) time.sleep(2) steering.turn_left(35) time.sleep(0.1) accelerator.forward_with_speed(30) time.sleep(1.5) steering.turn(90) time.sleep(1) steering.turn_right(135) time.sleep(3) steering.turn_right(135) while(True): if(Tracker.is_in_line() == True): steering.turn(90) time.sleep(0.1) accelerator.stop() time.sleep(0.1) accelerator.forward_with_speed(30) time.sleep(1) break; if(Tracker.read_digital() == [1,0,0,0,0]): steering.turn_left(35) time.sleep(0.1) accelerator.forward_with_speed(30) time.sleep(0.5) elif((Tracker.read_digital() == [1,1,0,0,0])): steering.turn_left(60) time.sleep(0.1) elif((Tracker.read_digital() == [0,1,0,0,0])): steering.turn_left(50) time.sleep(0.1) elif((Tracker.read_digital() == [1,1,1,0,0])): steering.turn_left(75) time.sleep(0.1) elif((Tracker.read_digital() == [0,1,1,0,0])): steering.turn_left(80) time.sleep(0.1) elif((Tracker.read_digital() == [1,1,1,1,0])): steering.turn_left(70) time.sleep(0.1) elif((Tracker.read_digital() == [1,0,0,1,0])): steering.turn_left(80) time.sleep(0.1) elif((Tracker.read_digital() == [0,0,1,0,0]) or (Tracker.read_digital() == [0,1,0,1,0]) or (Tracker.read_digital() == [0,1,1,1,0])): steering.turn(90) time.sleep(0.1) elif((Tracker.read_digital() == [0,0,0,0,1])): steering.turn_right(135) time.sleep(0.1) elif((Tracker.read_digital() == [0,0,0,1,1])): steering.turn_right(120) time.sleep(0.1) elif((Tracker.read_digital() == [0,0,1,1,1])): steering.turn_right(110) time.sleep(0.1) elif((Tracker.read_digital() == [0,1,1,1,1])): steering.turn_right(110) time.sleep(0.1) elif((Tracker.read_digital() == [0,0,1,1,0])): steering.turn_right(115) time.sleep(0.1) elif((Tracker.read_digital() == [0,0,0,1,0])): steering.turn_right(125) time.sleep(0.1) elif((Tracker.read_digital() == [0,1,0,0,1])): steering.turn_right(100) time.sleep(0.1) elif((Tracker.read_digital() == [0,0,0,0,0])): accelerator.stop() time.sleep(0.1) steering.turn(90) while(True): print("Tracker.is_in_line : ", Tracker.is_in_line()) time.sleep(0.1) accelerator.backward_with_speed(30) if(Tracker.is_in_line() == True): accelerator.stop() time.sleep(0.1) accelerator.forward_with_speed(20) break; elif((Tracker.read_digital() == [1,1,1,1,1])): count = count+1 if(count >= 3): break; accelerator.forward_with_speed(30) time.sleep(1) else: steering.turn(90) time.sleep(0.1) accelerator.stop() accelerator.power_down() # implement the assignment code here pass
def __init__(self, carName: str): file = open("driver_log.txt", "w") # ================================================================ # ULTRASONIC MODULE DRIVER INITIALIZE # ================================================================ try: self.distance_detector = Supersonic_Sensor.Supersonic_Sensor(35) file.write("ULTRASONIC MODULE DRIVER : OK\n") except: file.write("ULTRASONIC MODULE DRIVER : ERROR\n") # ================================================================ # TRACKING MODULE DRIVER INITIALIZE # ================================================================ try: self.line_detector = Tracking_Sensor.SEN040134_Tracking([16, 18, 22, 40, 32]) file.write("TRACKING MODULE DRIVER : OK\n") except: file.write("TRACKING MODULE DRIVER : ERROR\n") # ================================================================ # PCA9685(PWM 16-ch Extension Board) MODULE WAKEUP # ================================================================ try: self.carEngine = PWM_Controller.PWM() self.carEngine.startup() file.write("PCA9685 MODULE DRIVER : OK\n") except: file.write("PCA9685 MODULE WAKEUP : ERROR\n") # ================================================================ # FRONT WHEEL DRIVER SETUP # ================================================================ try: self.steering = front_wheels.Front_Wheels(db='config') self.steering.ready() file.write("FRONT WHEEL DRIVER : OK\n") except: file.write("FRONT WHEEL DRIVER : ERROR\n") # ================================================================ # REAR WHEEL DRIVER SETUP # ==================================================6============== try: self.accelerator = rear_wheels.Rear_Wheels(db='config') self.accelerator.ready() file.write("REAR WHEEL DRIVER SETUP : OK\n") except: file.write("REAR WHEEL DRIVER SETUP : ERROR\n") # ================================================================ # SET LIMIT OF TURNING DEGREE # =============================================================== self.steering.turning_max = 35 # ================================================================ # SET FRONT WHEEL CENTOR ALLIGNMENT # ================================================================ self.steering.center_alignment() # ================================================================ # SET APPOINTED OF CAR NAME # ================================================================ self.car_name = carName ''' except Exception as e: print("CONTACT TO Kookmin Univ. Teaching Assistant") print("Learn more : " + e) ''' file.close()