# Demo entry 6719535

Submitted by Xin Huang on Mar 12, 2018 at 04:07
Language: Python 3. Code size: 4.0 kB.

```#!/usr/bin/env python3
# coding=utf-8
# Author: Xin
# Version 2.0
# bug fixed

#Define
WaitTime = 2

from gps3 import agps3
import RPi.GPIO as GPIO
import time
from math import radians, cos, sin, asin, sqrt
import math
import numpy as np

def stop():
GPIO.output(38, 0)
GPIO.output(40, 0)

def left():
GPIO.output(38, 0)
GPIO.output(40, 1)

def right():
GPIO.output(38, 1)
GPIO.output(40, 0)

def forward():
GPIO.output(38, 1)
GPIO.output(40, 1)

def GetCurrentGPS():
for new_data in gps_socket:
if new_data:
data_stream.unpack(new_data)
global LatCurrent
LatCurrent = data_stream.lat
global LonCurrent
LonCurrent = data_stream.lon
if((LonCurrent != 'n/a') and (LatCurrent != 'n/a')):
print('Current Longitude = ', data_stream.lon)
print('Current Latitude = ', data_stream.lat)
print('Current Altitude = ', data_stream.alt)
break

#计算两点间距离-m
def geodistance(lng1,lat1,lng2,lat2):
lng1, lat1, lng2, lat2 = map(radians, [lng1, lat1, lng2, lat2])
dlon=lng2-lng1
dlat=lat2-lat1
a=sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
dis=2*asin(sqrt(a))*6371*1000
return dis

def check():
print('\nCheck the route...')
stop()
LonLast = LonCurrent
LatLast = LatCurrent
DistRemain = geodistance(float(LonCurrent),float(LatCurrent),float(LonGoal),float(LatGoal))
if(DistRemain < 0.3):
ArriveFlag = 1
return
print('The remain distance is',DistRemain,'m')
GetCurrentGPS()
x1 = LonCurrent - LonLast
y1 = LatCurrent - LatLast
u = float(LonGoal) - LonLast
v = float(LatGoal) - LatLast
if(x1==0 and y1 == 0):
x1 = u
y1 = v
x=np.array([x1*10000,y1*10000])
y=np.array([u*10000,v*10000])
Lx=np.sqrt(x.dot(x))
Ly=np.sqrt(y.dot(y))
#theta = acos((x1*v - u*y1)/(sqrt(x1**2+y1**2))*sqrt(u**2+v**2))
#theta = math.degrees(theta)
theta = x.dot(y)/(Lx*Ly)
angle1=np.arccos(theta)
angle2=angle1*360/2/np.pi
theta = angle2
print('The angle of error is',theta,'degrees')
if(theta >= 10):
TurningTime = 100*theta/10
print('Turning right')
right()
time.sleep(TurningTime)
elif (theta <= -10):
TurningTime = -100*theta/10
print('Turning left')
left()
time.sleep(TurningTime)
else:
print('Go straight')
stop()
time.sleep(1)
forward()

#------------------------main----------------------------
#inital command
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(38, GPIO.OUT)
GPIO.setup(40, GPIO.OUT)
stop() #stop at first

#Set a goal
LatGoal = input()
LonGoal = input()

#GPSD settings
#GPSDSocket creates a GPSD socket connection & request/retrieve GPSD output.
gps_socket = agps3.GPSDSocket()
#DataStream unpacks the streamed gpsd data into python dictionaries.
data_stream = agps3.DataStream()
gps_socket.connect()
gps_socket.watch()

GetCurrentGPS()
i = 0
ArriveFlag = 0
while(True):
time.sleep(WaitTime)
if(i >= 5):
check()#check every 5*wait time
i = 0
if(ArriveFlag):
break
else:
i += 1
#Arrive destination
print('Arrived!')
stop()
```

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.