def make_move(self):
t = (self.loop + (self.side == 3)) * 6
print("Moving for {}".format(t))
return Timed(VelocityX(0.3), t)
self._finish()
return
self.depth()
class ToPipe(FiniteTask):
def firstrun(self):
self.start = time.time()
self.count = time.time()
def run(self):
if shm.pipe_results.heuristic_score.get() < 3000:
self.start = time.time()
if self.this_run - self.start > .3 or self.this_run - self.count > 60:
self._finish()
class Pause(FiniteTask):
def firstrun(self, wait=0):
self.start = time.time()
def run(self, wait=0):
if self.this_run - self.start > wait:
self._finish()
Gate = Sequential(dive(), MasterConcurrent(ToPipe(), VelocityX(.4)),
VelocityX(0))
def on_run(self, *args, **kwargs):
self.align()
if self.align.finished and not self.align.success:
VelocityX(0)()
VelocityY(0)()
# This is the unholy cross between my (Will's) and Zander's styles of mission-writing
gate = Sequential(
Log('Depthing...'),
BigDepth(DEPTH_TARGET),
Log('Lining up...'),
ConsistentTask(Concurrent(
Depth(DEPTH_TARGET),
XTarget(x=results_groups.gate_center_x.get, db=0.03),
finite=False
)),
Log('Driving forward...'),
MasterConcurrent(
Consistent(test=lambda: results_groups.width.get() < settings.gate_width_threshold, count=0.2, total=0.3, invert=True, result=True),
Depth(DEPTH_TARGET),
VelocityX(0.1 if is_mainsub() else 0.1),
While(task_func=lambda: XTarget(x=results_groups.gate_center_x.get, db=0.018), condition=True),
),
# Jank
Timed(VelocityX(0 if is_mainsub() else -0.1), 2),
VelocityX(0),
Log('Lining up with red side...'),
ConsistentTask(Concurrent(
Depth(DEPTH_TARGET),
XTarget(x=results_groups.gate_center_x.get, db=0.05),
finite=False,
)),
Log('Charging...'),
Timed(VelocityX(0.5 if is_mainsub() else 0.2), settings.charge_dist),
Log('Through gate!'),
)
def stop(self):
VelocityY(0)()
VelocityX(0)()
def stop():
return Concurrent(
VelocityX(0),
VelocityY(0),
)
withReSearchCalledOnFail = lambda task: lambda: Retry(lambda: \
Conditional(main_task=task(), on_fail= \
Fail(TinySearch())), attempts=2)
# The final fallback case. If the called side cannot be found, attempt to ram any side of the triangular buoy if possible.
RamAnything = lambda backspeed=0.2, backtime=10: Sequential(
Log('Failed, backing up'), Zero(), Timeout(SearchTriangle(), 200),
AlignAnyNormal(), ApproachAny(), RamV())
# Decorator that wraps a task to Ram Anything if it fails
withRamAnythingOnFail = lambda task: lambda: Conditional(
main_task=Timeout(task(), 100), on_fail=RamAnything())
# Backs up and search for the triangular buoy again
SearchTriangleOnFail = lambda backspeed=0.2, backtime=10: Sequential(
Log('Failed, backing up'), Zero(), Timed(VelocityX(-backspeed), backtime),
Zero(), Timeout(SearchTriangle(), 120))
SearchSingleOnFail = lambda backspeed=0.2, backtime=10: Sequential(
Log('backing up'), Zero(), Timed(VelocityX(-backspeed), backtime), Zero(),
Timeout(SearchSingle(), 45))
# Decorator that wraps a task to search for the triangular buoy on fail
withSearchTriangleOnFail = lambda task: lambda: Retry(lambda: Conditional(
main_task=task(), on_fail=Fail(SearchTriangleOnFail())),
attempts=2)
withSearchSingleOnFail = lambda task: lambda: Retry(lambda: Conditional(
main_task=task(), on_fail=Fail(SearchSingleOnFail())),
attempts=2)
def firstrun(self, *args, **kwargs):
# x-axis on the camera corresponds to sway axis for the sub
self.pid_loop_x = PIDLoop(output_function=VelocityY(), negate=True)
self.pid_loop_y = PIDLoop(output_function=VelocityX(), negate=False)
else:
print("0")
return 0
def enable_hydrophones():
shm.hydrophones_settings.enabled.set(1)
track = Sequential(
# FunctionTask(enable_hydrophones),
Depth(1),
Concurrent(
While(lambda: FunctionTask(update), lambda: True),
While(lambda: Heading(get_target_heading), lambda: True),
While(lambda: VelocityX(calc_speed), lambda: True),
))
class _TrackPinger(Task):
def on_first_run(self, speed=0.4):
self.last_shmval = None
self.last_target_heading = None
self.last_target_elevation = 0
self.checker = ConsistencyCheck(count=3, total=7)
def update(self):
self.shmval = shm.hydrophones_results_track.tracked_ping_heading.get()
# print('last: ' + str(self.last_shmval) + ', new: ' + str(self.shmval))
if self.last_shmval is None or self.shmval != self.last_shmval:
lltarget_heading = self.last_target_heading if self.last_target_heading is not None else self.shmval
def make_repeat(self, width, stride, speed, rightFirst, checkBehind):
sway_time = width / speed
stride_time = stride / speed
dir = 1 if rightFirst else -1
if checkBehind:
self.repeat = Sequential(Timed(VelocityX(-speed), stride_time),
Timed(VelocityX(speed), stride_time),
VelocityX(0),
Timed(VelocityY(speed * dir), sway_time),
VelocityY(0.0),
Timed(VelocityX(speed), stride_time),
VelocityX(0.0),
Timed(VelocityY(-speed * dir), sway_time),
VelocityY(0.0),
Timed(VelocityY(-speed * dir), sway_time),
VelocityY(0.0),
Timed(VelocityX(speed), stride_time),
VelocityX(0.0),
Timed(VelocityY(speed * dir), sway_time),
VelocityY(0.0))
else:
self.repeat = Sequential(Timed(VelocityY(speed * dir), sway_time),
VelocityY(0.0),
Timed(VelocityX(speed), stride_time),
VelocityX(0.0),
Timed(VelocityY(-speed * dir), sway_time),
VelocityY(0.0),
Timed(VelocityY(-speed * dir), sway_time),
VelocityY(0.0),
Timed(VelocityX(speed), stride_time),
VelocityX(0.0),
Timed(VelocityY(speed * dir), sway_time),
VelocityY(0.0))
),
)
BackUpUntilVisible = lambda num, speed, timeout: Conditional(
Defer(
main_task=Either(
Sequential(
# Get at least a little bit away first
FakeMoveX(dist=-0.3, speed=0.2),
MasterConcurrent(
Consistent(lambda: shm_vars[num].visible.get(),
count=1,
total=1.5,
result=True,
invert=False),
VelocityX(-speed),
),
),
Fail(
Timer(timeout), # don't back up too far
),
),
deferred=Sequential(VelocityX(0), ),
),
on_success=Succeed(Sequential(NoOp(), ), ),
on_fail=Conditional(
FunctionTask(lambda: num == 0),
on_success=Sequential(
Log('Timed out, searching for buoy again'),
SearchBuoy(num=num, count=4, total=5),
),
def on_first_run(self, vel, *args, **kwargs):
if shm.jank_pos.x.get() < 0:
self.task = VelocityX(vel)
else:
self.task = VelocityX(vel * -1)
lambda: Sequential(
FunctionTask(lambda: shm.navigation_desires.heading.set(
(shm.kalman.heading.get() - .95 * math.degrees(
heading_sub_degrees(
math.pi / 2,
shm.path_results.angle_1.get() % math.pi, math.pi * 2))
) % 360)),
#Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get())) if shm.path_results.num_lines.get() == 2 else 'X'),
Timer(.05)),
lambda: True))
aaa = Sequential(
Depth(2),
Timer(1),
Log("following"),
Timed(Concurrent(VelocityX(.5), FollowLine), 23),
Log("following done"),
Timer(1),
MoveY(-.375, deadband=.05),
MoveX(1.1, deadband=.05),
Log("turning 1"),
RelativeToInitialHeading(90),
Log("turning 2"),
Timer(1),
MoveX(.75, deadband=.05),
Log("turning 1"),
RelativeToInitialHeading(90),
Log("turning 2"),
Timer(1),
MoveX(1.1, deadband=.05),
MoveY(.375, deadband=.05),
def firstrun(self):
self.go = VelocityX()
self.start = time.time()
self.progress = 0
def on_run(self, N):
new_angle = shm.kalman.roll.get()
angle_diff = new_angle - self.current_r
self.current_r = new_angle
Ad = ((angle_diff + 180) % 360) - 180
self.progress += Ad if N > 0 else -Ad
if self.progress > abs(N): self.finish()
pv = shm.settings_roll.kP.get()
roll = lambda: Sequential(
FunctionTask(lambda: shm.settings_roll.kP.set(.6)),
#*([RelativeToInitialRoll(90)] * 8),
#Timed(RelativeToCurrentRoll(90), 6),
MasterConcurrent(RollDegrees(720 - 45), RelativeToCurrentRoll(90),
VelocityX(.35)),
#MasterConcurrent(RollDegrees(360), RelativeToCurrentRoll(90), VelocityX(.35)),
#Timer(.5),
#MasterConcurrent(RollDegrees(360 - 45), RelativeToCurrentRoll(90), VelocityX(.35)),
#MasterConcurrent(RollDegrees(-720 + 45), RelativeToCurrentRoll(-90)),
FunctionTask(lambda: shm.settings_roll.kP.set(pv)),
Concurrent(
Roll(0),
VelocityX(0),
),
Timer(2),
)
t2 = Timed(RelativeToCurrentRoll(-90), 3)
def firstrun(self):
self.start = time.time()
self.surge = VelocityX(.3)
self.count = time.time()
self.surge()
While(
lambda: Sequential(
FunctionTask(lambda: shm.navigation_desires.heading.set(
(shm.kalman.heading.get() - .95 * math.degrees(
heading_sub_degrees(
math.pi / 2,
shm.path_results.angle_1.get() % math.pi, math.pi * 2))
) % 360)),
#Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get())) if shm.path_results.num_lines.get() == 2 else 'X'),
Timer(.05)),
lambda: True))
aaa = Sequential(Log("Started mission"), Depth(1.5), Log("At depth"),
FakeMoveX(2, .65), Zero(), Log("Starting tracking"), Timer(1),
Log("following"),
Timed(Concurrent(VelocityX(.5), FollowLine), 24),
Log("following done"), Timed(VelocityX(-.5), .1), Zero(),
Timer(1), FakeMoveY(-.6, .65), Timed(VelocityY(.5), .05),
Zero(), Timer(1), FakeMoveX(2.8, .65),
Timed(VelocityX(-.5), .1), Zero(), Timer(1), Log("turning 1"),
RelativeToInitialHeading(90), Log("turning 2"), Timer(1),
FakeMoveX(2.8, .65), Timed(VelocityX(-.5), .1), Zero(),
Timer(1), Log("turning 1"), RelativeToInitialHeading(90),
Log("turning 2"), Timer(1), FakeMoveX(2.8, .65),
Timed(VelocityX(-.5), .1), Zero(), Timer(1),
FakeMoveY(.8, .65), Timed(VelocityY(-.5), .05), Zero(),
Timer(1), Timed(Concurrent(VelocityX(.5), FollowLine), 25),
FunctionTask(lambda: shm.switches.soft_kill.set(1)))
#bbb = Sequential(
# MoveY(.375, deadband=.05),
CenterCentroid,
Disjunction,
ConsistentTask,
PrintDone,
Altitude,
AlignAmlan,
# AMLANS,
Infinite,
Except,
)
from mission.framework.search import (
SearchFor,
VelocitySwaySearch,
)
from mission.missions.actuate import (
FireGreen,
FireRed,
)
from mission.constants.config import cash_in as settings
stupid_castor = Sequential(
# Timed(VelocityX(0.4), 30),
MoveXRough(50),
VelocityX(0),
RelativeToInitialHeading(50),
Timed(VelocityX(0.4), 60),
finite=True
)
def on_first_run(self):
self.has_made_progress = True
self.seen_frames_checker = ConsistencyCheck(3, 3, strict=False)
def location_validator(buoy):
return self.seen_frames_checker.check(buoy.probability.get() != 0)
self.depth_task = DepthRestore()
self.heading_task = HeadingRestore()
self.up_task = DepthRestore(constants.BUOY_OVER_DEPTH)
self.dodge_vel = -.4 if yellowRight else .4
self.over_task = Timed(VelocityX(.4), 8)
self.heading_task = HeadingRestore()
self.task = Sequential(
Depth(constants.BUOY_SEARCH_DEPTH),
Conditional(
MasterConcurrent(
Sequential(
self.heading_task,
#Depth(0.9, error=.01)
#SeqLog("Looking for red buoy"), LocateFirstBuoy(lambda: location_validator(red_buoy_results), forward=True, right=BUOY_RIGHT_TO_REACH[0], middle=yellow_buoy_results),
Buoy(red_buoy_results,
first_buoy=True,
right=redRight()),
#self.depth_task,
#SeqLog("Looking for green buoy stage 1"), LocateBuoyStrafe(lambda: location_validator(yellow_buoy_results), right=True, timeout=3),
SeqLog("Looking for green buoy"),
LocateBuoyStrafe(
lambda: location_validator(green_buoy_results),
right=greenRight(),
timeout=3),
Buoy(green_buoy_results, right=greenRight()),
#self.depth_task,
SeqLog("Looking for yellow buoy"),
LocateBuoyStrafe(
lambda: location_validator(yellow_buoy_results),
right=yellowRight(),
timeout=2),
Buoy(yellow_buoy_results,
right=yellowRight(),
yellow=True),
Log("re-aligning red buoy"),
LocateBuoyStrafe(
lambda: location_validator(red_buoy_results),
right=secondRedRight(),
timeout=2),
Buoy(red_buoy_results,
right=secondRedRight(),
yellow=False,
align_only=True),
),
PreventSurfacing(),
),
on_success=Sequential(
Zero(),
self.heading_task,
SeqLog("Rising to Over depth"),
self.up_task,
SeqLog("Going over buoys"),
self.over_task,
),
on_fail=Sequential(
Zero(),
self.heading_task,
SeqLog("Going to Over depth"),
self.up_task,
SeqLog("Going over buoys"),
self.over_task,
Timed(VelocityX(.4), 8),
)))
print('flipped: ' + str(h))
if h != last_pinger_heading:
last_sub_heading = math.radians(shm.kalman.heading.get())
last_pinger_heading = h
return h + last_sub_heading
def get_desired_heading():
h = shm_heading()
if h < 0:
h += math.pi * 2
return math.degrees(h)
def get_desired_vel_x():
h = shm_heading()
if -math.pi / 4 < h < math.pi / 4:
return math.cos(h) * 0.3 # maybe change this function later?
else:
return 0
track = Sequential(
Depth(1.0),
While(
lambda: Sequential(
Heading(get_desired_heading),
VelocityX(get_desired_vel_x),
), lambda: True))
def on_first_run(self, *args, **kwargs):
self.pid_loop = PIDLoop(output_function=VelocityX())
largest_step=0.5,
timeout=4,
pause=0,
deadband=0.1):
init_depth = shm.kalman.depth.get()
steps = math.ceil(abs(depth - init_depth) / largest_step)
depth_steps = interpolate_list(init_depth, depth, steps)
params = [(step, timeout, pause, deadband) for step in depth_steps]
#self.use_task(Sequential(*interleave(tasks_from_params(Depth, depth_steps), tasks_from_param(Timer, timeout, length=steps))))
self.use_task(
Sequential(*tasks_from_params(Descend, params, tup=True)))
FakeMoveX = lambda dist, speed: Sequential(
MasterConcurrent(Timer(abs(dist / speed)),
VelocityX(-speed if dist < 0 else speed)),
MasterConcurrent(Timer(0.2), VelocityX(speed if dist < 0 else -speed)),
MasterConcurrent(Timer(0.2), VelocityX(0)),
)
FakeMoveY = lambda dist, speed: Sequential(
MasterConcurrent(Timer(abs(dist / speed)),
VelocityY(-speed if dist < 0 else speed)),
MasterConcurrent(Timer(0.2), VelocityY(speed if dist < 0 else -speed)),
MasterConcurrent(Timer(0.2), VelocityY(0)),
)
# A version of VelocitySwaySearch, but better.
# We use this for minisub because it doesn't drift as much.
class ForwardSearch(Task):
#While(lambda: Log("V: {} h: {} d: {} x: {} y: {} c: {}".format(shm.path_results.num_lines.get(), heading(), math.degrees(heading_sub_degrees(trg, heading(), math.pi*2)), shm.path_results.center_x.get(), shm.path_results.center_y.get(), heading_to_vector(heading()) * dst)), lambda: True),
#While(lambda: Log(s(int(math.degrees(heading_sub_degrees(trg, heading(), math.pi*2))) + 180)), lambda: True),
#While(lambda: Log(s2(int(math.degrees(trg)), int(math.degrees(heading()))) if shm.path_results.num_lines.get() == 2 else 'X'), lambda: True),
),
Log("Centered on Pipe in PipeAlign!"),
#FunctionTask(lambda: shm.navigation_desires.heading.set(-180/math.pi*heading()+shm.kalman.heading.get()))
)
def t180(a):
return (a + math.pi) % (2 * math.pi)
find_bins = lambda: Sequential(
Depth(.1),
FunctionTask(VelocityX(.5)),
While(lambda: NoOp(), lambda: not shm.bins_status.cover_visible.get()),
VelocityX(0),
)
def get_cover_vect():
return np.float32(
[shm.bins_status.cover_maj_x.get(),
shm.bins_status.cover_maj_y.get()])
def get_cover_center():
return shm.bins_status.cover_x.get(), shm.bins_status.cover_y.get()
success = condition()
if success:
self.finish(success=success)
else:
self.finished = False
task()
pv = shm.settings_roll.kP.get()
rolly_roll = lambda:\
Sequential(
FunctionTask(lambda: shm.settings_roll.kP.set(.6)),
MasterConcurrent(
RollDegrees(360 * 2 - 180),
RelativeToCurrentRoll(90),
VelocityX(0)
),
Timer(1),
FunctionTask(lambda: shm.settings_roll.kP.set(pv)),
Roll(0, error=10)
)
def focus_elem(elem_x, offset=0):
return HeadingTarget(
point=[lambda: elem_x().get(), 0],
target=lambda: [shm.gate.img_width.get() / 2 + offset, 0],
px=0.3,
deadband=(20, 1))
def PushLever():
def TargetLever(py):
return ForwardTarget(
point=lambda:
(shm.bins_status.lever_x.get(), shm.bins_status.lever_y.get() /
(1 + shm.bins_status.lever_sz.get() / 50)),
#target=lambda: (0, .3 * min(100, shm.bins_status.lever_sz.get()) / 100),
target=lambda: (0, .25 /
(1 + shm.bins_status.lever_sz.get() / 50)),
valid=shm.bins_status.lever_visible.get,
deadband=(DEADBAND, DEADBAND),
px=1,
py=py,
ix=.05,
iy=.05)
return Sequential(
Log("PushLever start"),
FunctionTask(VelocityX(.2)),
MasterConcurrent(
Consistent(lambda: shm.bins_status.lever_sz.get() > 90,
count=.5,
total=.75,
invert=False,
result=True),
While(lambda: TargetLever(1.5), lambda: True),
While(
lambda: FunctionTask(
VelocityX(.2 / (1 + 2 * (abs(shm.bins_status.lever_x.get(
)) + abs(shm.bins_status.lever_y.get() - .25))))),
lambda: True)),
#Log("Higher P"),
#MasterConcurrent(
# Consistent(lambda: shm.bins_status.lever_sz.get() > 100, count=.5, total=.75, invert=False, result=True),
# While(lambda: TargetLever(.8), lambda: True),
# While(lambda: FunctionTask(
# VelocityX(.2 / (1 + 2 * (abs(shm.bins_status.lever_x.get()) + abs(shm.bins_status.lever_y.get()-.15))))
# ), lambda: True)
#),
#Log("targeting"),
#TargetLever(),
Log("zoom zoom"),
#RelativeToInitialDepth(-.1),
#Timed(RelativeToCurrentDepth(-2), .7),
#FunctionTask(RelativeToInitialDepth(0)),
Concurrent(
Sequential(
Timer(1),
Log("Forwards!"),
FunctionTask(VelocityX(1)),
#VelocityDepth(0),
),
Sequential(
Timed(RelativeToCurrentDepth(-.4), 1),
RelativeToInitialDepth(0),
),
Timer(4),
),
#Timed(
# While(TargetLever, lambda: True),
# 5
#),
Timed(VelocityX(-.8), .5),
VelocityX(0),
Log("waiting"),
Timer(4),
#RelativeToInitialHeading(0),
#Timed(VelocityX(-.8), 1),
#RelativeToInitialHeading(0),
#FunctionTask(VelocityX(0)),
#Timer(5),
#TargetLever()
)
def make_repeat(self, forward, stride, speed, rightFirst, checkBehind):
dir = 1 if rightFirst else -1
if checkBehind:
self.repeat = Sequential(Timed(VelocityX(-speed), forward),
Timed(VelocityX(speed), forward),
VelocityX(0),
Timed(VelocityY(speed * dir), stride),
VelocityY(0.0),
Timed(VelocityX(speed), forward),
VelocityX(0.0),
Timed(VelocityY(-speed * dir), stride),
VelocityY(0.0),
Timed(VelocityY(-speed * dir), stride),
VelocityY(0.0),
Timed(VelocityX(speed), forward),
VelocityX(0.0),
Timed(VelocityY(speed * dir), stride),
VelocityY(0.0))
else:
self.repeat = Sequential(Timed(VelocityY(speed * dir), stride),
VelocityY(0.0),
Timed(VelocityX(speed), forward),
VelocityX(0.0),
Timed(VelocityY(-speed * dir), stride),
VelocityY(0.0),
Timed(VelocityY(-speed * dir), stride),
VelocityY(0.0),
Timed(VelocityX(speed), forward),
VelocityX(0.0),
Timed(VelocityY(speed * dir), stride),
VelocityY(0.0))
self.start = time.time()
self.state = 'a'
return
if self.state is 'a':
self.a()
if self.this_run - self.start > 6:
self.a._finish()
self._finish()
HeadingCenter = lambda: None
PipeCenter = center()
search = Search()
lineup = align()
test1 = Sequential(PipeCenter, lineup, VelocityX(.3))
check = CheckBuoy()
PipeNV = Sequential(Search(), center(), align(), VelocityX(.25))
PipeQ = Sequential(VisionModule("Recovery"), Pause(3), Search(20), center(), align())
Pipe = lambda begin, end: Sequential(VisionModule(begin), VisionModule(end, stop=True), Search(), center(), align(), Finite(Depth(2.2)))
Vision = lambda begin, end: Sequential(VisionModule(begin), VisionModule(end, stop=True))
BPipe = lambda begin, end: Sequential(VisionModule(begin), VisionModule(end, stop=True), center(), align(), Finite(Depth(2.4)))
PipeNoSearch = lambda begin, end: Sequential(VisionModule(begin), VisionModule(end, stop=True), Pause(2), SurgeToPipe(), center(), align())
FirstPipe = Sequential(VisionModule("redbuoy"), center(), align())
tobuoy = Sequential(FirstPipe, Forward(), MasterConcurrent(ToBuoy(), VelocityX(.6)), VelocityX(0))#, CheckBuoy())
towire = Sequential(Vision(begin="portal", end="redbuoy"), CheckPipe(), Finite(Depth(2.2)), MasterConcurrent(ToWire(), VelocityX(.35)), VelocityX(0))#, CheckWire())
totorpedoes = Sequential(PipeNoSearch(begin="torpedoes", end="portal"), MasterConcurrent(ToTorpedoes(), VelocityX(.25)), VelocityX(0))
# Require a really high fail rate - path vision can be finicky
Consistent(visible_test(1),
count=2.5,
total=3,
result=False,
invert=True),
),
Conditional(
FirstPipeGroupFirst(bend_right),
on_success=FollowPipe(shm.path_results.angle_1, shm.
path_results.angle_2),
on_fail=FollowPipe(shm.path_results.angle_2, shm.
path_results.angle_1)),
),
on_success=Sequential(
Timed(VelocityX(.1), settings.post_dist),
Log("Done!"),
Zero(),
Log("Finished path!"),
),
on_fail=Fail(
Sequential(
Log("Lost sight of path. Backing up..."),
FakeMoveX(-settings.failure_back_up_dist,
speed=settings.failure_back_up_speed),
), ),
),
),
attempts=5))
path = FullPipe()
def firstrun(self, *args, **kwargs):
print("forward started")
self.pid_loop_x = PIDLoop(output_function=VelocityX())
self.total_distance = 0