-
Notifications
You must be signed in to change notification settings - Fork 1
/
c_2.py
174 lines (153 loc) · 3.78 KB
/
c_2.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
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
import paho.mqtt.client as mqtt
import json
import os
#coding:utf-8
from socket import *
from time import ctime
import binascii
import RPi.GPIO as GPIO
import time
import threading
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
########################
ENA = 13 #//L298A
ENB = 20 #//L298B
IN1 = 19 #1
IN2 = 16 #2
IN3 = 21 #3
IN4 = 26 #4
########################
IR_R = 18 #
IR_L = 27 #
########################
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
########################
GPIO.setup(ENA,GPIO.OUT,initial=GPIO.LOW)
ENA_pwm=GPIO.PWM(ENA,1000) #set initial frequency
ENA_pwm.start(0) #start pwm
ENA_pwm.ChangeDutyCycle(40) #change percentage
GPIO.setup(IN1,GPIO.OUT,initial=GPIO.LOW)
GPIO.setup(IN2,GPIO.OUT,initial=GPIO.LOW)
GPIO.setup(ENB,GPIO.OUT,initial=GPIO.LOW)
ENB_pwm=GPIO.PWM(ENB,1000) #set initial frequency
ENB_pwm.start(0) #start pwm
ENB_pwm.ChangeDutyCycle(40) #change percentage
GPIO.setup(IN3,GPIO.OUT,initial=GPIO.LOW)
GPIO.setup(IN4,GPIO.OUT,initial=GPIO.LOW)
##################
GPIO.setup(IR_R,GPIO.IN,pull_up_down=GPIO.PUD_UP)
GPIO.setup(IR_L,GPIO.IN,pull_up_down=GPIO.PUD_UP)
##################
def ENA_Speed(EA_num):
speed=hex(eval('0x'+EA_num))
speed=int(speed,16)
print 'EA_A changed %d '%speed
ENA_pwm.ChangeDutyCycle(speed)
def ENB_Speed(EB_num):
speed=hex(eval('0x'+EB_num))
speed=int(speed,16)
print 'EB_B changed %d '%speed
ENB_pwm.ChangeDutyCycle(speed)
def Motor_Forward():
print 'motor forward'
GPIO.output(ENA,True)
GPIO.output(ENB,True)
GPIO.output(IN1,True)
GPIO.output(IN2,False)
GPIO.output(IN3,True)
GPIO.output(IN4,False)
def Motor_TurnLeft():
print 'motor_turnleft'
GPIO.output(ENA,True)
GPIO.output(ENB,True)
GPIO.output(IN1,True)
GPIO.output(IN2,False)
GPIO.output(IN3,False)
GPIO.output(IN4,True)
def Motor_TurnRight():
print 'motor_turnright'
GPIO.output(ENA,True)
GPIO.output(ENB,True)
GPIO.output(IN1,False)
GPIO.output(IN2,True)
GPIO.output(IN3,True)
GPIO.output(IN4,False)
def Motor_Backward():
print 'motor_backward'
GPIO.output(ENA,True)
GPIO.output(ENB,True)
GPIO.output(IN1,False)
GPIO.output(IN2,True)
GPIO.output(IN3,False)
GPIO.output(IN4,True)
'''
###################################
##meet_black_turnright+back_or_turnleft+back_or_back
###################################
def Avoiding():
print IR_R
time.sleep(1)
if (GPIO.input(IR_R) == True) or (GPIO.input(IR_L) == True):
if (GPIO.input(IR_L) == False):
Motor_Backward()
Motor_TurnLeft()
# time.sleep(1)
return
else:
Motor_Backward()
Motor_TurnRight()
# time.sleep(1)
return
# else ((GPIO.input(IR_R) == False)&( GPIO.input(IR_L) == False)):
else:
Motor_Forward()
return
if ((GPIO.input(IR_R) == False)&(GPIO.input(IR_L) == True)):
Motor_Backward()
time.sleep(2)
return
if ((GPIO.input(IR_R) == True)&(GPIO.input(IR_L) == True)):
Motor_Backward()
time.sleep(1)
return
try:
while True:
Avoiding()#glud
time.sleep(0.1)
except KeyboardInterrupt:
GPIO.cleanup()
'''
###################################
##connection
###################################
def on_connect(client, userdata, flags, rc):
print('Connected with result code ' + str(rc))
client.subscribe('mqtt')
def on_message(client, userdata, msg):
# print str(msg.payload)
# print json.loads(msg.payload)
# if msg.payload<5:
# Motor_Backward()
# else:
# Motor_Forward()
a = int(msg.payload)
def BackOrForward():
if (on_message<=5)
Motor_Backward()
print 'back'
else:
Motor_Forward()
print 'forward'
if __name__ == '__main__':
client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message
BackOrForward()
#####
try:
client.connect(host='10.16.89.91',port=1883,keepalive=60)
client.loop_forever()
except KeyboardInterrupt:
client.disconnect()