-
Notifications
You must be signed in to change notification settings - Fork 0
/
toMove.py
133 lines (119 loc) · 4.11 KB
/
toMove.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
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
import rospy
from std_msgs.msg import String,Int
import RPi.GPIO as GPIO
import time
IrRight=1
IrLeft=1
IrMid=1
d=1000
# declare IR sensor Pins
IrRight_pin = 14
IrLeft_pin= 15
IrMid_pin= 18
#declare Ultrasonic pins
GPIO_TRIGGER = 23
GPIO_ECHO = 24
# GPIO setup
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(IrRight_pin, GPIO.IN)
GPIO.setup(IrLeft_pin, GPIO.IN)
GPIO.setup(IrMid_pin, GPIO.IN)
GPIO.setup(GPIO_TRIGGER, GPIO.OUT)
GPIO.setup(GPIO_ECHO, GPIO.IN)
def distance():
# set Trigger to HIGH
GPIO.output(GPIO_TRIGGER, True)
# set Trigger after 0.01ms to LOW
time.sleep(0.00001)
GPIO.output(GPIO_TRIGGER, False)
StartTime = time.time()
StopTime = time.time()
# save StartTime
while GPIO.input(GPIO_ECHO) == 0:
StartTime = time.time()
# save time of arrival
while GPIO.input(GPIO_ECHO) == 1:
StopTime = time.time()
# time difference between start and arrival
TimeElapsed = StopTime - StartTime
# multiply with the sonic speed (34300 cm/s)
# and divide by 2, because there and back
distance = (TimeElapsed * 34300) / 2
return distance
def Walking():
#make it for Raspberry pi
#Motor_states = 2 right wheel will turn
#Motor_states = 3 left wheel will turn
while True:
Motor_states=1
#if(IrMid==0 and IrRight==1 and IrLeft==1):
# Motor_states = 1
IrRight=GPIO.input(IrRight_pin)
IrLeft=GPIO.input(IrLeft_pin)
IrMid=GPIO.input(IrMid_pin)
d=distance()
print('IR1:',IrRight)
if(IrMid==0 and IrRight==1 and IrLeft==0):
Motor_states = 2
elif(IrMid==0 and IrRight==0 and IrLeft==1):
Motor_states = 3
elif(IrMid==0 and IrRight==0 and IrLeft==0):
Motor_states=0
Motors_pub.publish(int(Motor_states))
break
Motors_pub.publish(int(Motor_states))
if __name__ == '__main__':
Speaker_pub = rospy.Publisher('chatter', String, queue_size=10)
Motors_pub = rospy.Publisher('Motors_States', Int, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
try:
while True:
success, img=cap.read()
###########################################
Walking()
###########################################
#HumanDetection()
###########################################
#QRCodeDetect(img,n)
""" for qrcode in decode(img):
qrdata=qrcode.data.decode('utf-8')
print(qrdata)
points=np.array([qrcode.polygon],np.int32)
points=points.reshape((-1,1,2))
cv2.polylines(img,[points],True,(0,255,0),5)
rect_points=qrcode.rect
cv2.putText(img,qrdata,(rect_points[0],rect_points[1]),cv2.FONT_HERSHEY_SIMPLEX,0.9,(0,255,0),2)
if(qrdata!=lastprint):
lastprint=qrdata
Speaker_pub.publish(lastprint)
rate.sleep()
"""
cv2.imshow('Following Camera',img)
cv2.waitKey(1)
except KeyboardInterrupt:
GPIO.cleanup()
while True:
ret, frame=cap.read()
frame=cv2.rotate(frame,cv2.ROTATE_90_COUNTERCLOCKWISE)
if(qrCounter==average_pass):
break
for qrcode in decode(frame):
qrdata=qrcode.data.decode('utf-8')
#qrdata=qrdata[5]
print(qrdata)
if qrdata in myDataList:
points=np.array([qrcode.polygon],np.int32)
points=points.reshape((-1,1,2))
cv2.polylines(frame,[points],True,(0,255,0),5)
rect_points=qrcode.rect
cv2.putText(frame,qrdata,(rect_points[0],rect_points[1]),cv2.FONT_HERSHEY_SIMPLEX,0.9,(0,255,0),2)
if(qrdata!=lastprint):
qrCounter+=1
lastprint=qrdata
speaker_pub.publish(lastprint)
led_pub.publish(True)
rate.sleep()
cv2.imshow('Following Camera',frame)
cv2.waitKey(1)