def perform_command(cmd, inc):
# 安排inc秒后再次运行自己，即周期运行
global nTime
schedule.enter(inc, 0, perform_command, (cmd,inc))
#os.system(cmd)
#print(time.time())
print 'execute time:' + str(time.time()), 'at nTIme:' + str(nTime)
nTime = nTime + 1
# 读命令
# mario_com = sys.stdin.readline()
mario_com = 'p'
dm.send_command_to_mario(arduino, mario_com)
time.sleep(0.5)
mario_resp = dm.read_response_from_mario(arduino)
print mario_resp
import time, os, sched, sys
import drive_mario as dm
if __name__ == '__main__':
arduino = dm.open_arduino_port()
while True:
mario_com = 'p'
dm.send_command_to_mario(arduino, mario_com)
time.sleep(0.1)
def median_filter(data):
return sorted(data)[len(data)/2]
if isinstance(zip_filter, bytes):
zip_filter = zip_filter.decode("ascii")
socket.setsockopt_string(zmq.SUBSCRIBE, zip_filter)
grip = False
arduino = dm.open_arduino_port()
data = [3000,3000,3000,3000,3000]
distance = 3000
result = 3000
while True:
string = socket.recv_string()
print(string)
if len(string.split(':')) > 1 and string.split(':')[1] != '':
result = int(string.split(':')[1])
if(result < 3000):
distance = result
data.pop(0)
data.append(distance)
distance = median_filter(data)
if(not grip and distance <= 5):#grip the ball
dm.send_command_to_mario(arduino, 'c')
grip = True
if (grip and distance > 8 and distance < 3000):
dm.send_command_to_mario(arduino, 'o')
grip = False
print(distance)
import sys
import zmq
import drive_mario as dm
context = zmq.Context()
socket = context.socket(zmq.SUB)
print("Collecting updates from publisher server...")
socket.connect("tcp://localhost:5556")
zip_filter = ""
mario_forward = 'w'
mario_backward = 's'
if isinstance(zip_filter,bytes):
zip_filter = zip_filter.decode("ascii")
socket.setsockopt_string(zmq.SUBSCRIBE, zip_filter)
arduino = dm.open_arduino_port()
while True:
string = socket.recv_string()
print(int(string))
if(int(string) > 0):
dm.send_command_to_mario(arduino, mario_forward)
else:
dm.send_command_to_mario(arduino, mario_backward)
#time.sleep()
