GGG
/
wrc_back_GXK
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: GGG:f3ee388ea7923093e0c0d39e84226ba643f34378
Branches Tags
GGG:master
...
compare: GGG:77f32edc6bee44f3fc522cff4c5dbac2ac90ab75
Branches Tags
GGG:master

2 Commits
f3ee388ea7 ... 77f32edc6b

Author SHA1 Message Date
wrc_back_GXK 77f32edc6b v0.2 p 2 years ago
wrc_back_GXK 020d310a8c v0.2 p 2 years ago
2 changed files with 286 additions and 49 deletions
Show Stats

2
pure5.1.py
Unescape Escape View File

@ -173,7 +173,7 @@ def main():
# logger.add('file.log')
# logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north))
v.append(state.v)
print('v', v[-1])
# print('v', v[-1])
t.append(time)
# plt.cla()

333
pure_back_1.py → pure_back_2.py
Unescape Escape View File

@ -309,53 +309,290 @@ def main():
# plt.pause(0.001)
def car_back_right():
def car_back_right(direction):
print('进入右倒库')
while True:
global fangxp
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
# d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))
ra_trace = abs(math.sqrt(pow((float(tcp4.east)-68.35),2) + pow((float(tcp4.north)-43.85),2)))
print('a_trace',ra_trace)
rb_trace = abs(math.sqrt((float(tcp4.east)-70.15) ** 2 + (float(tcp4.north)-38.6)** 2))
print('b_trace',rb_trace)
rc_trace = abs(math.sqrt((float(tcp4.east)-70.7) ** 2 + (float(tcp4.north)-38.4) ** 2))
print('c_trace',rc_trace)
rd_trace = abs(math.sqrt((float(tcp4.east)-72) ** 2 + (float(tcp4.north)-37.8) ** 2))
print('d_trace',rd_trace)
# print("tcp4.east",tcp4.east)
print('tcp_fangxiang',tcp4.fang_xiang)
if 140.0 < (float(tcp4.fang_xiang)) < 180.0 and ra_trace<1.2:
fangxp = -540
print('右打死',fangxp)
elif 70< float(tcp4.fang_xiang)< 110 and rb_trace<1.2:
fangxp = 0
print('回正',fangxp)
elif 70 < float(tcp4.fang_xiang) < 110 and rc_trace<1.2:
fangxp = -540
print('右打死',fangxp)
elif 70< float(tcp4.fang_xiang) < 90 and rd_trace<1.2:
fangxp = 0
print('回正',fangxp)
else:
print('lose')
a = 0
g1_trace = abs(math.sqrt((float(tcp4.east) - 67.97) ** 2 + (float(tcp4.north) - 45.75) ** 2))
g2_trace = abs(math.sqrt((float(tcp4.east) - 68.35) ** 2 + (float(tcp4.north) - 43.96) ** 2))
if g1_trace< a <g2_trace:
direction == "line"
else:
direction == "rightback"
if direction == "line":
def pure_pursuit_control(state, cx, cy, pind):
global fangxp
ind = calc_target_index(state, cx, cy)
# if pind >= ind:
# ind = pind
if ind < len(cx):
tx = cx[ind]
print('tx', tx)
ty = cy[ind]
print('ty', ty)
else:
tx = cx[-1]
# print('tx1',tx)
ty = cy[-1]
# print('ty1',ty)
ind = len(cx) - 1
alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
# print('alpha',alpha)
if state.v < 0: # back
alpha = math.pi - alpha
Lf = k * state.v + Lfc
delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
if delta > np.pi / 6.0:
delta = np.pi / 6.0
# print('delta1', delta)
elif delta < - np.pi / 6.0:
delta = - np.pi / 6.0
# print('delta2', delta)
angle = delta * 57.3
fangxp = int(angle * 18)
print('fxp', fangxp)
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
time.sleep(0.04)
# print(delta)
return delta, ind
def calc_target_index(state, cx, cy):
# 搜索最临近的路点
dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
ind = d.index(min(d))
# print('ind',ind)
L = 0.0
Lf = k * state.v + Lfc
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cx[ind + 1] - cx[ind]
L += math.sqrt(dx ** 2 + dy ** 2)
ind += 1
# print('ind1', ind)
return ind
def main():
# 设置目标路点
cx = []
cy = []
target_speed = 5 / 3.6 # [m/s]
T = 500.0 # 最大模拟时间
# 设置车辆的出事状态
state = VehicleState(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed)
# state = VehicleState(x=89, y=20, yaw=0.0, v=0.0)
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
# print('x',x)
y = [state.y]
# print('y',y)
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_target_index(state, cx, cy)
while True:
# print('heading',tcp4.heading)
ai = PControl(target_speed, state.v)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
state = update(state, ai, di)
time = time + dt
x.append(state.x)
x1 = x[-1]
print('x1', x[-1])
y.append(state.y)
y1 = y[-1]
print('y1', y[-1])
yaw.append(state.yaw)
yaw1 = y[-1] * 57.3
print('yaw', yaw[-1])
t.append(time)
# logger.add('file.log')
# logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north))
v.append(state.v)
# print('v', v[-1])
t.append(time)
# plt.cla()
# plt.plot(cx, cy, ".r", label="course")
# plt.plot(x, y, "-b", label="trajectory")
# plt.plot(cx[target_ind], cy[target_ind], "go", label="target")
# plt.axis("equal")
# plt.grid(True)
# plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
# plt.pause(0.001)
def pure_pursuit_control(state, cx, cy, pind):
global fangxp
ind = calc_target_index(state, cx, cy)
# if pind >= ind:
# ind = pind
if ind < len(cx):
tx = cx[ind]
print('tx', tx)
ty = cy[ind]
print('ty', ty)
else:
tx = cx[-1]
# print('tx1',tx)
ty = cy[-1]
# print('ty1',ty)
ind = len(cx) - 1
alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
# print('alpha',alpha)
if state.v < 0: # back
alpha = math.pi - alpha
Lf = k * state.v + Lfc
delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
if delta > np.pi / 6.0:
delta = np.pi / 6.0
# print('delta1', delta)
elif delta < - np.pi / 6.0:
delta = - np.pi / 6.0
# print('delta2', delta)
angle = delta * 57.3
fangxp = int(angle * 18)
print('fxp', fangxp)
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
time.sleep(0.04)
# print(delta)
return delta, ind
def calc_target_index(state, cx, cy):
# 搜索最临近的路点
dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
ind = d.index(min(d))
# print('ind',ind)
L = 0.0
Lf = k * state.v + Lfc
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cx[ind + 1] - cx[ind]
L += math.sqrt(dx ** 2 + dy ** 2)
ind += 1
# print('ind1', ind)
return ind
def main():
# 设置目标路点
cx = [67.982, 68.007, 68.028, 68.055, 68.077, 68.089, 68.114, 68.138, 68.158, 68.189, 68.208, 68.223, 68.24, 68.265, 68.29, 68.312, 68.338, 68.365]
cy = [45.775, 45.671, 45.569, 45.466, 45.368, 45.256, 45.156, 45.056, 44.945, 44.838, 44.739, 44.638, 44.533, 44.419, 44.314, 44.214, 44.101, 43.99]
target_speed = 5 / 3.6 # [m/s]
T = 500.0 # 最大模拟时间
# 设置车辆的出事状态
state = VehicleState(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed)
# state = VehicleState(x=89, y=20, yaw=0.0, v=0.0)
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
# print('x',x)
y = [state.y]
# print('y',y)
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_target_index(state, cx, cy)
while True:
# print('heading',tcp4.heading)
ai = PControl(target_speed, state.v)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
state = update(state, ai, di)
time = time + dt
x.append(state.x)
x1 = x[-1]
print('x1', x[-1])
y.append(state.y)
y1 = y[-1]
print('y1', y[-1])
yaw.append(state.yaw)
yaw1 = y[-1] * 57.3
print('yaw', yaw[-1])
t.append(time)
# logger.add('file.log')
# logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north))
v.append(state.v)
# print('v', v[-1])
t.append(time)
if direction == "rightback":
while True:
global fangxp
db_file = "turndb.db"
conn = sqlite3.connect(db_file)
cur = conn.cursor()
# sql='INSERT INTO turndb(id) VALUES(1);'
sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
cur.execute(sql)
conn.commit()
cur.close()
conn.close()
# d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))
ra_trace = abs(math.sqrt(pow((float(tcp4.east)-68.35),2) + pow((float(tcp4.north)-43.85),2)))
print('a_trace',ra_trace)
rb_trace = abs(math.sqrt((float(tcp4.east)-70.15) ** 2 + (float(tcp4.north)-38.6)** 2))
print('b_trace',rb_trace)
rc_trace = abs(math.sqrt((float(tcp4.east)-70.7) ** 2 + (float(tcp4.north)-38.4) ** 2))
print('c_trace',rc_trace)
rd_trace = abs(math.sqrt((float(tcp4.east)-72) ** 2 + (float(tcp4.north)-37.8) ** 2))
print('d_trace',rd_trace)
# print("tcp4.east",tcp4.east)
print('tcp_fangxiang',tcp4.fang_xiang)
if 140.0 < (float(tcp4.fang_xiang)) < 180.0 and ra_trace<1.2:
fangxp = -540
print('右打死',fangxp)
elif 70< float(tcp4.fang_xiang)< 110 and rb_trace<1.2:
fangxp = 0
print('回正',fangxp)
elif 70 < float(tcp4.fang_xiang) < 110 and rc_trace<1.25:
fangxp = -540
print('右打死',fangxp)
elif 70< float(tcp4.fang_xiang) < 90 and rd_trace<1.2:
fangxp = 0
print('回正',fangxp)
else:
print('lose')
print('\n')
time.sleep(0.5)
@ -499,8 +736,8 @@ if __name__ == '__main__':
t9 = threading.Thread(target=car_back_lelf)
t10 = threading.Thread(target=car_back_lelf_fly)
# 启动线程
# t5.start()
t8.start()
t5.start()
# t8.start()
# t9.start()
# t10.start()
t6.start()
Write Preview
Loading…
Cancel
Save