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()