def generate_target_course(
x, y
): #tar manuelt inmatate coordinater och skapar ett polynom som blir referens!
csp = cubic_spline_planner.Spline2D(x, y)
s = np.arange(0, csp.s[-1], 0.1)
d = np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH,
0.1) #Skapa 0.1 tät vinkelrät linje
s_len = s.size
d_len = d.size
LUTs = np.zeros((s_len, d_len))
LUTd = np.zeros((s_len, d_len))
LUTx = np.zeros((s_len, d_len))
LUTy = np.zeros((s_len, d_len))
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
ix, iy = csp.calc_position(
i_s
) #i_s = incremental s ->här kan vi göra visualitionsconstraintsen
rx.append(ix) #Ref x
ry.append(iy) #Ref y
ryaw.append(csp.calc_yaw(i_s)) #Ref yaw
rk.append(csp.calc_curvature(i_s)) #Ref curvature
LTs, LTd, LTx, LTy, refx, refy, refyaw = [], [], [], [], [], [], []
s_count, d_count = -1, -1
for i_ss in s:
s_count = s_count + 1
LTs = i_ss
refx, refy = csp.calc_position(i_ss)
refyaw = csp.calc_yaw(i_ss)
for i_dd in d:
if i_dd == -MAX_ROAD_WIDTH:
d_count = -1
d_count = d_count + 1
LTd = i_dd
LTx = refx + i_dd * math.sin(refyaw)
LTy = refy - i_dd * math.cos(refyaw)
LUTs[s_count, d_count] = LTs
LUTd[s_count, d_count] = LTd
LUTx[s_count, d_count] = LTx
LUTy[s_count, d_count] = LTy
'''
print('Matrix Analysis ')
print(len(LUTs))
print(len(LUTx))
print(LUTs)
print(LUTd)
print(LUTx)
print(LUTy)
'''
plt.plot(LUTx[:, :], LUTy[:, :])
plt.grid(True)
plt.show()
return rx, ry, ryaw, rk, csp, LUTs, LUTd, LUTx, LUTy
def generate_target_course(x, y):
csp = cubic_spline_planner.Spline2D(x, y)
s = np.arange(0, csp.s[-1], 0.1)
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
ix, iy = csp.calc_position(i_s)
rx.append(ix)
ry.append(iy)
ryaw.append(csp.calc_yaw(i_s))
rk.append(csp.calc_curvature(i_s))
return rx, ry, ryaw, rk, csp
def generate_target_course(x, y):
csp = cubic_spline_planner.Spline2D(x, y)
s = np.arange(0, csp.s[-1], 0.1)
d = np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH,
0.1) #Creates a 0.1 perp. line
s_len = s.size
d_len = d.size
LUTs = np.zeros((s_len, d_len))
LUTd = np.zeros((s_len, d_len))
LUTx = np.zeros((s_len, d_len))
LUTy = np.zeros((s_len, d_len))
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
ix, iy = csp.calc_position(
i_s) #i_s = incremental s ->Visual constraints
rx.append(ix) #Ref x
ry.append(iy) #Ref y
ryaw.append(csp.calc_yaw(i_s)) #Ref yaw
rk.append(csp.calc_curvature(i_s)) #Ref curvature
LTs, LTd, LTx, LTy, refx, refy, refyaw = [], [], [], [], [], [], []
s_count, d_count = -1, -1
for i_ss in s:
s_count = s_count + 1
LTs = i_ss
refx, refy = csp.calc_position(i_ss)
refyaw = csp.calc_yaw(i_ss)
for i_dd in d:
if i_dd == -MAX_ROAD_WIDTH:
d_count = -1
d_count = d_count + 1
LTd = i_dd
LTx = refx + i_dd * math.sin(refyaw)
LTy = refy - i_dd * math.cos(refyaw)
LUTs[s_count, d_count] = LTs
LUTd[s_count, d_count] = LTd
LUTx[s_count, d_count] = LTx
LUTy[s_count, d_count] = LTy
print('Matrix Analysis ')
print(len(LUTs))
print(len(LUTx))
print(LUTs)
print(LUTd)
print(LUTx)
print(LUTy)
plt.plot(LUTx[:, :], LUTy[:, :])
#plt.plot(LUTx[:,-1], LUTy[:,-1])
plt.grid(True)
plt.show()
return rx, ry, ryaw, rk, csp, LUTs, LUTd, LUTx, LUTy
def generate_coarse_course(x, y):
csp = cubic_spline_planner.Spline2D(x, y)
s = np.arange(0, csp.s[-1],
1) # 2m取一个间隔点，利用dp进行基于reference line的轨迹优化， 并保证一定的计算速度
rx, ry, ryaw, rk = [], [], [], []
for i_s in s: # 利用离散点x、y生成一条关于路程s的曲线？ 然后离散成x、y、yaw、curvature
ix, iy = csp.calc_position(i_s)
rx.append(ix)
ry.append(iy)
ryaw.append(csp.calc_yaw(i_s))
rk.append(csp.calc_curvature(i_s))
return rx, ry, ryaw, rk, csp
def generate_target_course(x, y):
'''
根据way points利用分段三次样条多项式生成平滑参考轨迹
:param x:
:param y:
:return: 参考信息[x,y,yaw,c,csp] csp:样条曲线类，内包含求位置、曲率、航向角的函数
'''
csp = cubic_spline_planner.Spline2D(x, y)
s = np.arange(0, csp.s[-1], 0.1)
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
ix, iy = csp.calc_position(i_s)
rx.append(ix)
ry.append(iy)
ryaw.append(csp.calc_yaw(i_s))
rk.append(csp.calc_curvature(i_s))
return rx, ry, ryaw, rk, csp
def __init__(self):
print("Initializaing Robot")
# Position Variables
# Will replace with actual values
self.x = 150
self.y = 0
self.yaw = 180
# Driving Variables
self.target_power = 50
self.target_velocity = 60
self.v = 0
self.max_turning_angle = 70 # Going straight, >0=CCW, <0=CW
self.min_turn_radius = 0.25 # [m]
self.wheel_base = 20.32
# Navigation Variables
self.state = HOME
self.circle_completion_state = False
self.current_circle = 0
self.spline = spline.Spline2D([0, 1], [0, 1])
#Condition Variables
self.object_count = 0
self.beam_state = True
self.end_time = False
self.off_track = False
self.start_time = time.time()
#UDP Server
#Sets the io scheme to be based on board pin numbers
#Use io.BCM for GPIO based classifications
io.setmode(io.BOARD)
# Motor initialization----------------------------------------------------------
self.PWM_MAX = 100
io.setwarnings(False)
self.leftMotor_DIR_pin = 15
io.setup(self.leftMotor_DIR_pin, io.OUT)
self.rightMotor_DIR_pin = 16
io.setup(self.rightMotor_DIR_pin, io.OUT)
io.output(self.leftMotor_DIR_pin, False)
io.output(self.rightMotor_DIR_pin, False)
self.leftMotor_PWM_pin = 11
self.rightMotor_PWM_pin = 12
io.setup(self.leftMotor_PWM_pin, io.OUT)
io.setup(self.rightMotor_PWM_pin, io.OUT)
# MAX Frequency 20 Hz
self.leftMotorPWM = io.PWM(self.leftMotor_PWM_pin, 1000)
self.rightMotorPWM = io.PWM(self.rightMotor_PWM_pin, 1000)
self.leftMotorPWM.start(0)
self.leftMotorPWM.ChangeDutyCycle(0)
self.rightMotorPWM.start(0)
self.rightMotorPWM.ChangeDutyCycle(0)
self.leftMotorPower = 0
self.rightMotorPower = 0
# Beam Break Setup----------------------------------------------------------------
io.setmode(io.BOARD)
io.setup(40, io.IN)
def generate_path(self):
x = self.x
y = self.y
c = self.current_circle
#print("Current Circle: "+ str(c))
state = self.state
#If returned from circle, move to turnaround state
if (state == PATHING_HOME):
print("Arrived home, turning arround")
self.state = TURNAROUND
self.object_count = 0
return
elif (state == TURNAROUND):
print(
"Turned around, currently at home preparing to path back out")
self.x = 150
self.y = 0
self.yaw = 180
self.state = HOME
return
#Else state is either HOME or PATHING_CIRCLE
else:
#If at home, path back into circle
if (state == HOME):
if (self.end_time == True):
endgame()
return
if (c >= 5):
c = 4
print("Pathing out of home to Circle: " +
str(self.current_circle))
self.state = PATHING_CIRCLE
cx = [
x, circleRadii[c + 1], circleXY[c], 0, -circleXY[c],
-circleRadii[c], -circleXY[c], 0, circleXY[c]
]
cy = [
y, 0, circleXY[c], circleRadii[c], circleXY[c], 0,
-circleXY[c], -circleRadii[c], -circleXY[c]
]
#If pathing circle, either return home or continue to next circle depending on the check states
elif (state == PATHING_CIRCLE):
#If bay area is full or time is out
if (self.object_count >= 3 or self.end_time == True):
print("Bay area full or time ran out, returning home")
self.state = PATHING_HOME
if (c >= 5):
c = 4
cx = [x, circleRadii[c + 1], 150]
cy = [y, 0, 0]
self.current_circle += 1
#Continue to next circle
else:
target_circle = c + 1
if (target_circle <= 4
and not self.circle_completion_state):
print("Pathing from Circle: " +
str(self.current_circle) + " around Circle: " +
str(target_circle))
cx = [
x, circleRadii[c], circleXY[c], 0, -circleXY[c],
-circleRadii[c], -circleXY[c], 0, circleXY[c]
]
cy = [
y, 0, circleXY[c], circleRadii[c], circleXY[c], 0,
-circleXY[c], -circleRadii[c], -circleXY[c]
]
self.current_circle += 1
else:
if (not self.circle_completion_state):
print(
"All circles pathed, now looping around Circle: 0"
)
self.circle_completion_state = True
self.current_circle = 0
cx = [
x, circleRadii[0], circleXY[0], 0, -circleXY[0],
-circleRadii[0], -circleXY[0], 0, circleXY[0]
]
cy = [
y, 0, circleXY[0], circleRadii[0], circleXY[0], 0,
-circleXY[0], -circleRadii[0], -circleXY[0]
]
self.spline = spline.Spline2D(cx, cy)
