def main(): hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=10, debug=False) # create MarvelmindHedge thread hedge.start() # start thread sleep(.1) start_pos = convert2D(hedge.position()) print hedge.position() print start_pos new_pos = start_pos dst = 0.0 gopigo.set_speed(100) gopigo.fwd() try: while (abs(dst) < 1): sleep(.1) new_pos = convert2D(hedge.position()) hedge.print_position() dst = distance(start_pos, new_pos) print "start: ", start_pos print "current: ", new_pos print "distance: ", dst except KeyboardInterrupt: hedge.stop() # stop and close serial port gopigo.stop() sys.exit() hedge.stop() gopigo.stop()
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(): 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
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()
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 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")
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()
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 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 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/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()
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(): 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 time import sleep import sys import math import time hedge = MarvelmindHedge(tty = "/dev/ttyACM0", debug=False) # create MarvelmindHedge thread hedge.start() # start thread SleepPeriod=0.2 DataCollectionDuration=10 positionX=list() positionY=list() start_time = time.time() while (time.time() - start_time) < DataCollectionDuration: sleep(SleepPeriod) current_psition=hedge.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] X1=sum(positionX)/len(positionX) Y1=sum(positionY)/len(positionY) '''STD=0 for element in positionX: STD=STD+math.pow((X1-element),2) STD=math.sqrt(STD)/(len(positionX)-1) XNew=list() for element in positionX:
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:
#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) data = hedge.position() self.setX(data[1]) self.setY(data[2]) #initialize direction direction = 0 #flush imu input to get most recent value (serial is a queue) ser.flushInput() time.sleep(.5) #read and parse the data rawDir = ser.readline() rawDir = rawDir.replace('\r\n', '') #initilze the offset offset = float(rawDir) + 24 time.sleep(1) print str(offset)
def main(X, Y, HEDGE): global newEvent1 global newEvent2 newEvent1 = False newEvent2 = False run = True state = None HEDGE = MarvelmindHedge(tty="/dev/ttyACM0", adr=10, debug=False) HEDGE.start() pos = HEDGE.position() x = pos[1] y = pos[2] xdiff = X - x ydiff = Y - y m_i = numpy.sqrt(xdiff * xdiff + ydiff * ydiff) print(m_i) # try: # Degrees = numpy.arctan(xdiff/ydiff) * 180 / numpy.pi # except ZeroDivisionError: # degrees = 0 # Print('press ctrl+c to quit') while run: try: pos = HEDGE.position() old_state = state x = pos[1] y = pos[2] print(x) print(y) xdiff = X - x ydiff = Y - y m_c = numpy.sqrt(xdiff * xdiff + ydiff * ydiff) print(m_c) print("Current position: ({}, {})".format(x, y)) speed = (m_c / m_i) * 100 print("speed magnitude: {}".format(speed)) s1.set_motor(speed, p1) s2.set_motor(speed, p2) Handler(xdiff, ydiff, speedfactor) if newEvent1: newEvent1 = False if moveUp: direction("forward") elif moveDown: direction("reverse") else: MotorOff() if newEvent2: newEvent2 = False if moveLeft: direction("left") elif moveRight: direction("right") else: MotorOff() time.sleep(1) except KeyboardInterrupt: print("interrupted") MotorOff() pwm1.stop() pwm2.stop()
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 driveCode(turn):
# initiate local positioning if use_marvelmind: marvelmind_vid = 1155 # VID of Marvelmind device found = False for port in comports(): if port.vid == marvelmind_vid: hedgeport = port.device found = True if found == True: print("Marvelmind port:", hedgeport) hedge = MarvelmindHedge(tty=hedgeport, adr=hedgehog_id, debug=False) # create MarvelmindHedge thread hedge.start() # start thread time.sleep(1) # pause to let it settle position = hedge.position() else: print("Marvelmind not found") # functions def get_position(): global hedgehog_x global hedgehog_y position = hedge.position() hedgehog_x = position[1] hedgehog_y = position[2] def displayspeed():
def main(): # Setup for MarvelMind and data socket hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=10, debug=False) hedge.start() socket_path = '/tmp/node-python-sock' client = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) client.connect(socket_path) # Variable declarations and initalization desired_x, desired_y, flight_x, flight_y = 0, 0, 0, 0 data = {} prev_position = [] data_log = [] data_log_raw = [] initial_position = [] position = [] first_run = True location_counter = 0 # Used to rotate data. 'rotation' is in radians rotation = 1.5708 #2.37365 origin = (0, 0) f = open("flight_data.txt", "w") # Loop to get location and fly drone while True: try: # Sleep delay. Change to change update speed sleep(0.4) # Code to run the first time if (first_run): # Gets initial position and rotates it 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) # Sets the previous position prev_position = copy.deepcopy(initial_position) print("Initial Position: " + str(initial_position)) first_run = False # Gets drone position position = hedge.position() # Rotates the drone position point = (position[1], position[2]) position[1], position[2] = rotate(origin, point, rotation) # Adds positiion data to raw data log data_log_raw.append((position[1], position[2], position[4])) # Smooths position data position[1], position[2] = position[1] - initial_position[ 1], position[2] - initial_position[2] position[1] = smooth_data(prev_position[1], position[1]) position[2] = smooth_data(prev_position[2], position[2]) # Adds position data to data log # data_log.append((position[1], position[2], position[4])) f.write( str(position[1]) + ", " + str(position[2]) + ", " + str(location_counter) + "\n") # Sets current value as previous value prev_position = copy.deepcopy(position) # Displays location print(position) # Prompts user for desired flight location if (flight_x == 0 and flight_y == 0): # Sends 0 power commands to quad data['power_x'] = 0 data['power_y'] = 0 data['time'] = position[4] jsonData = json.dumps(data) client.send(jsonData) # Gets location from user desired_x = get_desired_position('x') desired_y = get_desired_position('y') location_counter += 1 # Calculates the drone power flight_x = calculate_flight_power(position[1], desired_x) flight_y = calculate_flight_power(position[2], desired_y) # Sets the flight values into a json format data['power_x'] = flight_x data['power_y'] = flight_y data['time'] = position[4] jsonData = json.dumps(data) # Sends data to JavaScript (marvel_drone_socket.js) client.send(jsonData) # Ends infinite loop and closes threads except KeyboardInterrupt: client.close() hedge.stop() sys.exit()
class GPS(Thread): # initialize the class 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() # used if it is being run like a thread. Use the queue sent in to add commands to the gps def run(self): # FOREVER while not self.__thread_done: # if we have a command if not self.__command_queue.empty(): command = self.__command_queue.get() print("point received", command) # goto point self.__destination = command self.goto_point(self.__destination) # if we have nothing to do else: # update where we are and run callbacks self.get_position_callback() self.check_for_obstacles() time.sleep(.1) self.stop() print("GPS Thread stopped") # A callback sent to the hedge, it changes the position and rotation information every time it receives an update. def position_update(self): position = self.__hedge.position() if position[0] == self.__front_hedge: self.__transform.position = self.__convert_hedge_coords(position) else: self.__rear_position = self.__convert_hedge_coords(position) self.__transform.rotation = self.get_angle(self.__transform.position, self.__rear_position) # converts a hedge position into a 2D Vector in the transform @staticmethod def __convert_hedge_coords(position): return Vector(position[1], position[2]) # the following methods set callbacks or get them. # If they are getting them, they will do nothing if None is used def set_position_callback(self, callback): self.__position_callback = callback def get_position_callback(self): if self.__position_callback is not None: self.__position_callback(self.__transform.position) def set_obstacle_callback(self, callback): self.__obstacle_callback = callback def get_obstacle_callback(self, pos): if self.__obstacle_callback is not None: self.__obstacle_callback(pos) def set_reached_point_callback(self, callback): self.__reached_point_callback = callback def get_reached_point_callback(self): if self.__reached_point_callback is not None: self.__reached_point_callback(self.__transform.position) print("destination reached!") def set_no_obstacle_callback(self, callback): self.__no_obstacles_callback = callback def get_no_obstacle_callback(self, positions): if self.__no_obstacles_callback is not None: self.__no_obstacles_callback(positions) def cancel_movement(self): self.__cancel_early = True def stop_thread(self): self.__thread_done = False def set_min_distance(self, dst): self.__minimum_distance = dst def set_speed(self, spd): self.speed = spd self.gpg.set_speed(spd) # returns the current distance from the destination def distance(self, destination): return self.__distance(self.__transform.position, destination) # Gets the distance between start and end @staticmethod def __distance(a, b): return pow(pow(a.x - b.x, 2) + pow(a.y - b.y, 2), .5) def distance_to_destination(self): return self.distance(self.__destination) # get the current position of rover def get_position(self): return self.__transform.position # get the current rotation def get_rotation(self): return self.__transform.rotation # this stops the hedge from recording positions def stop(self): self.__hedge.stop() self.gpg.stop() self.__cancel_early = True self.__thread_done = True # get the angle between two points and horizontal axis # a is the destination # b is the pivot point def get_angle(self, a, b): # get the angle between vector and horizontal axis dx = a.x - b.x dy = a.y - b.y rot_in_rad = math.atan2(dy, dx) # convert to 0-360 degrees rotate = math.degrees(rot_in_rad) if rotate < 0: rotate = 360 + rotate # print debug info if self.__debug: print("get angle returned: ", rotate) return rotate # travel to a given destination def goto_point(self, coord): # mark our new destination self.__destination = coord # default local variables sleep_tick = .00100 distance_threshold = self.__threshold previous_locations = [] self.__cancel_early = False # if we are already there, don't do anything if self.distance_to_destination() <= distance_threshold: self.__destination = None self.get_reached_point_callback() self.gpg.set_speed(self.__speed) return # prep to move self.turn_to_face(coord) self.check_for_obstacles() time.sleep(sleep_tick) dst = self.distance_to_destination() self.__determine_speed(dst) # time to move self.gpg.forward() # while we haven't found our destination while dst >= distance_threshold: # if we need to cancel early if self.__cancel_early: self.gpg.stop() self.__cancel_early = False self.gpg.set_speed(self.__speed) print("canceling early") return # update distance dst = self.distance_to_destination() self.get_position_callback() # check for obstacles self.check_for_obstacles() # slow down if needed self.__determine_speed(dst) time.sleep(sleep_tick) # prep for rotation re-evaluation previous_locations.append(Vector(self.__transform.position)) # have we been going straight long enough to determine our own rotation? if len(previous_locations) == 2: # are we on an intercept trajectory? corrections = self.__plot_intersection() if corrections != 0: # correct rotation self.gpg.stop() self.turn_to_face(coord) # reset self.__determine_speed(dst) self.gpg.forward() previous_locations = [] # we are going the wrong way and are lost elif self.__distance(previous_locations[0], self.__destination) < self.distance_to_destination(): # reorient ourselves self.gpg.stop() self.turn_to_face(coord) # reset self.__determine_speed(dst) self.gpg.forward() previous_locations = [] # everything is running fine, remove the oldest position else: previous_locations.pop(0) # print debug info if self.__debug: print("distance", dst) print("current position", self.__transform.position) print("current rotation", self.__transform.rotation) print("destination", coord) print("************************") # We found the destination do final status update self.gpg.stop() self.get_position_callback() self.get_reached_point_callback() self.__destination = None self.gpg.set_speed(self.__speed) # gps is inaccurate at times, so if we are close we should slowdown to increase fidelity. def __determine_speed(self, dst): # if we are approaching the destination, slow down for more accuracy if dst <= self.__threshold * 10: self.gpg.set_speed(self.__speed / 2) else: self.gpg.set_speed(self.__speed) # This method is used to calculate an intercept trajectory with the circle surrounding the destination point. # it returns 0 if the trajectory bisects at all (tangents are considered misses) # it then return -1 if the current angle is greater, or 1 if it is the opposite def __plot_intersection(self): direction = self.__transform.position - self.__destination vx = math.cos(self.__transform.rotation) vy = math.sin(self.__transform.rotation) a = pow(vx, 2) + pow(vy, 2) b = 2 * (vx * direction.x + vy * direction.y) c = pow(direction.x, 2) + pow(direction.y, 2) - pow(self.__threshold / 2, 2) det = pow(b, 2) - 4 * a * c # if it hits somehow if det < 0: return 0 else: angle = self.get_angle(self.__transform.position, self.__destination) angle -= self.__transform.rotation if angle < 0: return -1 else: return 1 # rotate left by degrees def __turn_left(self, degrees): if self.__debug: print("turning left.") self.gpg.rotate_left(abs(degrees),True) # rotate right by degrees def __turn_right(self, degrees): if self.__debug: print("turning right.") self.gpg.rotate_right(abs(degrees),True) # turn to a specific angle def turn_to_angle(self, angle): tolerable_deviation = 1 # determine how much we need to rotate difference = abs(self.__transform.rotation - angle) # show debug info if self.__debug: print("current rotation: ", self.__transform.rotation) print("destination angle: ", angle) print("rotation needed: ", difference) # if we are outside of our margin for error if difference > tolerable_deviation: # slow down for higher fidelity self.gpg.set_speed(self.__speed/2) # if our current angle exceeds the angle we need if self.__transform.rotation > angle: # debug info if self.__debug: print("current angle is greater.") # ensure we aren't about to turn more than halfway around if difference <= 180: # print debug info if self.__debug: print("difference is less than 180.") self.__turn_right(difference) # we were, let's go the other way else: # print debug info if self.__debug: print("difference is greater than 180.") self.__turn_left(360 - difference) # the angle we need it greater than our current angle else: # print debug info if self.__debug: print("destination angle is greater.") # ensure we aren't about to turn more than halfway around if difference <= 180: # print debug info if self.__debug: print("difference is less than 180.") self.__turn_left(difference) # we were, let's go the other way else: # print debug info if self.__debug: print("difference is greater than 180.") self.__turn_right(360 - difference) # we have an accurate reading now. self.gpg.set_speed(self.__speed) time.sleep(.01) # rotate to face a specific point def turn_to_face(self, coord): angle = self.get_angle(coord, self.__transform.position) self.turn_to_angle(angle) # checks for an obstacle and reports its position to a callback def check_for_obstacles(self): # check for obstacle self.position_update() distance = self.distance_sensor.read() # if it was close enough if distance <= self.__minimum_distance: # get its position and send to callback distance += DISTANCE_FROM_CENTER distance = distance / 100 x = (math.cos(math.radians(self.__transform.rotation)) * distance) + self.__transform.position.x y = (math.sin(math.radians(self.__transform.rotation)) * distance) + self.__transform.position.y pos = Vector(x, y) self.get_obstacle_callback(pos) else: distance = self.__minimum_distance # get its position and send to callback distance += DISTANCE_FROM_CENTER distance = distance / 100 x = (math.cos(math.radians(self.__transform.rotation)) * distance) + self.__transform.position.x y = (math.sin(math.radians(self.__transform.rotation)) * distance) + self.__transform.position.y pos = Vector(x, y) self.get_no_obstacle_callback(pos)
iterations = 100 # Continue waiting for client input while iterations > 0: print("Iteration #" + str(iterations) + ":") try: # Client asks for location data data = conn.recv(1024) print("Data Received:", data) if data == b'1': # Server sends location data # print("valuesImuRawData:", hedge.valuesImuRawData()) position_list = hedge.position()[1:3] print("Position List:", position_list) position_str = str(position_list[0]) + ", " + str(position_list[1]) print(position_str) conn.sendall(position_str.encode(encoding='UTF-8', errors='strict')) except Exception as e: print(e) conn.close() s.close() iterations -= 1 # Close TCP connection and socket conn.close() s.close()
def main(): # Setup for MarvelMind and data socket hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=10, debug=False) hedge.start() # Local JavaScript Socket socket_path = '/tmp/node-python-sock' client = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) client.connect(socket_path) # TCP Data Socket Master TCP_IP = '127.0.0.1' TCP_PORT = 5005 BUFFER_SIZE = 1024 s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.bind((TCP_IP, TCP_PORT)) tcp, addr = s.accept() # Variable declarations and initalization desired_x_1, desired_y_1, flight_x_1, flight_y_1 = 0, 0, 0, 0 desired_x_5, desired_y_5, flight_x_5, flight_y_5 = 0, 0, 0, 0 data_1, data_5 = {}, {} prev_position_1, prev_position_5 = [], [] data_log_1, data_log_5 = [], [] data_log_raw_1, data_log_raw_5 = [], [] initial_position_1, initial_position_5 = [], [] position = [] first_run_1, first_run_5 = True location_counter_1, location_counter_5 = 0, 0 f1 = open("flight_data_1.txt", "w") f5 = open("flight_data_5.txt", "w") # Loop to get location and fly drone while True: try: # Sleep delay. Change to change update speed # For multiple beacons, 0.125 works best 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 = calculate_initial_position( initial_position) first_run_1 = False # Zeroing for Hedgehog 5 elif (initial_position[0] == 5 and first_run_5 == True): initial_position_5 = calculate_initial_position( initial_position) first_run_5 = False # Gets drone position position = hedge.position() if (position[0] == 1 and first_run_1 == False): position_1 = calculate_position(position, prev_position_1, initial_position_1) prev_position_1 = copy.deepcopy(position_1) print("1: " + str(position_1)) # Positioning for Hedgehog 5 elif (position[0] == 5 and first_run_5 == False): position_5 = calculate_position(position, prev_position_5, initial_position_5) prev_position_5 = copy.deepcopy(position_5) print("5: " + str(position_5)) # Prompts user for desired flight location if (flight_x_1 == 0 and flight_y_1 == 0): # Sends 0 power commands to quad send_power(client, 0, 0, 0) # Gets location from user desired_x_1, desired_y_1 = get_desired_position() location_counter_1 += 1 if (flight_x_5 == 0 and flight_y_5 == 0): # Sends 0 power commands to quad send_power(tcp, 0, 0, 0) # Gets location from user desired_x_5, desired_y_5 = get_desired_position() location_counter_5 += 1 # Gets jsonData jsonData_1 = get_json_data(position_1, desired_x_1, desired_y_1) jsonData_5 = get_json_data(position_5, desired_x_5, desired_y_5) # Sends data to JavaScript (marvel_drone_socket.js) client.send(jsonData_1) # Sends data to TCP Client (Raspberry Pi) tcp.send(jsonData_5) # Ends infinite loop and closes threads except KeyboardInterrupt: client.close() tcp.close() hedge.stop() sys.exit()
def drive_to(self, fx, fy): """ :param fx: Number Final x position :param fy: Number Final y position """ robot = MarvelmindHedge(self.tty) robot.start() addr0, x0, y0, z0, t0 = robot.position() m, b = self.set_line(x0, y0, fx, fy) going_left = True going_right = True on_track = 0 finalerror = 10 while True: print(robot.position()) time.sleep(0.5) robot.print_position() addr, cx, cy, cz, ts = robot.position() dx, dy = fx - cx, fy - cy ex = (cy - b) / m displacement = cx - ex if abs(dx) < finalerror and abs(dy) < finalerror: robot.stop() break elif displacement < -self.epsilon: if going_left: if on_track == 0: on_track += 1 elif on_track == 1: on_track += 1 else: self.alpha -= 5 self.turn_angle(self.alpha) going_left = False going_right = True self.forward(10) # move forward 10 cm elif going_right: on_track = 0 self.alpha += 10 self.turn_angle(self.alpha) self.forward(10) elif displacement > self.epsilon: if going_right: if on_track == 0: on_track += 1 elif on_track == 1: on_track += 1 else: self.alpha -= 5 self.turn_angle(-self.alpha) going_left = True going_right = False self.forward(10) elif going_left: on_track = 0 self.alpha += 10 self.turn_angle(-self.alpha) self.forward(10)
def main(): hedge = MarvelmindHedge(tty = "/dev/ttyACM0", adr=10, debug=False) # create MarvelmindHedge thread hedge.start() # start thread # next create a socket object s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) print ("Socket successfully created") # reserve a port on your computer in our # case it is 12345 but it can be anything port = 12345 # Next bind to the port # we have not typed any ip in the ip field # instead we have inputted an empty string # this makes the server listen to requests # coming from other computers on the network s.bind(('', port)) # put the socket into listening mode s.listen(5) print ("socket is listening") c, addr = s.accept() while True: try: sleep(1) matrix= hedge.valuesImuData rawdata=hedge.valuesImuRawData position=hedge.position() # quaternion_to_euler_angle(matrix[0][3],matrix[0][4],matrix[0][5],matrix[0][6]) i=0 w=0 x=0 y=0 z=0 xGaussData=0 yGaussData=0 while(i < 3): w=matrix[i][3]+w x=matrix[i][4]+x y=matrix[i][5]+y z=matrix[i][6]+z xGaussData=rawdata[i][6]+xGaussData yGaussData=rawdata[0][7]+yGaussData i=i+1 yGaussData=yGaussData/3 xGaussData=xGaussData/3 d=(math.atan2(yGaussData,xGaussData))* (180/math.pi) data=str(position[1])+","+str(position[2])+","+str(d) print("d is:",d) quaternion_to_euler_angle(w/3,x/3,y/3,z/3) print ('Got connection from', addr) c.send(data.encode('utf-16')) while(c.recv(2048).decode('utf-16')!="ack"): print("waiting for ack") print ("ack received!") #c.close() # 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() c.close() c.close()
#initialize Xbox controller joy = xbox.Joystick() #create socket UDPSock = socket(AF_INET, SOCK_DGRAM) #Marvelmind hedge setup hedge = MarvelmindHedge(tty="/dev/ttyAMC0", adr=10, debug=False) hedge.start() hedge.print_position() print("Press Back button to exit") #Runs through everything and sends it to target computer while not joy.Back(): hedge_pos = hedge.position() controller = get_input(joy) print controller for i in hedge_pos: UDPSock.sendto(str(i), addr) (data, addr) = UDPSock.recvfrom(buf) for i in controller: UDPSock.sendto(str(i), addr) (data, addr) = UDPSock.recvfrom(buf) UDPSock.sendto("exit", addr) #clean up UDPSock.close() os._exit(0) joy.close()