armando car with backoff

with_backoff.py

track = track4()
car = newCar(track)
startDisplay(track, car)

def control(sd, pastsd):
    # average these values to be more smooth
    leftt = sd[0] + pastsd[0];
    mid = sd[1] + pastsd[1];
    # this bit of assymetry is actually very clever, because 
    # "intuitively" assymetrical things has more information on it
    # and we want to have as much information as we can with only 3 sensors
    rightt = sd[2] + pastsd[2] + 5;
    
    # car is "safe" if 
    # 1. clear straight ahead (sum 50)
    # 2. left sensor == right sensor
    safe_straight = mid / 50;
    safe_turn = Math.min(leftt / rightt, rightt / leftt);
    # both above number range from 0 to 1, 1 being most safe
    safe = safe_straight * safe_turn;
    
    # cruising =====
    left_diff = mid-leftt;
    right_diff = mid-rightt;
    speed = 40 * safe;
    ang_diff = left_diff - right_diff;
    ang = 1.8*ang_diff;
    
    # hysteresis
    # cap min speed
    speed = Math.max(speed, 16)
    ang = Math.min(Math.max(ang, -35),35)

    # emergency jiggle
    sensor_total = leftt + mid + rightt;
    if (sensor_total < 50):
        print("performing backoff")
        speed = -20;
        ang = 99999 * (leftt - rightt);
        
    return [ang, speed]

print("Ready to simulate.")



simout = simulate(car, track, 750, control, true)
print("Done!")

displayResult(track, simout)