def resurface():
global tolerance
global location
while location[2] > tolerance:
cmdAgent.cmdAgent(Rise)
getPosition.getPosition()
return 0
#main code begins here setBoundary.setBoundary(bounds[0],bounds[1],bounds[2]) response = cmdAgent.cmdAgent(PwrOn) print "hi" print response difftime = time.clock() + prevtime #stringtime = str(difftime) #local variables endx = 0 beginy = 0 endy = 0 beginz = 0 endz = 0 #functional loop while i < 11: elapsetime+=time.clock() #check if destination has been reached if (location == destination) or (location > bounds):
import serial
import fgets
import cmdAgent
from array import*
port = '/dev/ttyACM0'
baud = 57600
ser = serial.Serial(port, baud, timeout=1)
ser.open()
location = 0
yedp = list()
maxnumber = 30
def avoid():
global location
yedp(30)
with open('/home/pi/Eco-Dolphin1/ControlCenter_lkg_06112015/position.txt' , 'r') as
positionfile
data = postionfile.readline()
if location-data < tol:
if location[2] < data[2]:
cmdAgent.cmdAgnet(dive)
else :
cmdAgent.cmdAgent(rise)
#fclose(f)
ygraphpos = list()
zgraphpos = list()
#Cp = measure battery
#loop counter
i = 0 #for file population (maximum: 100)
n = 0 #for IMU calibration (average acceleration)
#Serial communication definition and initialization
ser = serial.Serial(port, baud, timeout=1)
ser.open()
#main code begins here
setBoundary.setBoundary(bounds[0], bounds[1], bounds[2])
response = cmdAgent.cmdAgent(PwrOn)
print "hi"
print response
difftime = time.clock() + prevtime
#stringtime = str(difftime)
#local variables
endx = 0
beginy = 0
endy = 0
beginz = 0
endz = 0
#functional loop
while i < 11:
elapsetime += time.clock()
#check if destination has been reached
if (location == destination) or (location > bounds):
def hover(x, y, z):
global location
global tolerance
currenttime = time.clock()
targettime = time.clock() + 10
while (currenttime < targettime):
currenttime += time.clock()
#get current position
if (y < location[1] + tolerance):
cmdAgent.cmdAgent(Fwd)
elif (y > location[1] + tolerance):
cmdAgent.cmdAgent(Back)
else:
cmdAgent.cmdAgent(Idle)
if (x < location[0] + tolerance):
cmdAgent.cmdAgent(Right)
elif (x > location[0] + tolerance):
cmdAgent.cmdAgent(Left)
else:
cmdAgent.cmdAgent(Idle)
if (z < location[2] + tolerance):
cmdAgent.cmdAgent(Rise)
elif (z > location[2] + tolerance):
cmdAgent.cmdAgent(Dive)
else:
cmdAgent.cmdAgent(Idle)
def getPosition():
global acceleration
global location
global prevaccel
global diffaccel
global response
global prevlocation
global summ
global xgraphpos
global ygraphpos
global zgraphpos
#get the acceleration and separate string
cmdAgent.cmdAgent(PwrOn)
response = cmdAgent.cmdAgent(Accel)
print "hi2"
print response
accelstring = separateString.separateString(response)
print "hi3"
if isfloat(accelstring[0]) and isfloat(accelstring[1]) and isfloat(
accelstring[2]):
#convert and assign acceleration values
accelx = float(accelstring[0])
accely = float(accelstring[1])
accelz = float(accelstring[2])
acceleration = [accelx, accely, accelz]
print "acceleration"
print acceleration
#record the acceleration in a file
fob.write('Accel: ')
fob.write('%03d, ' % acceleration[0])
fob.write('%03d, ' % acceleration[1])
fob.write('%03d\n' % acceleration[2])
#calculate the change in acceleration
diffaccel[0] = acceleration[0] - prevaccel[0]
diffaccel[1] = acceleration[1] - prevaccel[1]
diffaccel[2] = acceleration[2] - prevaccel[2]
magaccel = math.sqrt((math.pow(diffaccel[0], 2)) +
(math.pow(diffaccel[1], 2)) +
(math.pow(diffaccel[2], 2)))
#assign previous acceleration to the current acceleration
prevaccel = acceleration
#establish current location
location[0] = 0.5 * diffaccel[0] * math.pow(difftime, 2)
location[1] = 0.5 * diffaccel[1] * math.pow(difftime, 2)
location[2] = 0.5 * diffaccel[2] * math.pow(difftime, 2)
#the vector summ shows the position
summ[0] += location[0] + prevlocation[0]
summ[1] += location[1] + prevlocation[1]
summ[2] += location[2] + prevlocation[2]
print "This is position"
print summ
#assign previous location to current location
summ = prevlocation
#publish location and elapsed time to file server
fob2.write('Place/Time: ')
fob2.write('%03d, ' % summ[0])
fob2.write('%03d, ' % summ[1])
fob2.write('%03d, ' % summ[2])
fob2.write('%03d\n' % elapsetime)
xgraphpos.append(summ[0])
ygraphpos.append(summ[1])
xgraphpos.append(summ[2])
def hover(x, y, z):
global location
global tolerance
currenttime = time.clock()
targettime = time.clock()+10
while(currenttime < targettime):
currenttime+=time.clock()
#get current position
if(y < location[1]+tolerance):
cmdAgent.cmdAgent(Fwd)
elif(y > location[1]+tolerance):
cmdAgent.cmdAgent(Back)
else:
cmdAgent.cmdAgent(Idle)
if(x < location[0]+tolerance):
cmdAgent.cmdAgent(Right)
elif(x > location[0]+tolerance):
cmdAgent.cmdAgent(Left)
else:
cmdAgent.cmdAgent(Idle)
if(z < location[2]+tolerance):
cmdAgent.cmdAgent(Rise)
elif(z > location[2]+tolerance):
cmdAgent.cmdAgent(Dive)
else:
cmdAgent.cmdAgent(Idle)
