Demo entry 6719535

NavigationV2_0.py

   

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
print('\nEE 175 AGV Navigation Part')
print('Please input the goal Lat')
LatGoal = input()
print('Please input the goal Lon')
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.

Delete this entry (admin only).