def send_controls(angle, control, us=0.0, seen=False, throttle=0.0):
groundSteeringRequest = opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_GroundSteeringRequest(
)
groundSteeringRequest.groundSteering = math.radians(angle)
if lateral_control_enabled:
if debug:
print "Steering: %.1f" % angle
session.send(1090, groundSteeringRequest.SerializeToString())
pedalPositionRequest = opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_PedalPositionRequest(
)
pedalPositionRequest.position = us
if longitudinal_control_enabled and (seen or us < 1.4):
session.send(1086, pedalPositionRequest.SerializeToString())
if noBlue + noYellow > 1:
error = 0.7
else:
error = 0.5
if seeingOrange == 1:
error = 0.01
seeingOrange = 0
groundSteeringRequest.groundSteering = error * (0.7 * angle +
0.3 * prevAngle)
prevAngle = angle
session.send(1090, groundSteeringRequest.SerializeToString())
# Uncomment the following lines to accelerate/decelerate; range: +0.25 (forward) .. -1.0 (backwards).
pedalPositionRequest = opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_PedalPositionRequest(
)
#when it sees a car
if closestBlack[1] < 300:
pedalPositionRequest.position = 0
# elif closestBlack[1] > 390:
# pedalPositionRequest.position = 0.09
else:
pedalPositionRequest.position = 0.09
"""
if distances["front"] < 0.03:
pedalPositionRequest.position=0
elif distances["front"] < 0.8:
#if len(sortedCloseCars) > 0:
carSpeed=keepDistance(distances["front"],carSpeed)
pedalPositionRequest.position=carSpeed
