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
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)
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():
global hedge
global dt_vision # ローカルに配置しておいた俯瞰図landscapeを視野画像の風景としてオンメモリーにロードしておくため
global angle
# X=20, y=20, angle=90の設定では
# Loaderの初期位置は原点（#73と#39ビーコンの間の角）に対して（20ポチ,20ポチ)=(0.16m, 0.16m)のところで#39ビーコンの方角を向いている
dt_vision = SpoolMobileVision(59,
20,
20,
angle,
4,
0,
scale,
landscape=Image.open(landscape_file_path))
hedge = MarvelmindHedge(
adr=59,
tty="/dev/ttyACM0",
baud=38400,
maxvaluescount=12,
recieveImuDataCallback=updateMiniatureWarehouseReferenceFrameData,
debug=False)
hedge.start()
while True:
try:
pass # time.sleep(0.020)
except KeyboardInterrupt:
hedge.stop() # stop and close serial port
sys.exit()
def main():
#create plot
global fig
fig = plt.figure()
ax = fig.add_subplot(111)
ax.plot([], [], 'ro')
ax.grid(True)
bx = fig.add_subplot(111)
bx.plot([], [], 'bo')
plt.axis('equal')
axes = plt.gca()
axes.set_xlim([-500, 500])
axes.set_ylim([-500, 500])
global hedge
hedge = MarvelmindHedge(tty="/dev/tty.usbmodem1411",
recieveDataCallback=update_line)
# create MarvelmindHedge thread
hedge.start()
plotThread = Thread(
target=printThread) # create and start console out thread
plotThread.start()
plt.show()
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()
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
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=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()
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.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()
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()
def main():
#read map
map_path = 'map.txt'
map = open(map_path, 'r')
counter = 0
map_lines = []
for line in map:
if counter%2 == 0:
map_pts = eval(line)
elif counter%2 == 1:
map_lines = eval(line)
counter += 1
line_pts = []
for pair in map_lines:
line_pt = [tuple(map_pts[pair[0]]), tuple(map_pts[pair[1]])]
line_pts.append(line_pt)
lc = mc.LineCollection(line_pts, linewidths=1)
#create plot
global fig
fig = plt.figure()
ax = fig.add_subplot(111)
ax.plot([],[], 'ro')
#ax.add_collection(lc)
ax.grid(True)
bx = fig.add_subplot(111)
bx.plot([],[], 'bo')
plt.axis('equal')
cx = fig.add_subplot(111)
cx.add_collection(lc)
axes = plt.gca()
axes.set_xlim([0,10])
axes.set_ylim([-5,5])
global hedge
hedge = MarvelmindHedge(tty = "/dev/ttyACM2", adr=10, recieveUltrasoundPositionCallback=update_line) # create MarvelmindHedge thread
hedge.start()
plotThread = Thread(target=printThread) # create and start console out thread
plotThread.start()
plt.show()
class Marvelmind:
def __init__(self, adr):
for port in comports():
if port.vid == marvelmind_vid:
self.hedge = MarvelmindHedge(tty=port.device,
adr=adr,
debug=False)
break
else:
raise IOError("Marvelmind position sensor not found")
self.hedge.start()
def read(self):
hedge_id, x, y, z, angle, timestamp = self.hedge.position()
return np.array([x, y])
def stop(self):
self.hedge.stop()
def marvelThread(status, GPSQueue):
hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=10,
debug=False) # create MarvelmindHedge thread
hedge.start() # start thread
while status[0]:
sleep(.1)
#Number of beacon, X, Y, Z, Time
GPSQueue.put(GPSCoord(hedge.position()))
print("The Marvel Thread: ")
hedge.print_position()
t = hedge.position()
f = open('dataouts.txt', 'a')
f.write('Marvel = {}'.format(t) + '\n')
f.close()
if not status[0]:
GPSQueue.put(GPSCoord([-1, -1, -1, -1, -1]))
print("Ended Marvel Queue")
def main():
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=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/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()
hedge.valuesImuData[-1][0], hedge.valuesImuData[-1][1],
hedge.valuesImuData[-1][2]
])
plotter.data[-1]['a_size'] = 25
plotter.imudata.append([
hedge.valuesImuData[-1][0], hedge.valuesImuData[-1][1],
hedge.valuesImuData[-1][2]
])
if (len(plotter.data) > plotter.pointlimiter + len(plotter.datastatic)):
plotter.data = plotter.data[-plotter.pointlimiter:]
plotter.data = np.append(plotter.datastatic, plotter.data)
global hedge
hedge = MarvelmindHedge(
tty="/dev/tty.usbmodem1421",
adr=20,
recieveUltrasoundPositionCallback=updateUSNavData,
recieveImuDataCallback=updateAccData,
debug=False
) # create MarvelmindHedge thread recieveAccelerometerDataCallback=updateAccData
import plotter3d2
from vispy import app
global plotter
plotter = plotter3d2.Canvas()
hedge.start()
app.run()
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)
from marvelmind import MarvelmindHedge
import threading
import time
import sys
from flask import Flask, render_template
global Position
from flask_socketio import SocketIO, send, emit
hedge = MarvelmindHedge(tty="COM4", adr=2, debug=False) # create MarvelmindHedge thread
hedge.start() # start thread
app = Flask(__name__)
@app.route("/")
def server():
print(hedge.IMUData())
while True:
return str(hedge.IMUData())x
@app.route("/test")
def test():
print(hedge.IMUData())
while True:
return render_template('reload.html', row=str(hedge.IMUData()))
@app.route("/test2")
def test():
print(hedge.IMUData())
while True:
row = str(hedge.IMUData())
socketio.emit('message', {'msg': 'test', 'text':row}, broadcast =True)
return render_template('reload.html')
if __name__=='__main__':
app.run(debug=False)
def manual():
global manual_init
global joystick
if manual_init:
manual_init = False
joystick = pygame.joystick.Joystick(0)
joystick.init()
try:
jesses_handler(pygame.event.get(), joystick)
except KeyboardInterrupt:
motor_off()
HEDGE = MarvelmindHedge(tty="/dev/ttyACM0", adr=10, debug=False)
HEDGE.start()
#HEDGE = fakehedge.Schmedge()
auton_init = True
def automatic(X, Y, HEDGE):
global auton_init
global pos, x, y, xdiff, ydiff, m_i
if auton_init:
auton_init = False
pos = HEDGE.position()
x = pos[1]
y = pos[2]
xdiff = X - x
ydiff = Y - y
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()
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:
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)
