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