class Drive(object):
def __init__(self):
self.pid = PIDController(kP=1.0, kI=0.0, kD=0.1)
# motors
try:
self.steerPair = MoveSteering(OUTPUT_B, OUTPUT_C)
except ev3dev2.DeviceNotFound as e:
Debug.print("Error:", e)
self.speed = Config.data['pid']['fast']['speed_max']
def updateConfig(self):
self.speed = Config.data['pid']['fast']['speed_max']
self.pid.updateConfig()
def followLine(self, sensValues):
colorLeft = sensValues["ColorLeft"][1] # TODO: HSL? Lichtwert anpassen
colorRight = sensValues["ColorRight"][1]
feedback = colorLeft - colorRight
self.pid.update(feedback)
turn = self.pid.output
self.steerPair.on(turn, self.speed)
def followLineSlow(self, speed, sensValues):
colorLeft = sensValues["ColorLeft"][1] # TODO: HSL? Lichtwert anpassen
colorRight = sensValues["ColorRight"][1]
feedback = colorLeft - colorRight
self.pid.update(feedback)
turn = self.pid.output
self.steerPair.on(turn, self.speed)
def turn(self, action):
def right():
self.steerPair.on_for_degrees(-100, 20, 200)
def left():
self.steerPair.on_for_degrees(100, 20, 200)
if action == "right":
right()
elif action == "left":
left()
elif action == "skip":
self.steerPair.on_for_degrees(
0, -20, 50) # back off until centered on cross
self.steerPair.wait_until_not_moving(2000)
left()
else:
raise AttributeError("no valid action string given for turn()")
def brake(self):
self.steerPair.off()
if lml.position > 328 or lmr.position > 328: #299
mt.off()
break
sleep(0.1)
lml.reset()
lmr.reset()
if os.path.isfile(GREEN_LIGHT_path):
os.remove(GREEN_LIGHT_path)
if os.path.isfile(GREEN_LIGHT_path):
print('Clean GREEN_LIGHT')
os.remove(GREEN_LIGHT_path)
# 第一個右彎
ms.on_for_rotations(50, 100, 1) # gs.angle > 76, 1.08
ms.wait_until_not_moving()
# 2
mt.on(rs+0.2, rs)
while True:
if csl.reflected_light_intensity > 39 or csr.reflected_light_intensity > 39:
mt.off()
break
'''
if os.path.isfile(RESTART_TRAFFIC_LIGHT_path):
os.remove(RESTART_TRAFFIC_LIGHT_path)
