def get_PathSegment(self):
'''
Forms the calculates Straight path and Turn path into an ordered list of points
If WPsEnded == 1 the only SWP is the starting point
'''
try:
LS = OL.O_PosData(self.Straight.SubWP[0, 0],
self.Straight.SubWP[1, 0], float('NaN'),
float('NaN'))
LF = OL.O_PosData(
self.Straight.SubWP[0, len(self.Straight.SubWP) - 1],
self.Straight.SubWP[1, len(self.Straight.SubWP) - 1],
float('NaN'), float('NaN'))
CS = OL.O_PosData(self.Path.TurnSWP[0, 0], self.Path.TurnSWP[1, 0],
float('NaN'), float('NaN'))
CF = OL.O_PosData(
self.Path.TurnSWP[0, len(self.Path.TurnSWP) - 1],
self.Straight.SubWP[1,
len(self.Path.TurnSWP) - 1], float('NaN'),
float('NaN'))
'''
The Turn and Straight is received as a list of points.
The method checks, whether the start or end point of the Straight is closer to the
position of the Ship. If required, the order of the Straight is mirrored.
'''
if FL.Distance(self.Pos, LS) > FL.Distance(self.Pos, LF):
line = numpy.fliplr(self.Straight.SubWP)
LF = LS
else:
line = self.Straight.SubWP
'''
Then the method checks the relation of the Turn path Start and End points
to the (new) End point of the Straight. If required, the Turn is mirrored as well.
'''
if FL.Distance(LF, CS) > FL.Distance(LF, CF):
curve = numpy.fliplr(self.Path.TurnSWP)
else:
curve = self.Path.TurnSWP
except:
line = self.Straight.SubWP
curve = self.Path.TurnSWP
'''
Last, the method joins the two ordered array of points to a single list of points
'''
self.SegmentCoords = numpy.append(line, curve, 1)
def Control_Step(self):
'''
Outputs the calculated motor speeds based on the sensor inputs
'''
while 1:
'''
Gets the current destination point
'''
if self.WPsEnded == 0:
if len(self.SegmentCoords) > 0 and self.NextSWP_No < len(
self.SegmentCoords[0]):
self.NextSWP = OL.O_PosData(
self.SegmentCoords[0, self.NextSWP_No],
self.SegmentCoords[1, self.NextSWP_No], float('NaN'),
float('NaN'))
if self.mark == 0:
plt.plot(self.NextSWP.get_Pos_X(),
self.NextSWP.get_Pos_Y(),
marker='o')
self.mark = 1
else:
'''
If there is no current destination point, the method requests a new path
'''
self.LastWP = self.LastWP + 1
ret = self.Plan_LocalPath(self.Path.Range)
if ret == -1:
self.WPsEnded = 1
self.NextSWP_No = 100000
#self.NextSWP = self.Waypoints.get_SingleWayPoint(self.LastWP + 1)
self.NextSWP = self.retpos
self.get_PathSegment()
else:
self.NextSWP = self.retpos
'''
Calculation of the required heading
and the current deviation from required heading
'''
Theta_r = self.get_Thera_r()
delta = self.get_Delta(Theta_r)
'''
Checks the validity of the current destination point.
If the Distance < FollowDistance the
navigation procedure jumps to the next Sub-Waypoint
'''
valid = (FL.Distance(self.Pos, self.NextSWP) > self.FollowDistance)
if valid == 0 and self.WPsEnded == 1:
self.EndPath = 1
break
if valid == 1:
break
if self.WPsEnded == 0:
self.NextSWP_No = self.NextSWP_No + 1
self.mark = 0
'''
#############################################
# RUN CONTROL ALGORITHM HERE
'''
Ref = numpy.matrix([[1], [self.x[1] - delta]])
N = -self.F * self.x + self.N * Ref
'''
#############################################
'''
'''
Results of the control step in the following format:
Vertical(!) numpy Matrix(!)
'''
if self.EndPath == 0:
return N
else:
return numpy.matrix([[0], [0]])
