This repository has been archived by the owner on Aug 13, 2018. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
robot.py
68 lines (57 loc) · 1.89 KB
/
robot.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
#!/usr/bin/python
import Queue
import time
import threading # Import threads
from motor_thread import MotorThread
from server import RobotNetworkService
def help(sendQueue):
sendQueue.put('~~Robot server help~~\n')
sendQueue.put('Commands:\n')
sendQueue.put('help - displays this menu\n')
sendQueue.put('quit - Quits client and server\n')
sendQueue.put('disconnect - Disconnect client only\n')
# Instantiate queues
motorQueue = Queue.Queue()
cmdQueue = Queue.Queue()
sendQueue = Queue.Queue()
# Kick off motorControl thread
motorControl = MotorThread(motorQueue, sendQueue)
motorControl.start()
# Start server
netService = RobotNetworkService(cmdQueue, sendQueue)
netService.start()
msg = ""
while (msg != "quit"):
# TODO: Verify threads stay alive.
if (motorControl.isAlive() == False):
print 'Motor control thread died!'
break
if (netService.isAlive() == False):
print 'Network service thread died!'
break
# TODO: Require a heartbeat message from the remote
try:
msg = cmdQueue.get(True, 1)
except Queue.Empty:
# Not a bad thing, just need to check the exit conditions.
pass
else:
print 'Received command: ' + msg
words = msg.split()
if (words[0] == "motor"):
motorControl.putCmd(msg)
#####################################################################
# Stop the motors and anything else we don't want running after the
# program exits.
#####################################################################
# TODO: Stop the motors and whatever else
motorQueue.put("motor 0 0")
# Let the motor thread get the message
# TODO: Is there a better way to guarantee this?
time.sleep(1)
#####################################################################
# Clean up all the threads
#####################################################################
netService.join()
motorControl.join()
print 'Robot exit'