DIY: A robot submarine to explore the waters of the Moscow River. Part 1

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