-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathNetworkHubv2.py
257 lines (240 loc) · 8.17 KB
/
NetworkHubv2.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
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
import socket
import struct
from threading import Timer
import time
import RPi.GPIO as GPIO
import sys
controllerSocket = None
networkHubSocket = None
motorServerSocket = None
servoServerSocket = None
motorSocket = None
servoSocket = None
INPUT_FORMAT = 'c i i ?'
MOTOR_FORMAT = 'i i ?'
SERVO_FORMAT = 'c h h'
'''MOTOR_OP_CODE = 0
SERVO_CONTROL_OP_COD = 2
SERVO_COMMAND_OP_CODE = 1'''
MOTOR_OP_CODE = 'm'
SERVO_CONTROL_OP_CODE = 'c'
SERVO_COMMAND_OP_CODE = 's'
LED1 = 20
LED2 = 21
def setupLED():
GPIO.setmode(GPIO.BCM)
GPIO.setup(LED1, GPIO.OUT)
GPIO.setup(LED2, GPIO.OUT)
def setupControllerConnection():
global controllerSocket
global networkHubSocket
global motorServerSocket
global LED1
global LED2
GPIO.output(LED1, GPIO.LOW)
GPIO.output(LED2, GPIO.LOW)
networkHubSocket = socket.socket()
hostname = '192.168.2.2'
#hostname = '192.168.2.2'
print(hostname)
port = 1234
networkHubSocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
while(1):
time.sleep(0.5)
print('Network Hub: Creating network hub connection')
try:
networkHubSocket.bind((hostname, port))
networkHubSocket.listen(100)
print('Network Hub: Created network hub connection')
break
except KeyboardInterrupt:
print('Connection Interupted')
command = (SERVO_COMMAND_OP_CODE, 0)
sendServoCommand(SERVO_COMMAND_OP_CODE, command)
except:
print('Network Hub: Couldnt create network hub connection')
print(sys.exc_info()[0])
print('Network Hub: Created network hub connection')
controllerSocket, addr = networkHubSocket.accept()
GPIO.output(LED1, GPIO.HIGH)
GPIO.output(LED2, GPIO.HIGH)
print("Network Hub: Controller connected")
def setupMotorProgramConnection():
global motorServerSocket
global motorSocket
motorServerSocket = socket.socket()
hostname = socket.gethostname()
port = 4001
while(1):
print('Network Hub: Waiting for motor program')
try:
motorServerSocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
motorServerSocket.bind((hostname, port))
motorServerSocket.listen(100)
print("Network Hub: Created motor program connection")
break
except:
print('Network Hub: Couldnt make motor program connection')
motorSocket, addr = motorServerSocket.accept()
def setupServoProgramConnection():
global servoServerSocket
global servoSocket
servoServerSocket = socket.socket()
hostname = socket.gethostname()
port = 2001
while(1):
print('Network Hub: Waiting for servo program')
try:
servoServerSocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
servoServerSocket.bind((hostname, port))
servoServerSocket.listen(100)
print("Network Hub: Created servo program connection")
break
except:
print('Network Hub: Couldnt make servo program connection')
servoSocket, addr = servoServerSocket.accept()
def sendMotorCommand(leftSpeed, rightSpeed, sensorOn):
global motorSocket
#print('Right motor speed: ' + str(rightSpeed) + 'Left motor speed' + str(leftSpeed))
motorCommand = struct.pack(MOTOR_FORMAT, leftSpeed, rightSpeed, sensorOn)
motorSocket.send(motorCommand)
def sendServoCommand(opCode, unpackedCommand):
servoCommand = unpackedCommand[1]
command = struct.pack(SERVO_FORMAT, opCode, servoCommand, 0)
servoSocket.send(command)
def sendServoControl(opCode, unpackedCommand):
servoChannel = unpackedCommand[1]
servoPosition = unpackedCommand[2]
command = struct.pack(SERVO_FORMAT, opCode, servoChannel, servoPosition)
servoSocket.send(command)
def sendStopCommand():
global controllerSocket
print("Network Hub: Stopping")
closeConnections()
def setupConnections():
setupMotorProgramConnection()
setupServoProgramConnection()
setupControllerConnection()
def stopMotors():
sendMotorCommand(0, 0, False)
def closeConnections():
global networkHubSocket
global controllerSocket
global motorServerSocket
global motorSocket
global servoServerSocket
global servoSocket
try:
networkHubSocket.shutdown(socket.SHUT_RDWR)
networkHubSocket.close
except:
print('Network Hub: Controller socket already closed')
try:
controllerSocket.shutdown(socket.SHUT_RDWR)
controllerSocket.close
except:
print('Network Hub: Controller socket already closed')
'''try:
motorSocket.shutdown(socket.SHUT_RDWR)
motorSocket.close
except:
print('Motor socket already closed')
try:
servoSocket.shutdown(socket.SHUT_RDWR)
servoSocket.close
except:
print('Servo socket already closed')
try:
motorServerSocket.shutdown(socket.SHUT_RDWR)
motorServerSocket.close
except:
print('Motor server socket already closed')
try:
servoServerSocket.shutdown(socket.SHUT_RDWR)
servoServerSocket.close
except:
print('Servo server socket already closed')'''
setupLED()
setupMotorProgramConnection()
setupServoProgramConnection()
setupControllerConnection()
'''while(1):
timer = Timer(1, sendStopCommand)
timer.start()
print('Waiting for command...')
print(struct.calcsize(INPUT_FORMAT))
packedCommand = controllerSocket.recv(struct.calcsize(INPUT_FORMAT))
print('Received command')
timer.cancel()
command = struct.unpack(INPUT_FORMAT, packedCommand)
#print(command)
opCode = command[0]
#print('op code is ' + str(opCode))
if(opCode == MOTOR_OP_CODE):
print('Sending motor command')
leftMotorSpeed = command[1]
rightMotorSpeed = command[2]
#sensorOn = command[3]
sendMotorCommand(leftMotorSpeed, rightMotorSpeed, True)
continue
elif(opCode == SERVO_COMMAND_OP_CODE):
print('Sending servo command')
sendServoCommand(opCode, command)
continue
elif(opCode == SERVO_CONTROL_OP_CODE):
print('Sending servo control')
sendServoControl(opCode, command)
continue'''
while(1):
try:
timer = Timer(1, sendStopCommand)
timer.start()
#print('Network Hub: Waiting for command...')
packedCommand = controllerSocket.recv(struct.calcsize(INPUT_FORMAT))
#print('Network Hub: Received command')
timer.cancel()
command = struct.unpack(INPUT_FORMAT, packedCommand)
opCode = command[0]
#print('op code is ' + str(opCode))
if(opCode == MOTOR_OP_CODE):
print('Network Hub: Sending motor command')
leftMotorSpeed = command[1]
rightMotorSpeed = command[2]
sensorOn = command[3]
sendMotorCommand(leftMotorSpeed, rightMotorSpeed, sensorOn)
continue
elif(opCode == SERVO_COMMAND_OP_CODE):
print('Network Hub: Sending servo command')
sendServoCommand(opCode, command)
continue
elif(opCode == SERVO_CONTROL_OP_CODE):
print('Network Hub: Sending servo control')
sendServoControl(opCode, command)
continue
except KeyboardInterrupt:
print("Network Hub: Program Ended")
stopMotors()
command = (SERVO_COMMAND_OP_CODE, 0)
sendServoCommand(SERVO_COMMAND_OP_CODE, command)
GPIO.output(LED1, GPIO.LOW)
GPIO.output(LED2, GPIO.LOW)
#closeConnections()
break
except socket.error:
print("Network Hub: Lost Connection")
stopMotors()
#command = (SERVO_COMMAND_OP_CODE, 0)
#sendServoCommand(SERVO_COMMAND_OP_CODE, command)
GPIO.output(LED1, GPIO.LOW)
GPIO.output(LED2, GPIO.LOW)
#closeConnections()
setupControllerConnection()
except:
print("Network Hub: Program Error")
stopMotors()
#command = (SERVO_COMMAND_OP_CODE, 0)
#sendServoCommand(SERVO_COMMAND_OP_CODE, command)
GPIO.output(LED1, GPIO.LOW)
GPIO.output(LED2, GPIO.LOW)
#closeConnections()
setupControllerConnection()