def main():
ctx = proContext()
pub = ctx.socket(zmq.PUB)
pub.bind('tcp://*:8081')
subMap = ctx.socket(zmq.SUB)
subMap.connect('tcp://localhost:8083')
subMap.setsockopt(zmq.SUBSCRIBE,'Diff')
pidDis = PID(P= 10.8, I = 0.0, D = 0.0)
pidDis.setWindup(50.0)
pidHead = PID(P= 4.2, I = 0.0, D = 0.0)
#pidHead = PID(P= 0.0, I = 0.0, D = 0.0)
pidHead.setWindup(50.0)
pidDHead = PID(P=4.6, I = 0.0, D = 0.0)
#pidDHead = PID(P=0.0, I = 0.0, D = 0.0)
pidDHead.setWindup(25.0)
def recvMap():
j = 0
while True:
content = subMap.recvPro()
pidDis.SetPoint = 0.0
pidDis.update(content['Dis'])
outDis = pidDis.output * -1
pidHead.SetPoint = 0
pidHead.update(content['Head'])
outHead = pidHead.output * -1
pidDHead.SetPoint = 0
pidDHead.update(content['DHead'])
outDHead = pidHead.output * -1
steer = outDis + outHead + outDHead
#if outDis < -50:
# steer = outDis + outHead
#steer = outHead + outDHead
#steer = outDis + outHead
#steer = outDis
#steer = outHead
#steer = outDHead
speed = math.fabs(10 * math.cos( math.radians(content['DHead'])*5 ) )
#content = {'Mode':0x20,'Value':int(steer - 16 + random.randint(0,1) ) }
#content = {'Mode':0x20,'Value':int(steer - 15) }
content = {'Mode':0x20,'Value':int(steer - 14) }
pub.sendPro('PlanSteer',content)
j = (j + 1) % 999999
if j % 2 == 0:
print('PlanSteer--->', int(steer) )
print('.......................................................')
content = {'Mode':0x00,'Value':speed,'Gear':0}
#pub.sendPro('PlanSpeed',content)
pass
pass
recvMap()
import sys
import time
import math
import threading
import signal
# some packages' dir
sys.path.append("../libs")
from proContext import *
from proMCU import *
from proPrint import *
from proControl import *
import UTM
# zmq tools
ctx = proContext()
SpeedSetAddr = 'tcp://127.0.0.1:8098'
pub = ctx.socket(zmq.PUB)
pub.bind(SpeedSetAddr)
while True:
#speed,gear = input()
speed = input()
#if str(speed).isdigit() and str(gear).isdigit():
if str(speed).isdigit():
speed = int(speed)
#gear = int(gear)
pub.sendPro("SpeedSet", speed)
def main():
hdMap = []
ctx = proContext()
pub = ctx.socket(zmq.PUB)
pub.bind('tcp://*:8083')
def loadmap():
with open('map.txt', 'r') as fi:
for line in fi.readlines():
args = line.split('\t')
time = args[1]
content = args[2]
hdMap.append(content)
pass
pass
loadmap()
def searchmap(node):
curDis = 9999
curPoint = 0
tarDis = 9999
tarPoint = 0
easting, northing, zone, zone_letter = UTM.from_latlon(
node['Lat'], node['Lon'])
#get nearest point from now position,but the same heading
for i in range(0, len(hdMap)):
if math.abs(node['Head'] - hdMap[i]['Head'] > 90):
#if math.abs( node['Head'] - hdMap[i]['Head'] > 90 ) or hdMap[i]['Status'] != 2:
continue
curEasting, curNorthing, curZone, curZone_letter = UTM.from_latlon(
hdMap[i]['Lat'], hdMap[i]['Lon'])
dis = math.sqrt(
math.pow(easting - curEasting, 2) +
math.pow(northing - curNorthing, 2))
if dis < curDis:
curDis = dis
curPoint = i
#current v and heading
head = node['Head']
v = math.sqrt(
math.pow(node['v_n'], 2) + math.pow(node['v_e'], 2) +
math.pow(node['v_earth'], 2))
near = []
#add near point from j = 0 -> num
num = int(v / 0.2)
j = 0
curE, curN, curZ, curZ_l = UTM.from_latlon(hdMap[curPoint]['Lat'],
hdMap[curPoint]['Lon'])
mark = curPoint
for i in range(curPoint, len(hdMap)):
if j == num:
break
E, N, Z, Z_l = UTM.from_latlon(hdMap[i]['Lat'], hdMap[i]['Lon'])
if math.sqrt(math.pow(E - curE, 2) + math.pow(N - curN, 2)) > 0.2:
mark = i
j = j + 1
x = E - curE
y = N - curN
x1 = x * math.cos(head) - y * math.sin(head)
y1 = x * math.sin(head) + y * math.cos(head)
near.append((x1, y1))
curE, curN, curZ, curZ_l = E, N, Z, Z_l
pass
x = 0
num = int(v / 0.2)
for i in range(0, len(near)):
x = x + near[i][0]
dis = x / num
far = []
j = 0
for i in range(mark, len(hdMap)):
if j == num:
break
E, N, Z, Z_l = UTM.from_latlon(hdMap[i]['Lat'], hdMap[i]['Lon'])
if math.sqrt(math.pow(E - curE, 2) + math.pow(N - curN, 2)) > 0.2:
mark = i
j = j + 1
x = E - curE
y = N - curN
x1 = x * math.cos(head) - y * math.sin(head)
y1 = x * math.sin(head) + y * math.cos(head)
far.append((x1, y1))
curE, curN, curZ, curZ_l = E, N, Z, Z_l
pass
x = 0
y = 0
for i in range(0, len(far)):
x = x + far[i][0]
y = y + far[i][1]
head = math.atan2(x, y)
dhead = 0
ddhead = 0
if curPoint < len(hdMap) - 1 and curPoint > 0:
dhead = hdMap[curPoint + 1] / 2 - hdMap[curPoint - 1] / 2
if curPoint < len(hdMap) - 2 and curPoint > 1:
ddhead = hdMap[curPoint + 2] / 4 - hdMap[
curPoint - 2] / 4 + hdMap[curPoint] / 2
return dis, head, dhead, ddhead
ctx = proContext()
subGPS = ctx.socket(zmq.SUB)
subGPS.connect('tcp://localhost:8082')
subGPS.setsockopt(zmq.SUBSCRIBE, 'CurGNSS')
while True:
content = subGPS.recvPro()
dis, head, dhead, ddhead = searchmap(content)
content = {'Dis': dis, 'Head': head, 'DHead': dhead, 'DDHead': ddhead}
pub.sendPro('Diff', content)
pass
def main():
global node
global current
global target
hdMap = []
ctx = proContext()
pub = ctx.socket(zmq.PUB)
pub.bind('tcp://*:8083')
def getRelatedXY(x0, y0, x1, y1, angle):
angle = angle + 180
x = math.cos(math.radians(angle)) * (x1 - x0) - math.sin(
math.radians(angle)) * (y1 - y0)
y = math.sin(math.radians(angle)) * (x1 - x0) + math.cos(
math.radians(angle)) * (y1 - y0)
return -x, y
def loadmap():
print('loading map............')
fi = open('./map/map.txt', 'r')
i = 0
lastE = 0
lastN = 0
line = fi.readlines()[0]
fi.close()
fi = open('./map/map.txt', 'r')
lastargs = line.split('\t')
for line in fi.readlines():
i = (i + 1) % 99999
args = line.split('\t')
#temp = args[1]
#args[1] = args[2]
#args[2] = temp
#args[3] = args[5]
E, N, Z, Z_l = UTM.from_latlon(float((args[1])), float((args[2])))
if (math.pow(E - lastE, 2) + math.pow(N - lastN, 2) > 0.25):
#hdMap.append( (float(lastargs[1])/2 + float(args[1])/2 , float(args[2])/2 + float(lastargs[2])/2 ,float(args[3])/2 + float(lastargs[3])/2 , float(args[4])/2 + float(lastargs[4]) /2 ) )
hdMap.append((float(args[1]), float(args[2]), float(args[3]),
float(args[4])))
#lastE = E
#lastN = N
#lastargs[1],lastargs[2],lastargs[3],lastargs[4] = args[1],args[2],args[3],args[4]
#hdMap.append( (float(args[1]),float(args[2]),float(args[3]),float(args[4])) )
fi.close()
print('length map is ', len(hdMap))
loadmap()
print('finished loading map')
def alpha(ve):
#return 1 * ve
#return 2.5 * math.log1p(ve)
#return ( ( math.log1p(ve) ) ** 3 + math.log1p(ve) ) / 2
#return 6
#return ( ( ve * math.log1p(ve) ) ) / 2 + ve + 2
#return ( ( math.log1p(ve) ) ) + ve + 2
if ve < 4:
return math.log1p(ve) + 5
return ve * 2 / 3 + 3.720
#return ve + 1
return math.log1p(ve * 5) * 2.1 - 1 # 12.05
def searchmap():
global node
global current
global target
j = 0
while True:
time.sleep(0.05)
if node['Status'] < 0 or node['Status'] > 4:
continue
curDis = 9999
curPoint = 0
tarDis = 9999
tarPoint = 0
dis = 99999
#try:
easting, northing, zone, zone_letter = UTM.from_latlon(
node['Lat'], node['Lon'])
for i in range(0, len(hdMap)):
dffhead = math.fabs(int(node['Head'] - hdMap[i][2]))
if dffhead > 90 and dffhead < 270:
continue
#get nearest point ---> curPoint
E, N, Z, Z_l = UTM.from_latlon(hdMap[i][0], hdMap[i][1])
dis = math.sqrt(
math.fabs(
math.pow(easting - E, 2) + math.pow(northing - N, 2)))
if dis > 15:
continue
if dis < curDis:
curDis = dis
curPoint = i
current = i
#current v
v = math.sqrt(
math.fabs(
math.pow(node['V_n'], 2) + math.pow(node['V_e'], 2) +
math.pow(node['V_earth'], 2)))
for i in range(curPoint, len(hdMap)):
E, N, Z, Z_l = UTM.from_latlon(hdMap[i][0], hdMap[i][1])
dis = math.fabs( math.sqrt( math.fabs(math.pow(easting - E,2 )) + \
math.pow(northing - N,2) ) - alpha(v) )
if dis < tarDis:
tarDis = dis
tarPoint = i
target = i
dis = curDis
head = hdMap[tarPoint][2]
head = head - node['Head']
if head < -180:
head = head + 360
if head > 180:
head = head - 360
if curPoint == 0:
curPoint = curPoint + 1
if curPoint == len(hdMap) - 1:
curPoint = curPoint - 1
E1,N1,Z1,Z_l1 = \
UTM.from_latlon(hdMap[curPoint][0] ,hdMap[curPoint][1] )
E2,N2,Z2,Z_l2 = \
UTM.from_latlon(hdMap[int ((tarPoint + curPoint)/2) ][0] ,hdMap[ int( (tarPoint + curPoint)/2)][1] )
dhead = 0
ddhead = 0
if curPoint + curPoint - tarPoint > 0:
#dhead = hdMap[tarPoint][2]/2 - hdMap[curPoint * 2 - tarPoint][2]/2
dhead = hdMap[tarPoint][2] / 2 - hdMap[curPoint][2] / 2
#dhead = hdMap[tarPoint][2]/2 - hdMap[curPoint + ( curPoint - tarPoint)/3 ][2]/2
if curPoint + curPoint - tarPoint > 0:
ddhead = hdMap[tarPoint][2] / 4 - hdMap[curPoint - 2][
2] / 2 + hdMap[curPoint - tarPoint + curPoint][2] / 4
#x,y = getRelatedXY(onlineData[curIndex].x, onlineData[curIndex].y, MapLanes[i].points[j].x + offsetX ,MapLanes[i].points[j].y + offsetY , onlineData[curIndex].head)
#print('easting diff is :: ',easting - E1)
#print('northing diff is :: ',northing - N1)
x, y = getRelatedXY(easting, northing, E1, N1, node['Head'])
content = {
'Dis': x,
'Head': head,
'DHead': dhead,
'DDHead': ddhead
}
pub.sendPro('Diff', content)
j = (j + 1) % 9999
if j % 5 == 0:
print(content)
print('head = ', node['Head'])
print(
'========================================================================================'
)
#except Exception:
thread.start_new_thread(searchmap, ())
def draw():
pygame.init()
screen = pygame.display.set_mode((500, 500))
#screen = pygame.display.set_mode((1361,1001))
pygame.display.set_caption("ququuququququuuq")
FPS = 50
fpsClock = pygame.time.Clock()
BLACK = (0, 0, 0)
WHITE = (255, 255, 255)
RED = (255, 0, 0)
DARKPINK = (255, 20, 147)
DARKRED = (138, 0, 0)
PURPLE = (160, 32, 240)
YELLOW = (255, 255, 0)
GREEN = (00, 255, 0)
BLUE = (0, 0, 255)
LIGHTBLUE = (176, 226, 255)
ORANGE4 = (139, 69, 0)
screen.fill(BLACK)
while True:
screen.fill(BLACK)
for event in pygame.event.get():
if event.type == QUIT:
pygame.quit()
sys.exit()
mat = []
centerE, centerN, Z, Z_l = UTM.from_latlon(node['Lat'],
node['Lon'])
NN = -100 * math.cos(math.radians(node['Head']))
EE = 100 * math.sin(math.radians(node['Head']))
for content in hdMap:
E, N, Z, Z_l = UTM.from_latlon(content[0], content[1])
#print(E - centerE)
#mat.append( [int ((E-centerE) * 20) + 300 , int ((N - centerN)* 20)+ 300] )
#print(mat)
pygame.draw.circle(screen, WHITE, [
int((E - centerE) * 20) + 250,
int(-1 * (N - centerN) * 5) + 250
], 1, 0)
#pygame.draw.lines(screen, WHITE,False, mat, 1)
pygame.draw.circle(screen, GREEN, [500, 400], 3, 0)
E, N, Z, Z_l = UTM.from_latlon(hdMap[current][0],
hdMap[current][1])
pygame.draw.circle(screen, YELLOW, [
int((E - centerE) * 20) + 250,
int(-1 * (N - centerN) * 5) + 250
], 2, 0)
E, N, Z, Z_l = UTM.from_latlon(hdMap[target][0], hdMap[target][1])
pygame.draw.circle(screen, RED, [
int((E - centerE) * 20) + 250,
int(-1 * (N - centerN) * 5) + 250
], 2, 0)
pygame.draw.line(screen, RED, [500, 400], [500 + EE, 250 + NN], 2)
pygame.display.update()
fpsClock.tick(FPS)
pass
#thread.start_new_thread(draw, ())
#recieve and search
ctx = proContext()
subGPS = ctx.socket(zmq.SUB)
subGPS.connect('tcp://localhost:8080')
subGPS.setsockopt(zmq.SUBSCRIBE, 'CurGNSS')
i = 0
while True:
content = subGPS.recvPro()
node = content
i = (i + 1) % 999
if i % 20 == 0:
pass
def main():
global node
global current
global target
target = 0
current = 0
hdMap = []
ctx = proContext()
pub = ctx.socket(zmq.PUB)
pub.bind('tcp://*:8083')
def loadmap():
print('loading map............')
fi = open('./map/map.txt','r')
i = 0
lastE = 0
lastN = 0
line = fi.readlines()[0]
fi.close()
fi = open('./map/map.txt','r')
lastargs = line.split('\t')
for line in fi.readlines():
i = (i + 1) % 99999
args = line.split('\t')
temp = args[1]
args[1] = args[2]
args[2] = temp
E,N,Z,Z_l = UTM.from_latlon( float((args[1])) , float((args[2])) )
#ids = math.pow(E-lastE,2) + math.pow(N-lastN,2)
#print(ids)
if( math.pow(E-lastE,2) + math.pow(N-lastN,2) > 0.25 ):
hdMap.append( (float(lastargs[1])/2 + float(args[1])/2 , float(args[2])/2 + float(lastargs[2])/2 ,float(args[5])/2 + float(lastargs[5])/2 ) )
hdMap.append( (float(args[1]),float(args[2]),float(args[5])) )
lastE = E
lastN = N
lastargs[1],lastargs[2],lastargs[5] = args[1],args[2],args[5]
fi.close()
print('length map is ',len(hdMap))
loadmap()
print('finished loading map')
def alpha(ve):
#return 1 * ve
#return 2.5 * math.log1p(ve)
#return ( ( math.log1p(ve) ) ** 3 + math.log1p(ve) ) / 2
#return 6
#return ( ( ve * math.log1p(ve) ) ) / 2 + ve + 2
#return ( ( math.log1p(ve) ) ) + ve + 2
#if ve < 4:
# return math.log1p(ve)
#return ve * 2/3 + 3
#return ve + 1
return math.log1p(ve * 5) * 2.5 # 12.05
if ve < 3:
return math.log1p(ve) + 1
return ve + 1
def searchmap():
global node
global current
global target
j = 0
while True:
#time.sleep(0.05)
args = node
status = 0
gnssSpeed = 5
#lat,lon,head,status,gnssSpeed,Steer = float(args[1]),float(args[2]),float(args[3]) ,float(args[4]),float(args[5]),float(args[6])
lat,lon,head = float(args[2]),float(args[1]),float(args[5])
if status < 0 or status > 4:
#continue
pass
curDis = 9999
curPoint = 0
tarDis = 9999
tarPoint = 0
#try:
easting,northing,zone,zone_letter = UTM.from_latlon(lat, lon)
for i in range(0,len(hdMap)):
if math.fabs( int(head - hdMap[i][2]) ) > 90 :
continue
#get nearest point ---> curPoint
E,N,Z,Z_l = UTM.from_latlon(hdMap[i][0] ,hdMap[i][1] )
dis = math.sqrt (math.fabs( math.pow(easting - E,2) + math.pow(northing - N,2) ) )
if dis < curDis:
curDis = dis
curPoint = i
current = i
#current v
v = gnssSpeed
tarDis = 0
tarPoint = curPoint
LE,LN,Z,Z_l= UTM.from_latlon(hdMap[curPoint][0] ,hdMap[curPoint][1] )
for i in range(curPoint + 1,len(hdMap)):
E,N,Z,Z_l= UTM.from_latlon(hdMap[i][0] ,hdMap[i][1] )
tarDis = tarDis + math.fabs( math.sqrt( math.pow(LE - E,2) + \
math.pow(LN - N,2) ) )
LE,LN = E,N
if tarDis > alpha(v):
tarPoint = i
target = i
break
Head = hdMap[tarPoint][2]
Head = Head - head
if Head < -180:
Head = Head + 360
if Head > 180:
Head = Head - 360
#hai lun
if curPoint == 0:
curPoint = curPoint + 3
if curPoint >= len(hdMap) - 3:
curPoint = curPoint - 3
E1,N1,Z1,Z_l1 = \
UTM.from_latlon(hdMap[curPoint + 3][0] ,hdMap[curPoint + 3][1] )
E2,N2,Z2,Z_l2 = \
UTM.from_latlon(hdMap[curPoint - 3][0] ,hdMap[curPoint - 3][1] )
a = math.sqrt( math.pow(easting - E1 ,2) + math.pow(northing - N1 ,2) )
b = math.sqrt( math.pow(easting - E2 ,2) + math.pow(northing - N2 ,2) )
c = math.sqrt( math.pow(E1 - E2 ,2) + math.pow(N1 - N2 ,2) )
p = (a + b + c)/2
h = 0
if c > 0:
h = math.sqrt( math.fabs((p * (p-a) * (p-b) * (p-c)) ) ) * 2 / c
x1 = (E1 - easting) * math.cos( math.radians(head) ) - (N1 - northing) * math.sin( math.radians(head) )
if x1 < 0:
dis = dis * -1
h = h * -1
pass
dis = x1
dhead = 0
ddhead = 0
if curPoint + curPoint - tarPoint > 0:
dhead = hdMap[tarPoint][2]/2 - hdMap[curPoint][2]/2
if curPoint + curPoint - tarPoint > 0:
ddhead = hdMap[tarPoint][2]/4 - hdMap[curPoint - 2][2]/2 + hdMap[curPoint - tarPoint + curPoint][2] / 4
content = {'Dis':h,'Head':Head,'DHead':dhead,'DDHead':ddhead}
pub.sendPro('Diff',content)
j = (j + 1) % 9999
time.sleep(0.05)
if j%5 ==0:
print(' v is ',v,' alpha(v) is ',alpha(v))
print('tar - cur is : ',tarPoint - curPoint )
#print('Dis:',h,' Head:',Head,' DHead:',dhead)
#print(content)
print('========================================================================================')
#except Exception:
thread.start_new_thread(searchmap, ())
def draw():
global current
global target
kk = 20
W = 1000
H = 1000
pygame.init()
screen = pygame.display.set_mode((W,H))
#screen = pygame.display.set_mode((1361,1001))
pygame.display.set_caption("ququuququququuuq")
FPS = 50
fpsClock = pygame.time.Clock()
BLACK = (0,0,0)
WHITE = (255,255,255)
RED = (255,0,0)
DARKPINK = (255,20,147)
DARKRED = (138,0,0)
PURPLE = (160,32,240)
YELLOW = (255,255,0)
GREEN = (00,255,0)
BLUE = (0,0,255)
LIGHTBLUE = (176,226,255)
ORANGE4 = (139,69,0)
screen.fill(BLACK)
while True:
screen.fill(BLACK)
for event in pygame.event.get():
if event.type == QUIT:
pygame.quit()
sys.exit()
mat = []
#temp = node[1]
#node[1] = node[2]
#node[2] = temp
centerE,centerN,Z,Z_l = UTM.from_latlon( float(node[2]), float(node[1]) )
for content in hdMap:
E,N,Z,Z_l = UTM.from_latlon( content[0], content[1] )
#print(E - centerE)
#mat.append( [int ((E-centerE) * 20) + 300 , int ((N - centerN)* 20)+ 300] )
#print(mat)
pygame.draw.circle(screen, WHITE, [int ((E-centerE) * kk) + W/2 , int (-1 * (N - centerN)* kk)+ H/2] , 1, 0)
#pygame.draw.lines(screen, WHITE,False, mat, 1)
pygame.draw.circle(screen,BLUE,[W/2,H/2] , 3 , 0)
E,N,Z,Z_l = UTM.from_latlon( hdMap[current][0], hdMap[current][1] )
pygame.draw.circle(screen, DARKPINK, [int ((E-centerE) * kk) + W/2 , int (-1 * (N - centerN)* kk)+ H/2] , 4, 0)
E,N,Z,Z_l = UTM.from_latlon( hdMap[target][0], hdMap[target][1] )
pygame.draw.circle(screen, RED, [int ((E-centerE) * kk) + W/2 , int (-1 * (N - centerN)* kk)+ H/2] , 3, 0)
off = 0.5
NN = -1000 * math.cos( math.radians(float(node[5]) + off ) )
EE = 1000 * math.sin( math.radians(float(node[5]) + off ) )
pygame.draw.line(screen, RED, [W/2, H/2], [W/2 + EE, H/2 + NN], 2)
pygame.display.update()
fpsClock.tick(FPS)
pass
thread.start_new_thread(draw, ())
#recieve and search
ctx = proContext()
subGPS = ctx.socket(zmq.SUB)
subGPS.connect('tcp://localhost:8080')
subGPS.setsockopt(zmq.SUBSCRIBE,'CurGNSS')
i = 0
while True:
content = subGPS.recvPro()
node = content
i = (i+1) % 999
if i % 20 == 0:
pass
def main():
global gnssLat
global gnssLon
global gnssHead
global gnssStatus
global gnssV
global canBrakeMode
global canBrakeTime
global canBrakeButton
global canBrakeRemoter
global canBrakePedal
global canBrakeRemoterS
global canBrakeReal
global canGunMode
global canGunDepth
global canGunSpeed
global canSteerMode
global canSteerTorque
global canSteerException
global canSteerAngleH
global canSteerAngleL
global canSteerCalib
global canSteerBy6
global canSteerCheck
global planSpeed
global planSpeedMode
global planSteer
global planSteerMode
global navDis
global navHead
global navDHead
global navDDHead
global controlWho
global controlMode
global controlValue
ctx = proContext()
gnssLat = 0
gnssLon = 0
gnssHead = 0
gnssStatus = 0
gnssV = 0
canBrakeMode = 0
canBrakeTime = 0
canBrakeButton = 0
canBrakeRemoter = 0
canBrakePedal = 0
canBrakeRemoterS = 0
canBrakeReal = 0
canGunMode = 0
canGunDepth = 0
canGunSpeed = 0
canSteerMode = 0
canSteerTorque = 0
canSteerException = 0
canSteerAngleH = 0
canSteerAngleL = 0
canSteerCalib = 0
canSteerBy6 = 0
canSteerCheck = 0
planSpeed = 0
planSpeedMode = 0
planSpeedGear = 0
planSteer = 0
planSteerMode = 0
navDis = 0
navHead = 0
navDHead = 0
navDDHead = 0
controlWho = 'No'
controlMode = 0x00
controlValue = 0x00
def logGNSS():
global gnssLat
global gnssLon
global gnssHead
global gnssStatus
global gnssV
sub = ctx.socket(zmq.SUB)
sub.connect('tcp://localhost:8080')
sub.setsockopt(zmq.SUBSCRIBE, 'CurGNSS')
filename = time.strftime("./data/%Y_%m_%d_%H_%M.anay",
time.localtime())
f = open(filename, 'w')
i = 0
while True:
content = sub.recvPro()
i = (i + 1) % 9999999
f.write(str(time.time()))
f.write('\t')
#f.write(str(content))
gnssLat = content['Lat']
gnssLon = content['Lon']
gnssHead = content['Head']
gnssStatus = content['Status']
gnssV = math.sqrt(content['V_n']**2 + content['V_e']**2 +
content['V_earth']**2)
content = str(content['Lat']) + '\t' + str(
content['Lon']) + '\t' + str(content['Head']) + '\t' + str(
content['Status']) + '\n'
#print(content)
f.write((content))
pass
def logCANBrake():
global canBrakeMode
global canBrakeTime
global canBrakeButton
global canBrakeRemoter
global canBrakePedal
global canBrakeRemoterS
global canBrakeReal
sub = ctx.socket(zmq.SUB)
sub.connect('tcp://localhost:8088')
sub.setsockopt(zmq.SUBSCRIBE, 'CANBrake')
while True:
content = sub.recvPro()
canBrakeTime = content['Time']
canBrakeButton = content['Button']
canBrakeRemoter = content['Remoter']
canBrakePedal = content['Pedal']
canBrakeRemoterS = content['BrakeRemoterS']
canBrakeReal = content['Real']
pass
def logCANSteer():
global canSteerMode
global canSteerTorque
global canSteerException
global canSteerAngleH
global canSteerAngleL
global canSteerCalib
global canSteerBy6
global canSteerCheck
sub = ctx.socket(zmq.SUB)
sub.connect('tcp://localhost:8088')
sub.setsockopt(zmq.SUBSCRIBE, 'CANSteer')
while True:
content = sub.recvPro()
canSteerMode = content['Mode']
canSteerTorque = content['Torque']
canSteerException = content['EException']
canSteerAngleH = content['AngleH']
canSteerAngleL = content['AngleL']
canSteerCalib = content['Calib']
canSteerBy6 = content['By6']
canSteerCheck = content['Check']
pass
def logCANGun():
global canGunMode
global canGunDepth
global canGunSpeed
sub = ctx.socket(zmq.SUB)
sub.connect('tcp://localhost:8088')
sub.setsockopt(zmq.SUBSCRIBE, 'CANGun')
while True:
content = sub.recvPro()
canGunMode = content['Mode']
canGunDepth = content['Depth']
canGunSpeed = content['Speed']
pass
def logPlanSpeed():
global planSpeed
global planSpeedMode
global planSpeedGear
sub = ctx.socket(zmq.SUB)
sub.connect('tcp://localhost:8081')
sub.setsockopt(zmq.SUBSCRIBE, 'PlanSpeed')
while True:
content = sub.recvPro()
planSpeedMode = content['Mode']
planSpeed = content['Value']
planSpeedGear = content['Gear']
pass
def logPlanSteer():
global planSteer
global planSteerMode
sub = ctx.socket(zmq.SUB)
sub.connect('tcp://localhost:8081')
sub.setsockopt(zmq.SUBSCRIBE, 'PlanSteer')
while True:
content = sub.recvPro()
planSteerMode = content['Mode']
planSteer = content['Value']
pass
def logNav():
global navDis
global navHead
global navDHead
global navDDHead
subMap = ctx.socket(zmq.SUB)
subMap.connect('tcp://localhost:8083')
subMap.setsockopt(zmq.SUBSCRIBE, 'Diff')
while True:
content = subMap.recvPro()
navDis = content['Dis']
navHead = content['Head']
navDHead = content['DHead']
navDDHead = content['DDHead']
pass
def logCtrl():
global controlWho
global controlMode
global controlValue
sub = ctx.socket(zmq.SUB)
sub.connect('tcp://localhost:8082')
sub.setsockopt(zmq.SUBSCRIBE, 'Cmd')
while True:
content = sub.recvPro()
controlWho = content['Who']
controlMode = content['Mode']
controlValue = content['Value']
pass
thread.start_new_thread(logGNSS, ())
thread.start_new_thread(logCANBrake, ())
thread.start_new_thread(logCANSteer, ())
thread.start_new_thread(logCANGun, ())
thread.start_new_thread(logPlanSpeed, ())
thread.start_new_thread(logPlanSteer, ())
thread.start_new_thread(logNav, ())
thread.start_new_thread(logCtrl, ())
filename = time.strftime("./data/%Y_%m_%d_%H_%M_%S.all", time.localtime())
f = open(filename, 'w')
i = 0
while True:
i = i + 1
i = i % 999999
time.sleep(0.1)
line = (str(time.time()) + '\t' + \
str(gnssLat) + '\t' + \
str(gnssLon) + '\t' + \
str(gnssHead) + '\t' + \
str(gnssStatus) + '\t' + \
str(gnssV) + '\t' + \
str(canBrakeMode) + '\t' + \
str(canBrakeTime) + '\t' + \
str(canBrakeButton) + '\t' + \
str(canBrakeRemoter) + '\t' + \
str(canBrakePedal) + '\t' + \
str(canBrakeRemoterS) + '\t' + \
str(canBrakeReal) + '\t' + \
str(canGunMode) + '\t' + \
str(canGunDepth) + '\t' + \
str(canGunSpeed) + '\t' + \
str(canSteerMode) + '\t' + \
str(canSteerTorque) + '\t' + \
str(canSteerException) + '\t' + \
str(canSteerAngleH) + '\t' + \
str(canSteerAngleL) + '\t' + \
str(canSteerCalib) + '\t' + \
str(canSteerBy6) + '\t' + \
str(canSteerCheck) + '\t' + \
str(planSpeed) + '\t' + \
str(planSpeedMode) + '\t' + \
str(planSpeedGear) + '\t' + \
str(planSteer) + '\t' + \
str(planSteerMode) + '\t' + \
str(navDis) + '\t' + \
str(navHead) + '\t' + \
str(navDHead) + '\t' + \
str(navDDHead) + '\t' + \
str(controlWho) + '\t' + \
str(controlMode) + '\t' + \
str(controlValue) + '\n')
if i % 10 == 0:
print(line)
f.write(line)
def main():
global speed_set
global speed_back
global speed_mode
global speed_gear
speed_gear = 0
#publish current all can status
ctx = proContext()
pub = ctx.socket(zmq.PUB)
pub.bind('tcp://*:8082')
#speed
#read from canGun
speed_back = 0
#read from planned speed
speed_set = 0
speed_mode = 0x00
#receive comand from plan(decision) system
def recvSpeedAndSet():
global speed_set
global speed_mode
global speed_gear
subPlanSpeed = ctx.socket(zmq.SUB)
subPlanSpeed.connect('tcp://localhost:8081')
subPlanSpeed.setsockopt(zmq.SUBSCRIBE, 'PlanSpeed')
while True:
content = subPlanSpeed.recvPro()
speed_mode = content['Mode']
speed_set = content['Value']
speed_gear = content['Gear']
speed_gear = max(speed_gear, 6)
speed_gear = min(speed_gear, 0)
def recvSpeedAndBack():
global speed_back
subCANSpeed = ctx.socket(zmq.SUB)
subCANSpeed.connect('tcp://localhost:8088')
subCANSpeed.setsockopt(zmq.SUBSCRIBE, 'CANGun')
while True:
content = subCANSpeed.recvPro()
#print(content)
speed_back = content['Speed']
def control():
global speed_set
global speed_back
global speed_mode
global speed_gear
global output
pid10 = PID(P=2.6, I=9.8, D=0.0)
pid10.SetPoint = speed_set
pid10.setWindup(6.0)
pid15 = PID(P=3.6, I=12.8, D=0.0)
pid15.SetPoint = speed_set
pid15.setWindup(6.0)
pid20 = PID(P=4.0, I=2.6, D=0.6)
pid20.SetPoint = speed_set
pid30 = PID(P=4.5, I=3.8, D=0.8)
pid30.SetPoint = speed_set
pid40 = PID(P=5.3, I=4.8, D=1.0)
pid40.SetPoint = speed_set
pid50 = PID(P=6.6, I=5.9, D=1.2)
pid50.SetPoint = speed_set
pid60 = PID(P=7.8, I=6.9, D=1.5)
pid60.SetPoint = speed_set
i = 0
while True:
i = (i + 1) % 9999
time.sleep(0.2)
if speed_set > 60:
speed_set = 60
pid10.SetPoint = speed_set
pid10.update(speed_back)
pid15.SetPoint = speed_set
pid15.update(speed_back)
pid20.SetPoint = speed_set
pid20.update(speed_back)
pid30.SetPoint = speed_set
pid30.update(speed_back)
pid40.SetPoint = speed_set
pid40.update(speed_back)
pid50.SetPoint = speed_set
pid50.update(speed_back)
pid60.SetPoint = speed_set
pid60.update(speed_back)
#slow speed
if speed_back - speed_set > 5:
content = {
'Who': 'Brake',
'Mode': speed_mode,
'Value': 57.5 + int(speed_gear * 22.5)
}
pub.sendPro('Cmd', content)
continue
pass
#brake to stop
if speed_set <= 0:
speed_set = max(speed_set, -6)
content = {
'Who': 'Brake',
'Mode': speed_mode,
'Value': 57.5 + int(speed_gear * 22.5)
}
pub.sendPro('Cmd', content)
continue
pass
#different args for different speed_set
if speed_set <= 10:
output = pid10.output
elif speed_set <= 15:
output = pid15.output
elif speed_set <= 25:
output = pid20.output
elif speed_set <= 35:
output = pid30.output
elif speed_set <= 45:
output = pid40.output
elif speed_set <= 55:
output = pid50.output
elif speed_set <= 65:
output = pid60.output
if i % 4 == 0:
print('output is : ', output)
#accelerate
if output > 0:
Depth = 0
if (speed_set <= 10):
Depth = min(35 + speed_gear * 2, int(0.42 * output))
if (speed_set <= 15):
Depth = min(35 + speed_gear * 3, int(0.42 * output))
elif (speed_set <= 25):
Depth = min(40 + speed_gear * 4, int(0.80 * output))
elif (speed_set <= 35):
Depth = min(50 + speed_gear * 5, int(0.58 * output))
elif (speed_set <= 45):
Depth = min(60 + speed_gear * 5, int(0.47 * output))
elif (speed_set <= 55):
Depth = min(65 + speed_gear * 5, int(0.47 * output))
else:
Depth = min(70 + speed_gear * 6, int(0.46 * output))
content = {'Who': 'Gun', 'Mode': speed_mode, 'Value': Depth}
pub.sendPro('Cmd', content)
if i % 4 == 0:
print('control depth is : ', Depth)
#slow down
if output < 0:
if output > -50:
Depth = 0
content = {
'Who': 'Brake',
'Mode': speed_mode,
'Value': Depth
}
pub.sendPro('Cmd', content)
if i % 4 == 0:
print('control depth is : ', Depth)
elif output > -66:
Depth = int(-0.35 * output)
content = {
'Who': 'Brake',
'Mode': speed_mode,
'Value': Depth
}
pub.sendPro('Cmd', content)
if i % 4 == 0:
print('control depth is : ', Depth)
elif output > -71:
Depth = int(-0.45 * output)
content = {
'Who': 'Brake',
'Mode': speed_mode,
'Value': Depth
}
pub.sendPro('Cmd', content)
if i % 4 == 0:
print('control depth is : ', Depth)
elif output > -79:
Depth = int(-0.5 * output)
content = {
'Who': 'Brake',
'Mode': speed_mode,
'Value': Depth
}
pub.sendPro('Cmd', content)
if i % 4 == 0:
print('control depth is : ', Depth)
elif output > -85:
Depth = int(-0.6 * output)
content = {
'Who': 'Brake',
'Mode': speed_mode,
'Value': Depth
}
pub.sendPro('Cmd', content)
if i % 4 == 0:
print('control depth is : ', Depth)
elif output > -90:
Depth = int(-0.8 * output)
content = {
'Who': 'Brake',
'Mode': speed_mode,
'Value': Depth
}
pub.sendPro('Cmd', content)
if i % 4 == 0:
print('control depth is : ', Depth)
elif output < 0:
Depth = int(-1.0 * output)
content = {
'Who': 'Brake',
'Mode': speed_mode,
'Value': Depth
}
pub.sendPro('Cmd', content)
if i % 4 == 0:
print('control depth is : ', Depth)
if output == 0:
Depth = 0x00
content = {'Who': 'Brake', 'Mode': speed_mode, 'Value': Depth}
pub.sendPro('Cmd', content)
Depth = 0x00
content = {'Who': 'Gun', 'Mode': speed_mode, 'Value': Depth}
pub.sendPro('Cmd', content)
if i % 4 == 0:
print('speedBack is : ', speed_back)
print('speedSet is : ', speed_set)
print('speedMode is : ', speed_mode)
print('=======================================')
thread.start_new_thread(recvSpeedAndSet, ())
thread.start_new_thread(recvSpeedAndBack, ())
#read, update can,as fallcack
thread.start_new_thread(control, ())
i = 0
while True:
i = i + 1
i = i % 999999
time.sleep(1)
def main():
can = CAN()
#publish current all can status
ctx = proContext()
pub = ctx.socket(zmq.PUB)
pub.bind('tcp://*:8080')
#subcribe current plan about speed and steer
#steer
subSteer = ctx.socket(zmq.SUB)
subSteer.connect('tcp://localhost:8081')
subSteer.setsockopt(zmq.SUBSCRIBE, 'PlanSteer')
#speed
subSpeed = ctx.socket(zmq.SUB)
subSpeed.connect('tcp://localhost:8081')
subSpeed.setsockopt(zmq.SUBSCRIBE, 'PlanSpeed')
#read from canGun
speed_back = 0
#read from planned speed
speed_set = 0
speed_mode = 0x00
#if gun, send gun ,if brake brake.
speed_way = 'gun'
#read from CAN BUS, and publish from localhost:8080
def readAndPub():
while True:
#read and pub gun msg, update speed_back from canBUS
can.readGUn()
content = {
'Mode': can.gunRead.Mode,
'Depth': can.gunRead.Depth,
'Speed': can.gunRead.Speed
}
pub.sendPro('CANgun', content)
speed_back = can.gunRead.Speed
#read and pub brake msg
can.readBrake()
content = {
'Time': can.brakeRead.Time,
'Button': can.brakeRead.Button,
'Remoter': can.brakeRead.Remoter,
'Pedal': can.brakeRead.Pedal,
'Can': can.brakeRead.Pedal,
'RemoterS': can.brakeRead.RemoterS,
'Real': can.brakeRead.Real
}
pub.sendPro('CANbrake', content)
#read and pub steer msg
can.readSteer()
content = {
'Mode': can.brakeRead.Mode,
'Torque': can.brakeRead.Torque,
'EException': can.brakeRead.EException,
'AngleH': can.brakeRead.AngleH,
'AngleL': can.brakeRead.AngleL,
'Calib': can.brakeRead.Calib,
'By6': can.brakeRead.By6,
'Check': can.brakeRead.Check
}
pub.sendPro('CANsteer', content)
time.sleep(0.02)
pass
pass
#receive comand from plan(decision) system
def recvAndSet():
while True:
#receive speed
content = subSpeed.recvPro()
speed_mode = content['Mode']
can.sendBrake.Mode = speed_mode
can.sendGun.Mode = speed_mode
speed_set = content['Value']
#receive steer
content = subSteer.recvPro()
can.sendSteer.Mode = content['Mode']
can.sendSteer.AngleH = int((content['Value'] + 1024) / 256)
can.sendSteer.AngleL = int((content['Value'] + 1024) % 256)
pass
pass
#send msg to CAN BUS
def sendCmd():
while True:
if speed_way == 'brake':
can.sendBrake()
if speed_way == 'gun':
can.sendGun()
can.sendSteer()
time.sleep(0.1)
pass
pass
def control():
pid = PID()
pid.clear()
pid.SetPoint = speed_set
pid.setSampleTime(10)
while True:
pid.SetPoint = speed_set
pid.update(speed_back)
output = pid.output
if output > 0:
speed_way = 'gun'
can.sendGun.Mode = speed_mode
can.sendGun.Depth = output * 1.0
if output < 0:
speed_way = 'brake'
can.sendBrake.Mode = speed_mode
can.sendBrake.Depth = -1 * output * 0.1
time.sleep(0.05)
pass
pass
#read, update can,as fallcack
thread.start_new_thread(readAndPub, ())
#control ,update value of cmd to CAN BUS
thread.start_new_thread(control, ())
#recv decision speed, steer, mode
threading.start(recvAndSet)
#send, send know control depth
threading.start(sendCmd)
def main():
global canSpeed
global canSteer
global planSteer
ctx = proContext()
pub = ctx.socket(zmq.PUB)
pub.bind('tcp://*:8080')
pubCAN = ctx.socket(zmq.PUB)
pubCAN.bind('tcp://*:8088')
mcu = MCU()
canSpeed = 0
uartSpeed = 0
canSteer = 0
lastCmd = 0
lastSteer = 0
control = True
def readGNSS():
global canSpeed
global canSteer
imuError = 0.5
i = 0
while True:
mcu.readGNSS()
uartSpeed = math.sqrt(mcu.gnssRead.v_n**2 + mcu.gnssRead.v_e**2 +
mcu.gnssRead.v_earth**2)
content = {"Length":mcu.gnssRead.length,"Mode":mcu.gnssRead.mode,"Time1":mcu.gnssRead.time1,"Time2":mcu.gnssRead.time2, \
"Num":mcu.gnssRead.num,"Lat":mcu.gnssRead.lat,"Lon":mcu.gnssRead.lon,"Height":mcu.gnssRead.height, \
"V_n":mcu.gnssRead.v_n,"V_e":mcu.gnssRead.v_e,"V_earth":mcu.gnssRead.v_earth, \
"Roll":mcu.gnssRead.roll,"Pitch":mcu.gnssRead.pitch,"Head":mcu.gnssRead.head + imuError, \
"A_n":mcu.gnssRead.a_n,"A_e":mcu.gnssRead.a_e,"A_earth":mcu.gnssRead.a_earth, \
"V_roll":mcu.gnssRead.v_roll,"V_pitch":mcu.gnssRead.v_pitch,"V_head":mcu.gnssRead.v_head, \
"Status":mcu.gnssRead.status}
i = (i + 1) % 9999
if i % 20 == 0:
print(content)
print('***************')
print('*************************************')
pub.sendPro('CurGNSS', content)
pass
def readCAN():
global canSpeed
global canSteer
i = 0
while True:
time.sleep(0.05)
mcu.readBrake()
content = {'Time':mcu.brakeRead.Time, 'Button':mcu.brakeRead.Button, 'Remoter':mcu.brakeRead.Remoter,\
'Pedal':mcu.brakeRead.Pedal, 'Can':mcu.brakeRead.Pedal, 'RemoterS':mcu.brakeRead.RemoterS, \
'Real':mcu.brakeRead.Real}
pubCAN.sendPro('CANBrake', content)
i = (i + 1) % 9999
if i % 25 == 0:
#print(content)
pass
mcu.readGun()
content = {
'Mode': mcu.gunRead.Mode,
'Depth': mcu.gunRead.Depth,
'Speed': mcu.gunRead.Speed
}
canSpeed = mcu.gunRead.Speed
#print('canSpeed',canSpeed)
#print('**********************************************************')
pubCAN.sendPro('CANGun', content)
mcu.readSteer()
content = {'Mode':mcu.steerRead.Mode, 'Torque':mcu.steerRead.Torque, 'EException':mcu.steerRead.EException, \
'AngleH':mcu.steerRead.AngleH, 'AngleL':mcu.steerRead.AngleL, 'Calib':mcu.steerRead.Calib, \
'By6':mcu.steerRead.By6, 'Check':mcu.steerRead.Check}
canSteer = mcu.steerRead.AngleH * 256 + mcu.steerRead.AngleL - 1024
#print('---------------------------------',canSteer)
pubCAN.sendPro('CANSteer', content)
pass
def alpha(v):
return 100 / (v**2)
def sendCmd(content):
global planSteer
if content['Who'] == 'Brake':
mcu.brakeSend.Mode = content['Mode']
mcu.brakeSend.Depth = content['Value']
mcu.sendBrake()
pass
elif content['Who'] == 'Gun':
mcu.gunSend.Mode = content['Mode']
mcu.gunSend.Depth = content['Value']
speed = min(canSpeed / 3.6, uartSpeed)
if speed > 0 and math.fabs(steer) > alpha(speed):
mcu.gunSend.Depth = 0x00
mcu.sendGun()
pass
elif content['Who'] == 'Steer':
steer = content['Value']
#print('send steer is : ',steer)
mcu.steerSend.Mode = content['Mode']
mcu.steerSend.AngleH = int((steer + 1024) / 256)
mcu.steerSend.AngleL = int((steer + 1024) % 256)
mcu.sendSteer()
pass
pass
def recvControl():
subCAN = ctx.socket(zmq.SUB)
subCAN.connect('tcp://localhost:8082')
subCAN.setsockopt(zmq.SUBSCRIBE, 'Cmd')
while True:
lastCmd = time.time()
content = subCAN.recvPro()
#print(content)
sendCmd(content)
pass
def recvSteer():
steer_limit = []
def loadSteerConfig():
with open('config') as f:
for line in f.readlines():
steer_limit.append(int(line))
loadSteerConfig()
lastSteer = time.time()
subSteer = ctx.socket(zmq.SUB)
subSteer.connect('tcp://localhost:8081')
subSteer.setsockopt(zmq.SUBSCRIBE, 'PlanSteer')
lastSteer = 0
steer = 0
i = 0
while True:
content = subSteer.recvPro()
steer = content['Value']
gap = 0
if (canSpeed < len(steer_limit)):
gap = steer_limit[int(canSpeed)]
if steer - lastSteer > gap:
steer = lastSteer + gap
elif steer - lastSteer < -1 * gap:
steer = lastSteer - gap
lastSteer = steer
content = {'Who': 'Steer', 'Mode': content['Mode'], 'Value': steer}
i = (i + 1) % 9999
if i % 25 == 0:
pass
#print(content)
sendCmd(content)
pass
thread.start_new_thread(readGNSS, ())
thread.start_new_thread(readCAN, ())
thread.start_new_thread(recvControl, ())
thread.start_new_thread(recvSteer, ())
i = 0
while True:
if time.time() - lastSteer > 2:
# content = {'Who':'Steer','Mode':0x10,'Value':0x00}
# sendCmd(content)
# content = {'Who':'Brake','Mode':0x01,'Value':0x9f}
# sendCmd(content)
# content = {'Who':'Gun','Mode':0x00,'Value':0x00}
# sendCmd(content)
pass
if time.time() - lastCmd > 2:
pass
i = (i + 1) % 9999
time.sleep(1)
