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