-
Notifications
You must be signed in to change notification settings - Fork 0
/
test_wthout_motor.py
106 lines (88 loc) · 2.89 KB
/
test_wthout_motor.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
from cv2 import cv2
import time
#CONFIG
from config.config_load import *
from detect_lane.two_middle.two_middile_manager import TwoMidMethodManager
from camera.camera import CameraGet
from controls.control_car import ControlCar
# from controls.car_state_dto import CarStateDTO
import pigpio
config =pigpio.pi()
#Prepair_for_running
control_car_obj = ControlCar(NORMAL_SPEED_VAL,config,all_config)
control_car_obj.prepair_run()
time.sleep(1)
#Start_motor
control_car_obj.car_state.set_motor_value(NORMAL_SPEED_VAL)
# control_car_obj.control_speed()
#Start_Camera
cam = CameraGet(CAMERA_INDEX).start()
#Proccess_detect_lane (1st Thread)
detect_lane = TwoMidMethodManager(cam.frame,all_config)
detect_lane.start()
#Check_Lane_Missing_Variable
is_missing_lane = False
#End_processing_car
def stopCar():
control_car_obj.stop()
cam.stop()
detect_lane.stop()
exit()
#__PROCESSING__
while(True):
try:
#set_key
key = cv2.waitKey(1)
if(key==ord('q')):
break
if(key==ord('p')):
print('writed')
cv2.imwrite('a.jpg',final_image)
#Set_frame
frame = cam.frame
detect_lane.frame = frame
#Set_image
final_image = detect_lane.final_image
#Set_l1
l1 = detect_lane.l1
#Set_servo
servo_value = detect_lane.servo_val
if(servo_value is None):
continue
#xu ly chay nhanh cham
if(servo_value<1300 or servo_value>1700):
control_car_obj.car_state.set_motor_value(SLOW_DOWN_VAL)
# control_car_obj.control_speed()
elif(servo_value>1300 and servo_value<1700):
control_car_obj.car_state.set_motor_value(NORMAL_SPEED_VAL)
# control_car_obj.control_speed()
if servo_value is not None:
control_car_obj.car_state.set_servo_value(servo_value)
control_car_obj.control_servo()
#Check the lane width
if(l1 is not None):
if(l1>620):
# print('no lane detect')
is_missing_lane = True
control_car_obj.car_state.set_motor_value(PAUSE_SPEED_VAL)
# control_car_obj.control_speed()
else:
if is_missing_lane:
print("Detecting lanes")
control_car_obj.car_state.set_motor_value(NORMAL_SPEED_VAL)
# control_car_obj.control_speed()
is_missing_lane = False
else:
print("Sleep")
time.sleep(0.2)
if frame is not None and final_image is not None:
cv2.imshow('frame',frame)
cv2.imshow('fn2',final_image)
#cv2.imshow('fn_2',final2)
if (cam.grabbed==False):
break
except Exception as e:
print("ERRRORR")
print(e)
stopCar()
stopCar()