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