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 __init__(self, forwardpin, leftpin, rightpin, destinationpin,
modeindicatorpin):
##position##
self.pos = currentposition()
self.pos.position()
self.x1 = self.pos.position()[1]
self.y1 = self.pos.position()[2]
sleep(.5)
self.x2 = self.pos.position()[1]
self.y2 = self.pos.position()[2]
self.x3 = 5.4
self.y3 = 2.8
self.targetnumber = 2
self.deltaT = 1.0
self.hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=16, debug=False)
self.hedge.start()
###TRIG###
self.trig = trig()
###USS###
# IO setup
self.forwardpin = forwardpin
self.rightpin = rightpin
self.leftpin = leftpin
self.destinationpin = destinationpin
self.modeindicatorpin = modeindicatorpin
IO.setmode(IO.BCM)
IO.setup(self.forwardpin, IO.OUT)
IO.setup(self.rightpin, IO.OUT)
IO.setup(self.leftpin, IO.OUT)
IO.setup(self.destinationpin, IO.OUT)
IO.setup(self.modeindicatorpin, IO.OUT)
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
from marvelmind import MarvelmindHedge 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()
#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([-1, 1])
axes.set_ylim([-1, 1])
global hedge
hedge = MarvelmindHedge(tty="/dev/ttyACM0",
adr=None,
recieveUltrasoundPositionCallback=update_line
) # create MarvelmindHedge thread
hedge.start()
#plotThread = Thread(target=printThread) # create and start console out thread
plotProcess = Process(
target=printThread
) #creo un proceso, mejor que thread porque puedo detener y ble
plotProcess.start()
#plotThread.start()
plt.show()
# main()
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)
from nanpy import ArduinoApi, SerialManager
from marvelmind import MarvelmindHedge
from time import sleep
from Point import Point
from Vector import Vector
#Set up the arduino as a slave to the raspberry pi through a serial port
c = SerialManager(device='/dev/ttyACM0')
a = ArduinoApi(connection=c)
#Arduino Motor Control Pins
dirA = 8
dirB = 10
pwmA = 9
pwmB = 11
# initialize the MarvelMindHedge GPS
hedge = MarvelmindHedge(tty="/dev/ttyACM2", adr=9, debug=False)
#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)
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()
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 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()
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()
counter = 0
offset = 0
heading = 0
heading2 = 0
range = 0
direction = 0
filecounter = 0
now = 0
old_time = int(round(time.time() * 1000))
old_error = 0
i_term = 0
radians_degrees = 57.3 # constant to convert from radians to degrees
# initiate local positioning
hedge = MarvelmindHedge(tty="/dev/ttyACM0",
maxvaluescount=1,
adr=hedgehog_id,
debug=False) # create MarvelmindHedge thread
hedge.start() # start thread
time.sleep(1) # pause to let it settle
class BluetoothDeviceManager(gatt.DeviceManager):
robot = None # root robot device
def device_discovered(self, device):
print("[%s] Discovered: %s" % (device.mac_address, device.alias()))
self.stop_discovery() # Stop searching
self.robot = RootDevice(mac_address=device.mac_address, manager=self)
self.robot.connect()
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()
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()
def __init__(self):
self.hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=16, debug=False)
self.hedge.start() #start thread
from math import sqrt
from marvelmind import MarvelmindHedge
import time
port = "/dev/tty.usbmodem1451"
hedge = MarvelmindHedge(port)
hedge.start()
time.sleep(2)
print ("starting")
dmin = 700
dmax = 1023
dd = dmax - dmin
base_speed = 800
#100 - rm faster + base speed
#- 100 lm faster + base speed
def duty_offset(distance):
global base_speed
offset = distance * 10
return base_speed + offset
right = base_speed
left = base_speed
for robot in self.robots:
robot.turn(positions)
for robot in self.robots:
robot.set_line()
def run(self):
"""
runs robot swarm to final positions
:return: None
"""
self.start()
while not self.done:
print('positions asked')
positions = self.get_positions() # marvelmind chokes speed
print('positions:', positions)
for robot in self.robots:
print(robot, 'running')
robot.run(positions) # web requests choke speed
print(robot, 'run')
self.check_done()
for robot in self.robots:
robot.when_done()
print('SWARM DONE!')
hedge = MarvelmindHedge('/dev/tty.usbmodem1421')
hedge.start()
time.sleep(2)
swarm = RobotSwarm('swarm.txt')
swarm.run()
recordwriter.writerow([
"Time", "Marvelmind X", "Marvelmind Y", "Fake Marvel X",
"Fake Marvel Y", "Realsense X", "Realsense Y"
])
# 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]
self.move('turn', self.alpha)
self.on_track = 0
time.sleep(.2)
def run(self):
while not self.done:
self.move('forward', 1)
time.sleep(.2)
self.turn()
def make_robot(robot):
name, position = robot.split(":")
number, ip = name.split(" ")
endx, endy = position.split(",")
return number, ip, endx, endy
filename = "ma.txt"
f = open(filename)
lines = f.readlines()
f.close()
robot_list = []
for line in lines:
n, ip, ex, ey = make_robot(line)
robot_list.append(DrivingRobot(n, ip, ex, ey))
print(robot_list)
hedgeswarm = MarvelmindHedge(tty='/dev/tty.usbmodem1421')
import sys
order = [2, 4, 6, 8, 10, 12, 1, 3, 5, 7, 9, 11]
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 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)
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()
from marvelmind import MarvelmindHedge
from time import sleep
import socket
import sys
# Globals
HOST = '' # Symbolic name meaning all available interfaces
PORT = 50007 # Arbitrary non-privileged port
# Instantiate hedge location tracking
hedge = MarvelmindHedge(tty="/dev/ttyACM0") # create MarvelmindHedge thread
hedge.start() # start thread
# Set up TCP socket
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind((HOST, PORT))
s.listen(1)
print("Server listening on port 50007...")
# Accept client connection
conn, addr = s.accept()
print('Successfully connected with ' + addr[0] + '...')
iterations = 100
# Continue waiting for client input
while iterations > 0:
print("Iteration #" + str(iterations) + ":")
try:
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:
import math ##position## pos = currentposition() pos.position() x1 = pos.position()[1] y1 = pos.position()[2] sleep(.5) #pos.position() x2 = pos.position()[1] y2 = pos.position()[2] x3 = 5.4 y3 = 2.8 targetnumber = 2 deltaT = 1.0 hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=16, debug=False) hedge.start() ###TRIG### trig = trig() ###USS### ###motorcontrol### in1 = 6 #motor driver in2 = 5 en = 12 in3 = 1 in4 = 7 en2 = 13
