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)