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):
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 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 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 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")
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)
for i in range(10):
distance = distance_detector.get_distance()
print("Distance is ", distance)
time.sleep(1)
except KeyboardInterrupt:
back.stop()
back.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
time.sleep(1)
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
from SR02 import SR02_Ultrasonic as Ultrasonic_Sensor
if __name__=="__main__":
ultrasonic_pin = 35
detector = Ultrasonic_Sensor.Ultrasonic_Avoidance(ultrasonic_pin)
print(detector.get_distance())
