def main(): #create plot global fig fig = plt.figure() ax = fig.add_subplot(111) ax.plot([], [], 'ro') ax.grid(True) bx = fig.add_subplot(111) bx.plot([], [], 'bo') plt.axis('equal') axes = plt.gca() axes.set_xlim([-500, 500]) axes.set_ylim([-500, 500]) global hedge hedge = MarvelmindHedge(tty="/dev/tty.usbmodem1411", recieveDataCallback=update_line) # create MarvelmindHedge thread hedge.start() plotThread = Thread( target=printThread) # create and start console out thread plotThread.start() plt.show()
class currentposition(): def __init__(self): self.hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=16, debug=False) self.hedge.start() #start thread def position(self): pos = self.hedge.position() return pos
def __init__(self): # Manually ask user to enter beacon address and port number for beacon self.addr = input("Who is we? ") #self.conn = input("What ACM port is Marvelmind Beacon plugged in to?: ") #self.port = '/dev/ttyACM' + str(self.conn) self.hedge = MarvelmindHedge(tty='dev/ttyUSB-MarvelmindBEACON', adr=self.addr, debug=False) # create MarvelmindHedge thread self.hedge.start() # start thread
class TrackingFrame(Frame): def __init__(self, root): Frame.__init__(self, root) self.root = root self.canvas = CanvasWidget(self) self.controls_container = CanvasControls(self, self.canvas) self.controls_container.grid(row=2, column=0, sticky=W) self.hedge = MarvelmindHedge( tty="\\.\COM3", adr=None, debug=False) # create MarvelmindHedge thread self.start_comms() self.ram_log = [] @staticmethod def valid_coords(coords): if coords[0] == 0 and coords[1] == 0 and coords[2] == 0 and coords[ 3] == 0: return False return True def start_comms(self, i=1): self.hedge.start() # start marvelmind thread schedule.every(1).seconds.do(self.communicate) cease = threading.Event() class ScheduleThread(threading.Thread): @classmethod def run(cls): while not cease.is_set(): schedule.run_pending() time.sleep(i) continuous_thread = ScheduleThread() continuous_thread.start() return cease def communicate(self): coords = self.hedge.position() if self.valid_coords(coords): self.parse_hedgehogs(list(self.hedge.valuesUltrasoundPosition)) else: print("Modem not connected!") def parse_hedgehogs(self, raw_data): parsed_hedgehogs = [] for arr in raw_data: id = arr[0] if id not in parsed_hedgehogs: parsed_hedgehogs.append(id) self.ram_log.append(arr) self.canvas.draw_hedgehog(arr) def onclose(self): self.hedge.stop() self.destroy()
def __init__(self, root): Frame.__init__(self, root) self.root = root self.canvas = CanvasWidget(self) self.controls_container = CanvasControls(self, self.canvas) self.controls_container.grid(row=2, column=0, sticky=W) self.hedge = MarvelmindHedge( tty="\\.\COM3", adr=None, debug=False) # create MarvelmindHedge thread self.start_comms() self.ram_log = []
def __init__(self, adr): for port in comports(): if port.vid == marvelmind_vid: self.hedge = MarvelmindHedge(tty=port.device, adr=adr, debug=False) break else: raise IOError("Marvelmind position sensor not found") self.hedge.start()
class currentpos(): def __init__(self, addr): self.addr = addr self.hedge = MarvelmindHedge(tty = "/dev/ttyACM0", adr=self.addr, debug=False) # create MarvelmindHedge thread self.hedge.start() # start thread def position(self): pos = self.hedge.position() # sleep(0.5) # print("X:", pos[1], "Y: ", pos[2]) return pos # spit out the full arraw returned by the Marvelmindhedge fxn
def main(): hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=10, debug=False) # create MarvelmindHedge thread hedge.start() # start thread x = hedge.position() data = {} data['x'] = x[0] data['y'] = x[1] data['z'] = x[2] data['time'] = x[3] print(data)
def main(): hedge = MarvelmindHedge(tty = "/dev/ttyACM0", adr=None, debug=False) # create MarvelmindHedge thread hedge.start() # start thread while True: try: sleep(1) # print (hedge.position()) # get last position and print hedge.print_position() if (hedge.distancesUpdated): hedge.print_distances() except KeyboardInterrupt: hedge.stop() # stop and close serial port sys.exit()
class Navigation(): def __init__(self): # Manually ask user to enter beacon address and port number for beacon self.addr = input("Who is we? ") #self.conn = input("What ACM port is Marvelmind Beacon plugged in to?: ") #self.port = '/dev/ttyACM' + str(self.conn) self.hedge = MarvelmindHedge(tty='dev/ttyUSB-MarvelmindBEACON', adr=self.addr, debug=False) # create MarvelmindHedge thread self.hedge.start() # start thread def position(self): # try: pos = self.hedge.position() return pos # spit out the full array returned by the Marvelmindhedge fxn # except KeyboardInterrupt: # self.hedge.stop() def trigsettle( self, trigPin, echoPin ): # call this function only once at the beginning of the code self.trigPin = trigPin self.echoPin = echoPin IO.setup(self.trigPin, IO.OUT) # set up trigger and echo pins IO.setup(self.echoPin, IO.IN) IO.output(self.trigPin, IO.LOW) print("Waiting for sensor to settle.." ) # settle the trigger pin just in case its has a high value time.sleep(0.25) def ping(self): print "Calculating distance..." IO.output(self.trigPin, IO.HIGH) time.sleep(0.00001) IO.output(self.trigPin, IO.LOW) while IO.input(self.echoPin) == 0: # if echo pin has no received a signal after sending a ping through transmitter, start timer startTime = time.time() while IO.input(self.echoPin) == 1: # if echo pin receives signal, end timer endTime = time.time() duration = endTime - startTime # calculate total time signal traveled distance = (duration * 34300) / 2 # find distance using speed of sound eq and divding by two to account for there and back # print("Distance:", distance, "cm") return distance # returns this distance in centimeters
def main(): hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=10, debug=False) # create MarvelmindHedge thread hedge.start() # start thread origin = (0, 0) rotation = 1.5708 #2.37365 first_run_1 = True first_run_5 = True initial_position = [] position = [] while True: try: sleep(0.125) # First run zeroing if (first_run_1 or first_run_5): # Gets initial position initial_position = copy.deepcopy(hedge.position()) # Zeroing for Hedgehog 1 if (initial_position[0] == 1 and first_run_1 == True): initial_position_1 = copy.deepcopy(initial_position) initial_point_1 = (initial_position_1[1], initial_position_1[2]) initial_position_1[1], initial_position_1[2] = rotate( origin, initial_point_1, rotation) print("INITIAL POSITION 1: " + str(initial_position_1)) first_run_1 = False # Zeroing for Hedgehog 5 elif (initial_position[0] == 5 and first_run_5 == True): initial_position_5 = copy.deepcopy(initial_position) initial_point_5 = (initial_position_5[1], initial_position_5[2]) initial_position_5[1], initial_position_5[2] = rotate( origin, initial_point_5, rotation) print("INITIAL POSITION 5: " + str(initial_position)) first_run_5 = False # Gets Hedgehog position position = hedge.position() # Positioning for Hedgehog 1 if (position[0] == 1 and first_run_1 == False): point = (position[1], position[2]) position[1], position[2] = rotate(origin, point, rotation) position[1], position[2] = position[1] - initial_position_1[ 1], position[2] - initial_position_1[2] print(position) # Positioning for Hedgehog 5 elif (position[0] == 5 and first_run_5 == False): point = (position[1], position[2]) position[1], position[2] = rotate(origin, point, rotation) position[1], position[2] = position[1] - initial_position_5[ 1], position[2] - initial_position_5[2] print(position) except KeyboardInterrupt: hedge.stop() # stop and close serial port sys.exit()
def main(): hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=10, debug=False) # create MarvelmindHedge thread hedge.start() # start thread origin = (0, 0) rotation = 1.5708 #2.37365 first_run = True initial_position = [] position = [] while True: try: sleep(0.125) if (first_run): initial_position = copy.deepcopy(hedge.position()) initial_point = (initial_position[1], initial_position[2]) initial_position[1], initial_position[2] = rotate( origin, initial_point, rotation) print("INITIAL POSITION: " + str(initial_position)) first_run = False # print (hedge.position()) # get last position and print position = hedge.position() point = (position[1], position[2]) position[1], position[2] = rotate(origin, point, rotation) position[1], position[2] = position[1] - initial_position[ 1], position[2] - initial_position[2] print(position) except KeyboardInterrupt: hedge.stop() # stop and close serial port sys.exit()
def main(): # TODO: automate reading usb serial name # res = os.system("ls /dev/tty.usb*") # get usb port with ls /dev/tty.usb* hedge = MarvelmindHedge(tty=tty, adr=97, debug=False) # create MarvelmindHedge thread hedge.start() # start thread while True: try: sleep(1) # print (hedge.position()) # get last position and print hedge.print_position() if (hedge.distancesUpdated): hedge.print_distances() except KeyboardInterrupt: hedge.stop() # stop and close serial port sys.exit()
def main(): hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=10, debug=False) hedge.start() swarm = [] quad1 = drone(1) # quad5 = drone(5) swarm.append(quad1) # swarm.append(quad5) while True: try: sleep(0.125) hedge_pos = hedge.position() for quad in swarm: if (hedge_pos[0] == quad.drone_num and quad.first_run): quad.calculate_initial_position(hedge_pos) for quad in swarm: if (hedge_pos[0] == quad.drone_num): quad.fly(hedge_pos) except KeyboardInterrupt: quad1.send_land() # quad5.send_land() client.close() hedge.stop() sys.exit()
def main(): hedge = MarvelmindHedge(tty="/dev/tty.usbmodem1421", adr=None, debug=False) # create MarvelmindHedge thread hedge.start() # start thread s = socket.socket() s.connect(('127.0.0.1', 12347)) i = 0 while True: try: #i = i+1 sleep(2) # print (hedge.position()) # get last position and print # -------------------------- # my additions to Marvelmind's code # list contains the X,Y,Z coordinates from the hedgehog list = hedge.send_position() print(str(list)) temp = list[1:3] temp.append(list[4]) final = [] for x in temp: if x < 0: x = x * -1 #the multiplier is the proportion of the room #take highest value of x,y and divide 150 by it #so x*150 / highestValueOfRoom x = x * 30 final.append(x) print(final) toSend = json.dumps(final) s.send(toSend.encode()) print("i is", i) #if i == 0: # toSend = json.dumps("Bye") # s.send(toSend.encode()) # s.close() # hedge.stop() # sys.exit() #print(str(list[1:5])[1:-1]) # -------------------------- if (hedge.distancesUpdated): hedge.print_distances() except KeyboardInterrupt: hedge.stop() # stop and close serial port sys.exit()
def main(): #read map map_path = 'map.txt' map = open(map_path, 'r') counter = 0 map_lines = [] for line in map: if counter%2 == 0: map_pts = eval(line) elif counter%2 == 1: map_lines = eval(line) counter += 1 line_pts = [] for pair in map_lines: line_pt = [tuple(map_pts[pair[0]]), tuple(map_pts[pair[1]])] line_pts.append(line_pt) lc = mc.LineCollection(line_pts, linewidths=1) #create plot global fig fig = plt.figure() ax = fig.add_subplot(111) ax.plot([],[], 'ro') #ax.add_collection(lc) ax.grid(True) bx = fig.add_subplot(111) bx.plot([],[], 'bo') plt.axis('equal') cx = fig.add_subplot(111) cx.add_collection(lc) axes = plt.gca() axes.set_xlim([0,10]) axes.set_ylim([-5,5]) global hedge hedge = MarvelmindHedge(tty = "/dev/ttyACM2", adr=10, recieveUltrasoundPositionCallback=update_line) # create MarvelmindHedge thread hedge.start() plotThread = Thread(target=printThread) # create and start console out thread plotThread.start() plt.show()
class Marvelmind: def __init__(self, adr): for port in comports(): if port.vid == marvelmind_vid: self.hedge = MarvelmindHedge(tty=port.device, adr=adr, debug=False) break else: raise IOError("Marvelmind position sensor not found") self.hedge.start() def read(self): hedge_id, x, y, z, angle, timestamp = self.hedge.position() return np.array([x, y]) def stop(self): self.hedge.stop()
def main(): # the below line is for a hedgehog with address 10. Change to whatever address your hedgehog actually is hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=10, debug=False) # create MarvelmindHedge thread hedge.start() # start thread while True: try: sleep(1) # print (hedge.position()) # get last position and print hedge.print_position() except KeyboardInterrupt: hedge.stop() # stop and close serial port sys.exit()
def main(): global hedge global dt_vision # ローカルに配置しておいた俯瞰図landscapeを視野画像の風景としてオンメモリーにロードしておくため global angle # X=20, y=20, angle=90の設定では # Loaderの初期位置は原点(#73と#39ビーコンの間の角)に対して(20ポチ,20ポチ)=(0.16m, 0.16m)のところで#39ビーコンの方角を向いている dt_vision = SpoolMobileVision(59, 20, 20, angle, 4, 0, scale, landscape=Image.open(landscape_file_path)) hedge = MarvelmindHedge( adr=59, tty="/dev/ttyACM0", baud=38400, maxvaluescount=12, recieveImuDataCallback=updateMiniatureWarehouseReferenceFrameData, debug=False) hedge.start() while True: try: pass # time.sleep(0.020) except KeyboardInterrupt: hedge.stop() # stop and close serial port sys.exit()
def __init__(self, front_hedge, rear_hedge, gopigo, speed=300, q=None, debug_mode=False): Thread.__init__(self) # initialize the gopigo components self.gpg = gopigo self.distance_sensor = self.gpg.gpg.init_distance_sensor() # initialize gps "settings" self.__threshold = .06 self.__command_queue = q self.__minimum_distance = 45 self.__debug = debug_mode self.__cancel_early = False self.__thread_done = False # setup geometry information self.__transform = Transform() self.__rear_position = Vector() self.__speed = speed self.gpg.set_speed(speed) self.__destination = None # setup thread information self.path = [] # start the hedge self.__front_hedge = front_hedge self.__rear_hedge = rear_hedge self.__hedge = MarvelmindHedge(tty="/dev/ttyACM0", recieveUltrasoundPositionCallback=self.position_update) self.__hedge.start() # setup callbacks self.__position_callback = None self.__obstacle_callback = None self.__reached_point_callback = None self.__no_obstacles_callback = None # set initial position if self.__debug: self.__hedge.print_position()
def main(): hedge = MarvelmindHedge(tty="/dev/tty.usbmodem1411", adr=10, debug=False) # create MarvelmindHedge thread hedge.start() # start thread while True: try: sleep(1) print('IMU Data:') print(hedge.valuesImuData) print('Raw IMU Data:') print(hedge.valuesImuRawData) # get last position and print print('Position Raw:') print(hedge.position()) # get last position and print print('Position:') hedge.print_position() except KeyboardInterrupt: hedge.stop() # stop and close serial port sys.exit()
def marvelThread(status, GPSQueue): hedge = MarvelmindHedge(tty = "/dev/ttyACM0", adr=10, debug=False) # create MarvelmindHedge thread hedge.start() # start thread while status[0]: sleep(.1) #Number of beacon, X, Y, Z, Time GPSQueue.put(GPSCoord(hedge.position())) print ("The Marvel Thread: ", '') hedge.print_position() if not status[0]: GPSQueue.put(GPSCoord([-1,-1,-1,-1,-1])) print("Broke out of marvel")
def __init__(self, forwardpin, leftpin, rightpin, destinationpin, modeindicatorpin): ##position## self.pos = currentposition() self.pos.position() self.x1 = self.pos.position()[1] self.y1 = self.pos.position()[2] sleep(.5) self.x2 = self.pos.position()[1] self.y2 = self.pos.position()[2] self.x3 = 5.4 self.y3 = 2.8 self.targetnumber = 2 self.deltaT = 1.0 self.hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=16, debug=False) self.hedge.start() ###TRIG### self.trig = trig() ###USS### # IO setup self.forwardpin = forwardpin self.rightpin = rightpin self.leftpin = leftpin self.destinationpin = destinationpin self.modeindicatorpin = modeindicatorpin IO.setmode(IO.BCM) IO.setup(self.forwardpin, IO.OUT) IO.setup(self.rightpin, IO.OUT) IO.setup(self.leftpin, IO.OUT) IO.setup(self.destinationpin, IO.OUT) IO.setup(self.modeindicatorpin, IO.OUT)
def marvelThread(status, GPSQueue): hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=10, debug=False) # create MarvelmindHedge thread hedge.start() # start thread while status[0]: sleep(.1) #Number of beacon, X, Y, Z, Time GPSQueue.put(GPSCoord(hedge.position())) print("The Marvel Thread: ") hedge.print_position() t = hedge.position() f = open('dataouts.txt', 'a') f.write('Marvel = {}'.format(t) + '\n') f.close() if not status[0]: GPSQueue.put(GPSCoord([-1, -1, -1, -1, -1])) print("Ended Marvel Queue")
def main(): hedge = MarvelmindHedge(tty="/dev/ttyACM2", adr=10, debug=False) # create MarvelmindHedge thread hedge.start() # start thread while True: try: sleep(1) matrix = hedge.valuesImuData i = 0 w = 0 x = 0 y = 0 z = 0 #get quaternion data and average it while (i < 3): w = matrix[i][3] + w x = matrix[i][4] + x y = matrix[i][5] + y z = matrix[i][6] + z i = i + 1 quaternion_to_euler_angle(w / 3, x / 3, y / 3, z / 3) rawdata = hedge.valuesImuRawData xGaussData = rawdata[0][6] #compass data around x axis yGaussData = rawdata[0][7] #compass data around y axis #convert compass data into degrees d = (math.atan2(yGaussData, xGaussData)) * (180 / math.pi) print("degree is:", d) print(hedge._bufferSerialDeque) print('Position of hedge sensors:') print(hedge.position()) # get last position and print except KeyboardInterrupt: hedge.stop() # stop and close serial port sys.exit()
from marvelmind import MarvelmindHedge from time import sleep import sys import time hedge1 = MarvelmindHedge(tty="/dev/ttyACM0", adr=2, debug=False) hedge2 = MarvelmindHedge(tty="/dev/ttyACM1", adr=8, debug=False) hedge1.start() hedge2.start() sleep(1) positionX = list() positionY = list() start_time = time.time() while (time.time() - start_time) < 5: sleep(1) current_psition = hedge1.position() if current_psition[1] == 0 or current_psition[2] == 0: print 'error in GPS positioning' continue positionX.append(current_psition[1]) positionY.append(current_psition[2]) print 'current_psition=', current_psition[1], ',', current_psition[2] #hedge1.stop() X1 = 100 * (sum(positionX) / len(positionX)) Y1 = 100 * (sum(positionY) / len(positionY)) print 'hedge1', X1, '', Y1 positionX = list() positionY = list() start_time = time.time() while (time.time() - start_time) < 5:
from marvelmind import MarvelmindHedge import threading import time import sys from flask import Flask, render_template global Position from flask_socketio import SocketIO, send, emit hedge = MarvelmindHedge(tty="COM4", adr=2, debug=False) # create MarvelmindHedge thread hedge.start() # start thread app = Flask(__name__) @app.route("/") def server(): print(hedge.IMUData()) while True: return str(hedge.IMUData())x @app.route("/test") def test(): print(hedge.IMUData()) while True: return render_template('reload.html', row=str(hedge.IMUData())) @app.route("/test2") def test(): print(hedge.IMUData()) while True: row = str(hedge.IMUData()) socketio.emit('message', {'msg': 'test', 'text':row}, broadcast =True) return render_template('reload.html') if __name__=='__main__': app.run(debug=False)
hedge.valuesImuData[-1][0], hedge.valuesImuData[-1][1], hedge.valuesImuData[-1][2] ]) plotter.data[-1]['a_size'] = 25 plotter.imudata.append([ hedge.valuesImuData[-1][0], hedge.valuesImuData[-1][1], hedge.valuesImuData[-1][2] ]) if (len(plotter.data) > plotter.pointlimiter + len(plotter.datastatic)): plotter.data = plotter.data[-plotter.pointlimiter:] plotter.data = np.append(plotter.datastatic, plotter.data) global hedge hedge = MarvelmindHedge( tty="/dev/tty.usbmodem1421", adr=20, recieveUltrasoundPositionCallback=updateUSNavData, recieveImuDataCallback=updateAccData, debug=False ) # create MarvelmindHedge thread recieveAccelerometerDataCallback=updateAccData import plotter3d2 from vispy import app global plotter plotter = plotter3d2.Canvas() hedge.start() app.run()
from nanpy import ArduinoApi, SerialManager from marvelmind import MarvelmindHedge from time import sleep from Point import Point from Vector import Vector #Set up the arduino as a slave to the raspberry pi through a serial port c = SerialManager(device='/dev/ttyACM0') a = ArduinoApi(connection=c) #Arduino Motor Control Pins dirA = 8 dirB = 10 pwmA = 9 pwmB = 11 # initialize the MarvelMindHedge GPS hedge = MarvelmindHedge(tty="/dev/ttyACM2", adr=9, debug=False) #initialize the input from the IMU ser = serial.Serial('/dev/ttyACM1', 115200, timeout=0) # set the path path = [ Point(.2, -32), Point(0.016, -28.119), Point(-0.591, -21.995), Point(-0.197, -15.945), Point(0.293, -9.742), Point(0.077, -2.05), Point(.565, -2.212) ] #set arduino pins - motors start off #initalize self point self = Point(0, 0)
import sys order = [2, 4, 6, 8, 10, 12, 1, 3, 5, 7, 9, 11] waycoord = [[1, 2], [3, 4], [5, 6], [7, 8], [9, 10], [11, 12], [13, 14], [15, 16], [17, 18], [19, 20], [21, 22], [23, 24]] sensor_pin = 5 RPL.pinMode(sensor_pin, RPL.INPUT) def analogRead(pin): putPacket(ANREAD, bytearray([5]), 1) buff = getPacket() return int(buff[3][1]) | (int(buff[3][2]) << 8) hedge = MarvelmindHedge(tty="/dev/ttyAMC0", adr=None, debug=False) hedge.start() intposx = float(hedge.position()[1]) #initial coordinates intposy = float(hedge.position()[2]) def driveMath(intposx, intposy, currentx, currenty, wayx, wayy): #gimme fuel gimme fire gimme that which i desire dist = math.sqrt((wayx - currentx)**2 + (wayy - currenty)**2) u = wayx - intposx v = wayy - intposy u1 = currentx - intposx v1 = currenty - intposy return (u * v1 - v * u1) #vector math to see if the turn is up or down
def main(): portNumber = 0 hedge = MarvelmindHedge(tty="/dev/ttyACM" + `portNumber`) hedge.start() bridge = bridgeclient() lastMeasureTime = 0 while True: try: sleep(0.2) if hedge.getError() == "NO_ERROR": position = hedge.position() measureTime = position[3] if measureTime == lastMeasureTime: bridge.put("error", ("E")) hedge.stop() hedge=MarvelmindHedge(tty="/dev/ttyACM"+`portNumber%10`) hedge.start() else: lastMeasureTime = measureTime bridge.put("error", ("N")) bridge.put("x", (str(position[0]))) bridge.put("y", (str(position[1]))) #print(str(position[0]),str(position[1])) else: bridge.put("error", ("S")) portNumber += 1 hedge.stop() hedge=MarvelmindHedge(tty="/dev/ttyACM" + `portNumber%10`) hedge.start() except KeyboardInterrupt: hedge.stop() # stop and close serial port sys.exit() except Exception: bridge = bridgeclient() continue