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(): 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) 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/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(): 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()
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 main(): 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(): # 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(): 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(): hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=12, debug=False) # create MarvelmindHedge thread hedge.start() # start thread pos = hedge.position() while True: try: sleep(1) print(pos) # 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 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 array returned by the Marvelmindhedge fxn def hedgeQuit(self): self.hedge.stop()
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/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(): 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/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()
def main(): hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=None, debug=False) # create MarvelmindHedge thread if (len(sys.argv) > 1): hedge.tty = sys.argv[1] hedge.start() # start thread while True: try: hedge.dataEvent.wait(1) hedge.dataEvent.clear() if (hedge.positionUpdated): hedge.print_position() if (hedge.distancesUpdated): hedge.print_distances() if (hedge.rawImuUpdated): hedge.print_raw_imu() if (hedge.fusionImuUpdated): hedge.print_imu_fusion() if (hedge.telemetryUpdated): hedge.print_telemetry() if (hedge.qualityUpdated): hedge.print_quality() if (hedge.waypointsUpdated): hedge.print_waypoint() except KeyboardInterrupt: hedge.stop() # stop and close serial port sys.exit()
def main(): hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=9, debug=False) hedge.start() My_hedge = hedge_Positions() #Motor 1 pins (assumed front right) Motor3 = Motor(17, 27) Motor3.front = True Motor3.right = True #Motor 2 pins (assumed back right) Motor2 = Motor(22, 23) Motor2.right = True Motor2.back = True #Motor 3 pins (assumed front left) Motor4 = Motor(24, 25) Motor4.left = True Motor4.front = True #Motor 4 pins (assumed back left) Motor1 = Motor(5, 6) Motor1.left = True Motor1.back = True #Current direction flags. These will determine wether or not the robot needs to decelerate before changing directions Left_dir_flag = False Right_dir_flag = False Back_dir_flag = False Front_dir_flag = False #PWM wave setup Power_a = PWM(18, 100) Power_b = PWM(13, 100) while True: try: My_hedge.update_position() print('position updated') print(My_hedge.Movement_needed) if My_hedge.Movement_needed[0] > 0.1: print('Moving Left') if Left_dir_flag == False: #all decelerate PWM_a.Accelerate(0, 0.05, -1) PWM_b.Accelerate(0, 0.05, -1) #Set direction for left movement Motor1.set_direction(0) Motor2.set_direction(1) Motor3.set_direction(1) Motor4.set_direction(0) Left_dir_flag = True #all accelerate PWM_a.Accelerate(50, 0.05, 1) PWM_b.Accelerate(50, 0.05, 1) elif (My_hedge.Movement_needed[0] < 0.1) and (My_hedge.Movement_needed[0] > -0.1): #move ahead if My_hedge.Movement_needed[1] > 0.1: print('Moving forward') if Front_dir_flag == False: #all decelerate PWM_a.Accelerate(0, 0.05, -1) PWM_b.Accelerate(0, 0.05, -1) #Set direction for forward movement Motor1.set_direction(0) Motor2.set_direction(0) Motor3.set_direction(0) Motor4.set_direction(0) Front_dir_flag = True #all accelerate PWM_a.Accelerate(50, 0.05, 1) PWM_b.Accelerate(50, 0.05, 1) elif (My_hedge.Movement_needed[1] < 0.1) and (My_hedge.Movement_needed[1] > -0.1): #At position print('Destination acheived') hedge.stop() sys.exit() #all decelerate else: print('Moving backwards') if Back_dir_flag == False: #all decelerate PWM_a.Accelerate(0, 0.05, -1) PWM_b.Accelerate(0, 0.05, -1) #Set direction for backwards movement Motor1.set_direction(1) Motor2.set_direction(1) Motor3.set_direction(1) Motor4.set_direction(1) Back_dir_flag = True #all accelerate PWM_a.Accelerate(50, 0.05, 1) PWM_b.Accelerate(50, 0.05, 1) else: print('Moving right') if Right_dir_flag == False: #all decelerate PWM_a.Accelerate(0, 0.05, -1) PWM_b.Accelerate(0, 0.05, -1) #Set direction for right movement Motor1.set_direction(1) Motor2.set_direction(0) Motor3.set_direction(0) Motor4.set_direction(1) Right_dir_flag = True #all accelerate PWM_a.Accelerate(50, 0.05, 1) PWM_b.Accelerate(50, 0.05, 1) time.sleep(1) except KeyboardInterrupt: break #hedge.stop() #sys.exit() print('Now outside of While loop') hedge.stop() sys.exit()
direction) rc.ResetEncoders(address) if (heading < desired_angle - 0.25) or ( heading > desired_angle + 0.25 ): # go fast if you're more than .25 radians away speed = 2500 # go fast else: speed = 1500 # go slow rc.SpeedDistanceM1(address, -1 * direction * speed, 1 * tickdistanceL, 1) # rotate a few degrees rc.SpeedDistanceM2(address, direction * speed, 1 * tickdistanceR, 1) buffers = (0, 0, 0) while (buffers[1] != 0x80 and buffers[2] != 0x80 ): #Loop until distance command has completed # displayspeed() buffers = rc.ReadBuffers(address) new_waypoint = False rc.ResetEncoders(address) turn = False except KeyboardInterrupt: rc.ForwardM1(address, 0) # kill motors rc.ForwardM2(address, 0) pipe.stop() if (datalog or recordmode): csvfile.close() # close the file you've been writing if use_marvelmind: hedge.stop() # stop and close serial port
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 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()
while 1: pygame.event.pump() pressed = pygame.key.get_pressed() a_held = pressed[pygame.K_a] m_held = pressed[pygame.K_m] if a_held: auton = True elif m_held: auton = False if auton: if automatic(auton_target[0], auton_target[1], HEDGE): auton_init = False if auton_target == (5.7, -4.2): auton_target = (8.5, -1.7) elif auton_target == (8.5, -1.7): auton_target = (4.5, 2.5) elif auton_target == (4.5, 2.5): auton = False auton_init = True auton_target = (5.7, -4.2) else: manual() try: main() except KeyboardInterrupt: motor_off() HEDGE.stop() sys.exit()
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: if((element<(X1+STD)) and (element>(X1-STD))): XNew.append(element) print XNew X1=sum(XNew)/len(XNew) STD=0 for counter in positionY: STD=STD+math.pow((Y1-counter),2) STD=math.sqrt(STD)/(len(positionY)-1) YNew=list() for element in positionY: if((element<(Y1+STD)) and (element>(Y1-STD))): YNew.append(element) print YNew Y1=sum(YNew)/len(YNew)''' print X1,'',Y1 hedge.stop()
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)
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)
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: sleep(1) current_psition = hedge2.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() hedge2.stop() X2 = 100 * (sum(positionX) / len(positionX)) Y2 = 100 * (sum(positionY) / len(positionY)) print 'hedge2', X2, '', Y2
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()