自己动手：潜艇，探索莫斯科河的水域。第一部分

The registration deadline is 30 June, 2016 (11:59 PM UTC/4:59 PM PST). The registration fee is $2,000.00 USD.

import math,android,time
coordmas = [[55.671110, 37.686625],[55.668448, 37.675467],[55.660847, 37.671776],[55.654649, 37.671175]]
droid = android.Android()
droid.startSensingTimed(1,200)
droid.startLocating(5000, 30)
def getgps():
	locs = droid.getLastKnownLocation()
	gpspos = locs.result["gps"]
	return gpspos
def distazim(llat1,llong1,llat2,llong2):
	rad=6372795
	#
	lat1=llat1*math.pi/180.
	lat2=llat2*math.pi/180.
	long1=llong1*math.pi/180.
	long2=llong2*math.pi/180.
	cl1=math.cos(lat1)
	cl2=math.cos(lat2)
	sl1=math.sin(lat1)
	sl2=math.sin(lat2)
	delta=long2-long1
	cdelta=math.cos(delta)
	sdelta=math.sin(delta)
	y=math.sqrt(math.pow(cl2*sdelta,2)+math.pow(cl1*sl2-sl1*cl2*cdelta,2))
	x=sl1*sl2+cl1*cl2*cdelta
	ad=math.atan2(y,x)
	dist=ad*rad
	x=(cl1*sl2)-(sl1*cl2*cdelta)
	y=sdelta*cl2
	z=math.degrees(math.atan(-y/x))
	if(x<0):
		z=z+180.
	z2=(z+180.)%360.-180.
	z2=-math.radians(z2)
	anglerad2=z2-((2*math.pi)*math.floor((z2/(2*math.pi))))
	angledeg=(anglerad2*180.)/math.pi
	return [dist,angledeg]
def servoangle(azdiff):
	if azdiff>10 or azdiff<-10:
		deg=azdiff
		if deg>90:
			deg=90
		if deg<-90:
			deg=-90
	return deg
gpspos=getgps()
oldlat = gpspos["latitude"]
oldlon = gpspos["longitude"]
for c in range(len(coordmas)):
	#    
	curcoord=coordmas[c]
	targetlat=curcoord[0]
	targetlon=curcoord[1]
	darange=11;
	while darange>10:
		gpspos=getgps()
		curlat = gpspos["latitude"]
		curlon = gpspos["longitude"]
		time.sleep(0.5)
		da=distazim(curlat,curlon,targetlat,targetlon)
		darange = da[0]
		dazimut = round(da[1])
		pol=droid.sensorsReadOrientation()
		pol2=pol.result
		turn = round(pol2[0])
		turn = (-turn) *180/ 3.2
		azdiff=turn-dazimut
		deg=servoangle(azdiff)
	oldlat = curlat
	oldlon = curlon

import math,android,time

# 
coordmas = [[55.671110, 37.686625],[55.668448, 37.675467],[55.660847, 37.671776],[55.654649, 37.671175]]

#,  GPS, 
droid = android.Android()
droid.startSensingTimed(1,200)
droid.startLocating(5000, 30)

#
#GPS
def getgps():
	locs = droid.getLastKnownLocation()
	gpspos = locs.result["gps"]
	return gpspos
#       GPS-
def distazim(llat1,llong1,llat2,llong2):
	rad=6372795
	#
	lat1=llat1*math.pi/180.
	lat2=llat2*math.pi/180.
	long1=llong1*math.pi/180.
	long2=llong2*math.pi/180.

	#
	cl1=math.cos(lat1)
	cl2=math.cos(lat2)
	sl1=math.sin(lat1)
	sl2=math.sin(lat2)
	delta=long2-long1
	cdelta=math.cos(delta)
	sdelta=math.sin(delta)

	#
	y=math.sqrt(math.pow(cl2*sdelta,2)+math.pow(cl1*sl2-sl1*cl2*cdelta,2))
	x=sl1*sl2+cl1*cl2*cdelta
	ad=math.atan2(y,x)
	dist=ad*rad

	#
	x=(cl1*sl2)-(sl1*cl2*cdelta)
	y=sdelta*cl2
	z=math.degrees(math.atan(-y/x))

	if(x<0):
	z=z+180.

	z2=(z+180.)%360.-180.
	z2=-math.radians(z2)
	anglerad2=z2-((2*math.pi)*math.floor((z2/(2*math.pi))))
	angledeg=(anglerad2*180.)/math.pi

	return [dist,angledeg]

def servoangle(azdiff):
	if azdiff>10 or azdiff<-10:
		deg=azdiff
		if deg>90:
			deg=90
		if deg<-90:
			deg=-90
	return deg

#     
gpspos=getgps()
oldlat = gpspos["latitude"]
oldlon = gpspos["longitude"]

#
for c in range(len(coordmas)):
	#    
	curcoord=coordmas[c]
	targetlat=curcoord[0]
	targetlon=curcoord[1]

	#     10   .
	darange=11;
	while darange>10:
		# 	
		gpspos=getgps()
		curlat = gpspos["latitude"]
		curlon = gpspos["longitude"]
		time.sleep(0.5)
		#     . .
		da=distazim(curlat,curlon,targetlat,targetlon)
		darange = da[0]
		dazimut = round(da[1])
		#  
		pol=droid.sensorsReadOrientation()
		pol2=pol.result
		turn = round(pol2[0])
		#[1.57=left, 0=stop, -1.57=right]
		turn = (-turn) *180/ 3.2
		#        10     
		azdiff=turn-dazimut
		#      
		deg=servoangle(azdiff)
		#     

	#   , ..     
	oldlat = curlat
	oldlon = curlon