make turn decision for line following
def check_shift_turn(angle, shift):
turn_state = 0
if angle < tconf.turn_angle or angle > 180 - tconf.turn_angle:
turn_state = np.sign(90 - angle)
shift_state = 0
if abs(shift) > tconf.shift_max:
shift_state = np.sign(shift)
return turn_state, shift_state
def get_turn(turn_state, shift_state):
turn_dir = 0
turn_val = 0
if shift_state != 0:
turn_dir = shift_state
turn_val = tconf.shift_step if shift_state != turn_state else tconf.turn_step
elif turn_state != 0:
turn_dir = turn_state
turn_val = tconf.turn_step
return turn_dir, turn_val