-
Notifications
You must be signed in to change notification settings - Fork 1
/
c.py
158 lines (137 loc) · 3.41 KB
/
c.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
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(50) #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(50) #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):
Motor_Backward()
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 (100+json.loads(msg.payload))
if __name__ == '__main__':
client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message
a = on_message
print a
try:
client.connect(host='10.16.89.91',port=1883,keepalive=60)
client.loop_forever()
except KeyboardInterrupt:
client.disconnect()
'''