def assignment_main(self): distance_detector = Ultrasonic_Sensor.Ultrasonic_Avoidance(35) distance = distance_detector.get_distance() driving_controller = rear_wheels.Rear_Wheels(db='config') driving_controller.ready() driving_controller.forward_with_speed(30) if (distance == 15): driving_controller.stop() time.sleep(1) driving_controller.backward_with_speed(30) time.sleep(4) driving_controller.forward_with_speed(50) if (distance == 20): driving_controller.stop() time.sleep(1) driving_controller.backward_with_speed(50) time.sleep(4) driving_controller.forward_with_speed(70) if (distance == 25): driving_controller.stop() time.sleep(1) driving_controller.backward_with_speed(70) time.sleep(4) driving_controller.stop() driving_controller.power_down() # Implement the assignment code here. pass
def assignment_main(self): direction_controller = front_wheels.Front_Wheels(db='config') distance = self.distance_detector.get_distance() direction_controller.turn_straight() time.sleep(1) driving_controller = rear_wheels.Rear_Wheels(db='config') driving_controller.ready() while distance > 15: driving_controller.forward_with_speed(30) distance = self.distance_detector.get_distance() time.sleep(0.3) driving_controller.stop() time.sleep(1) driving_controller.backward_with_speed(30) time.sleep(4) while distance > 20: driving_controller.forward_with_speed(50) distance = self.distance_detector.get_distance() time.sleep(0.3) driving_controller.stop() time.sleep(1) driving_controller.backward_with_speed(30) time.sleep(4) while distance > 25: driving_controller.forward_with_speed(70) distance = self.distance_detector.get_distance() time.sleep(0.3) driving_controller.stop() time.sleep(1) driving_controller.backward_with_speed(70) time.sleep(4) 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 = 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, 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 assignment_main(self): distance_detector = Ultrasonic_Sensor.Ultrasonic_Avoidance(35) # Example Of Front Servo Motor Control direction_controller = front_wheels.Front_Wheels(db='config') direction_controller.turn_straight() ## time.sleep(1) # Example Of Real Motor Control driving_controller = rear_wheels.Rear_Wheels(db='config') # Implement the assignment code here. driving_controller.ready() driving_controller.forward_with_speed(30) while (1): distance = distance_detector.get_distance() print(distance) time.sleep(0.1) if distance <= 25: driving_controller.stop() time.sleep(1) break driving_controller.backward_with_speed(30) time.sleep(4) driving_controller.stop() time.sleep(1) driving_controller.forward_with_speed(50) while(1): distance = distance_detector.get_distance() time.sleep(0.1) if distance <= 35: driving_controller.stop() time.sleep(1) break driving_controller.backward_with_speed(50) time.sleep(4) driving_controller.stop() time.sleep(1) driving_controller.forward_with_speed(70) while(1): distance = distance_detector.get_distance() time.sleep(0.1) if distance <= 40: driving_controller.stop() time.sleep(1) break driving_controller.backward_with_speed(70) time.sleep(4) driving_controller.stop() time.sleep(1) driving_controller.stop() driving_controller.power_down()
def assignment_main(self): sw = 0 rw = rear_wheels.Rear_Wheels() sr = Ultrasonic_Sensor.Ultrasonic_Avoidance(35) while (1): print(sr.get_distance()) if (sr.get_distance() < 30): sw = 1 time.sleep(0.01) else: sw = 0 if (sr.get_distance() < 15 and sw == 1): sw = 0 rw.stop() time.sleep(2) rw.backward_with_speed(30) time.sleep(4) break else: rw.forward_with_speed(30) while (1): print(sr.get_distance()) if (sr.get_distance() < 30): sw = 1 time.sleep(0.01) else: sw = 0 if (sr.get_distance() < 20 and sw == 1): sw = 0 rw.stop() time.sleep(2) rw.backward_with_speed(50) time.sleep(4) break else: rw.forward_with_speed(50) while (1): print(sr.get_distance()) if (sr.get_distance() < 30): sw = 1 time.sleep(0.01) else: sw = 0 if (sr.get_distance() < 25 and sw == 1): sw = 0 rw.stop() time.sleep(2) rw.backward_with_speed(70) time.sleep(4) rw.stop() break else: rw.forward_with_speed(70) #print(sr.get_distance()) # Implement the assignment code here. pass
def init_modules(self): # front_wheels PWM Driver Initialize self.fr_wheels = front_wheels.Front_Wheels(db='config') self.fr_wheels.ready() self.fr_wheels.calibration() # rear_wheels PWM Driver Initialize self.rear_wheels_drive = rear_wheels.Rear_Wheels(db='config') self.rear_wheels_drive.ready()
def init_modules(self): # PCA9685(PWM 16-ch Extension Board) Initialize PWM_Controller.PWM().setup() # front_wheels PWM Driver Initialize self.steering = front_wheels.Front_Wheels(db='config') self.steering.ready() self.steering.calibration() # rear_wheels PWM Driver Initialize self.accelerator = rear_wheels.Rear_Wheels(db='config') self.accelerator.ready()
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: self.distance_detector = Ultrasonic_Sensor.Ultrasonic_Avoidance(35) self.front_steering = front_wheels.Front_Wheels(db='config') self.front_steering.ready() self.front_steering.turn_straight() self.rear_wheels_drive = rear_wheels.Rear_Wheels(db='config') self.rear_wheels_drive.ready() except: print("moduleInitialize ERROR") self.drive_parking()
def car_startup(self): # Implement the assignment code here. distance_detector = Supersonic_Sensor.Supersonic_Sensor(35) steering = front_wheels.Front_Wheels(db='config') steering.center_alignment() accelerator = rear_wheels.Rear_Wheels(db='config') accelerator.ready() current = 0 previous = 0 start_time = time.time() accelerator.go_forward(100) while True: distance = distance_detector.get_distance() current = distance current, previous == previous, current print("Distance : ", distance) time.sleep(0.0001) if current < (previous - 25) or current > (previous + 25): current == previous if 40 < current <= 60: accelerator.go_forward(95) elif 20 < current <= 40: accelerator.go_forward(90) elif 0 < current <= 10: accelerator.stop() steering.center_alignment() time.sleep(0.1) stop_time = time.time() time.sleep(0.01) break else: continue accelerator.go_backward(75) time.sleep(stop_time - start_time) accelerator.stop() accelerator.power_down()
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): steering = front_wheels.Front_Wheels(db='config') steering.center_alignment() distance_detector = Supersonic_Sensor.Supersonic_Sensor(35) accelerator = rear_wheels.Rear_Wheels(db='config') accelerator.ready() #first driving accelerator.go_forward(30) while True: if 0 < distance_detector.get_distance() <= 17: break accelerator.stop() time.sleep(1) accelerator.go_backward(30) time.sleep(4) accelerator.stop() time.sleep(1) #second driving accelerator.go_forward(50) while True: if 0 < distance_detector.get_distance() <= 24: break accelerator.stop() time.sleep(1) accelerator.go_backward(50) time.sleep(4) accelerator.stop() time.sleep(1) #third driving accelerator.go_forward(70) while True: if 0 < distance_detector.get_distance() <= 33: break accelerator.stop() time.sleep(1) accelerator.go_backward(70) time.sleep(4) accelerator.stop() time.sleep(1) accelerator.power_down()
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 assignment_main(self): # Implement the assignment code here accelerator = rear_wheels.Rear_Wheels(db='config') accelerator.ready() aim = 15 speed = 30 for i in range(4): accelerator.go_forward(speed) while True: distance = self.distance_detector.get_distance() time.sleep(0.1) if (distance <= aim): accelerator.stop() break accelerator.go_backward(speed) time.sleep(4) accelerator.stop() aim += 5 speed += 20 accelerator.power_down()
def assignment_main(self): #================================================================= #Variable #================================================================= handle = front_wheels.Front_Wheels(db='config') #초음파 센서로 부터 거리값을 받아와 distance에 저장합니다. distance = self.distance_detector.get_distance() #뒷바퀴 오브젝트를 accelerator로 받아옵니다. accelerator = rear_wheels.Rear_Wheels(db='config') #뒷바퀴를 준비시킵니다. accelerator.ready() #시간 측정을 위해 시간을 저장합니다. start_time = time.time() #거리가 아직 먼 경우에 반복하고 너무 먼 경우 -1이 리턴 되므로 하나의 경우를 더 처리합니다. while (distance > 25 or distance < 0): #70의 속도로 전진합니다. accelerator.forward_with_speed(70) #distance값을 계속 받아옵니다. distance = self.distance_detector.get_distance() #간격을 두지 않았더니 값이 계속 튀어서 0.2초의 간격을 두었습니다. time.sleep(0.20) #에코 체킹을 위해 거리값을 출력합니다. print(distance) #만약 거리가 너무 가가까워진 경우 if (distance < 25 and distance > 0): #사용자에게 거리가 가까움을 출력합니다. print("Too close") #진행하는 동안 걸린 시간을 측정합니다, end_time = time.time() #차를 멈추고 accelerator.stop() #후진 accelerator.backward_with_speed(80) #주행하는데 걸린 시간만큼 후진합니다. time.sleep(end_time - start_time) #정지 accelerator.stop() #앞바퀴 정렬 handle.turn_straight()
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 assignment_main(self): #================================================================= #Variable #================================================================= handle = front_wheels.Front_Wheels(db='config') distance = self.distance_detector.get_distance() accelrator = rear_wheels.Rear_Wheels(db='config') self.rear_wheels_drive.ready() self.rear_wheels_drive.forward_with_speed(50) start_time = time.time() while (distance > 20): distance = self.distance_detector.get_distance() time.sleep(0.5) if (distance <= 20): print("Too close") end_time = time.time() self.rear_wheels_drive.stop() self.rear_wheels_drive.backward_with_speed(50) time.sleep(end_time - start_time)
def car_startup(self): line = SEN040134_Tracking([16, 18, 22, 40, 32]) drive = rear_wheels.Rear_Wheels(db='config') front = front_wheels.Front_Wheels() drive.ready() temp = 0 num = 0 curve = 0 test = 1 t = threading.Thread(target=sing, daemon=True) while (True): rawData = self.car.color_getter.get_raw_data() print("R", rawData[0]) print("G", rawData[1]) print("B", rawData[2]) print(test) print(line.read_digital()) distance = self.car.distance_detector.get_distance() print(distance) if test == 1: drive.go_forward(30) if line.is_equal_status([1, 0, 0, 0, 0]): temp = 70 front.turn(temp) elif line.is_equal_status([1, 1, 0, 0, 0]): temp = 75 front.turn(temp) elif line.is_equal_status([0, 1, 0, 0, 0]): temp = 80 front.turn(temp) elif line.is_equal_status([0, 1, 1, 0, 0]): temp = 85 front.turn(temp) elif line.is_equal_status([0, 0, 1, 0, 0]): temp = 90 front.turn(temp) elif line.is_equal_status([0, 0, 1, 1, 0]): temp = 95 front.turn(temp) elif line.is_equal_status([0, 0, 0, 1, 0]): temp = 100 front.turn(temp) elif line.is_equal_status([0, 0, 0, 1, 1]): temp = 105 front.turn(temp) elif line.is_equal_status([0, 0, 0, 0, 1]): temp = 110 front.turn(temp) elif line.is_equal_status([0, 0, 0, 0, 0]): front.turn(120) drive.go_backward(25) time.sleep(0.1) elif line.is_equal_status( [1, 1, 1, 1, 1]) or line.is_equal_status([1, 1, 1, 1, 0]): drive.stop() break elif line.is_equal_status([1, 1, 1, 0, 0]) and num == 0: try: t.start() except: pass front.turn(90) time.sleep(1.3) for i in range(2): front.turn(60) drive.go_backward(30) time.sleep(1.5) front.turn(120) drive.go_forward(30) time.sleep(1.3) front.turn(90) drive.go_backward(30) time.sleep(3) drive.stop() time.sleep(5) front.turn(60) drive.go_forward(30) time.sleep(0.7) num += 1 else: front.turn(temp) if 0 < distance < 8: drive.go_backward(30) time.sleep(0.2) if rawData[0] > rawData[1] and rawData[0] > rawData[2]: drive.stop() print("Stop") print("R", rawData[0]) print("G", rawData[1]) print("B", rawData[2]) time.sleep(5) elif rawData[1] > rawData[0] and rawData[1] > rawData[2]: drive.stop() drive.go_backward(20) time.sleep(0.1) drive.stop() print("Go") print("R", rawData[0]) print("G", rawData[1]) print("B", rawData[2]) front.turn(120) drive.go_backward(60) time.sleep(0.3) test = 2 time.sleep(0.1) elif test == 2: drive.go_forward(60) front.turn(60) time.sleep(0.9) front.turn(120) time.sleep(1.3) test = 1 elif test == 3: drive.stop()
from SR02 import SR02_Supersonic as Supersonic_Sensor import front_wheels import rear_wheels import time if __name__ == '__main__': try: # Example Of Front Servo Motor Control steering = front_wheels.Front_Wheels(db='config') steering.center_alignment() time.sleep(1) # Example Of Real Motor Control accelerator = rear_wheels.Rear_Wheels(db='config') accelerator.ready() accelerator.go_forward(50) time.sleep(1) accelerator.stop() time.sleep(1) accelerator.go_backward(50) time.sleep(1) accelerator.stop() accelerator.power_down() # Example of Ultrasonic Sensor distance_detector = Supersonic_Sensor.Supersonic_Sensor(35) for i in range(10): distance = distance_detector.get_distance()
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()
import time if __name__ == '__main__': try: # Example Of Front Servo Motor Control direction_controller = front_wheels.Front_Wheels(db='config') direction_controller.turn_straight() time.sleep(1) # Example Of Real Motor Control driving_controller = rear_wheels.Rear_Wheels(db='config') driving_controller.ready() driving_controller.forward_with_speed(50) time.sleep(1) driving_controller.stop() time.sleep(1) driving_controller.backward_with_speed(50) time.sleep(1) driving_controller.stop() driving_controller.power_down() # Example of Ultrasonic Sensor distance_detector = Ultrasonic_Sensor.Ultrasonic_Avoidance(35)
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 car_startup(self): # Implement the assignment code here. distance_detector = Supersonic_Sensor.Supersonic_Sensor(35) # config 파일로부터 calibration 된 값을 가져와 앞바퀴 정렬 steering = front_wheels.Front_Wheels(db='config') steering.center_alignment() # config 파일로부터 뒷바퀴 방향 값을 가져옴으로써 구동체 출발 준비 완료 accelerator = rear_wheels.Rear_Wheels(db='config') accelerator.ready() # 초음파 센서의 하드웨어적 문제를 해결하기 위해 작성한 에러처리 코드에 필요한 변수 current = 0 previous = 0 # 시간 측정 시작 start_time = time.time() # 구동체 속도 100으로 출발 accelerator.go_forward(95) while True: # 5번의 거리 측정값을 변수로 지정 measurement1 = distance_detector.get_distance() measurement2 = distance_detector.get_distance() measurement3 = distance_detector.get_distance() measurement4 = distance_detector.get_distance() measurement5 = distance_detector.get_distance() # 에러처리를 위한 코드 부분 # 거리 측정값들을 리스트에 담은 뒤 정렬함 distances = [ measurement1, measurement2, measurement3, measurement4, measurement5 ] sorted_distances = sorted(distances) # 절사평균을 구하기 위해 양 끝 값을 제거함 del sorted_distances[0] del sorted_distances[3] # 절사평균 구함 average = (sorted_distances[0] + sorted_distances[1] + sorted_distances[2]) // 3 # 에러처리 확인을 위해 측정된 거리 값 출력 print("Distance : ", average) time.sleep(0.0001) # 현재 측정된 값이 직전 측정값에 비해 30이상 차이나면 현재 측정값 대신 이전 측정값을 진짜 측정값으로 처리함 #if current < (previous - 25) or current > (previous + 25): #current == previous # 측정된 거리 값이 40에서 60 사이면 속도를 100에서 95로 줄임 if 40 < average <= 60: accelerator.go_forward(95) # 측정된 거리 값이 20에서 40 사이면 속도를 95에서 90으로 줄임 elif 20 < average <= 40: accelerator.go_forward(90) # 측정된 거리 값이 0에서 10 사이면 정지한 후 앞바퀴 정렬을 다시함 elif 0 < average <= 11: accelerator.stop() steering.center_alignment() time.sleep(0.1) stop_time = time.time() # 정지했을 때의 시간 측정 time.sleep(0.01) break # 위 과정이 끝나면 반복문을 빠져나감 # 그 외의 값들은 무시하고 반복문의 내용을 처리함 else: continue # 75의 속도로 멈춘 시간에서 출발한 시간을 뺀 만큼 후진함 accelerator.go_backward(75) time.sleep(stop_time - start_time) # 후진이 끝나면 정지한 후 종료함 accelerator.stop() accelerator.power_down()
import rear_wheels as r r_whell = r.Rear_Wheels() r_whell.ready r_whell.forward
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()
def assignment_main(self): # Implement the assignment code here. distance_detector = Ultrasonic_Sensor.Ultrasonic_Avoidance(35) front_steering = front_wheels.Front_Wheels() rear_wheels_drive = rear_wheels.Rear_Wheels() # ready for move front_steering.turn_straight() time.sleep(1) rear_wheels_drive.ready() rear_wheels_drive.stop() # 전진 직전 현재 위치에서 정확한 거리값을 연산, 단 70cm 이상일 경우 70을 리턴 def ultrasonic_init(): a = deque([]) i = 0 final = 0 while len(a) <= 3: dis = distance_detector.get_distance() if 10 < dis < 70 and dis != -1: a.append(dis) time.sleep(0.1) next = distance_detector.get_distance() if abs(a[i] - next) > 10: del a[i] else: a.append(next) i = i + 1 elif dis >= 70 or dis == -1: a.append(70) i = i + 1 time.sleep(0.1) for i in range(3): final = final + a[i] final = final / 3 return final # 속도와 타겟거리를 입력하여 실제 브레이크 시작 위치를 계산 def brake_distance(speed, target): brake = target + (speed + 5) * 0.3 return brake # 속도와 타겟거리를 입력받아 전진 def go_straight(speed, stop_target): final = ultrasonic_init() rear_wheels_drive.forward_with_speed(speed) while (1): nowdis = distance_detector.get_distance() if nowdis < final and abs(nowdis - final) < 20: final = nowdis if nowdis < stop_target: rear_wheels_drive.stop() break time.sleep(0.1) # 속도와 시간을 입력받아 후진 def back_straight(speed, move_time): rear_wheels_drive.backward_with_speed(speed) time.sleep(move_time) rear_wheels_drive.stop() # speed 30 test go_straight(30, brake_distance(30, 15)) time.sleep(1) back_straight(30, 4) time.sleep(1) # speed 50 test go_straight(50, brake_distance(50, 20)) time.sleep(1) back_straight(50, 4) time.sleep(1) # speed 70 test go_straight(70, brake_distance(70, 25)) time.sleep(1) back_straight(70, 4) rear_wheels_drive.power_down() 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): 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