update set_led function

hal.py

@@ -0,0 +1,136 @@
+from utime import sleep_us
+from machine import Pin, PWM, ADC, time_pulse_us
+
+#LEDs
+ENEMY = 0
+STATUS = 1
+LEFT_LINE = 2
+RIGHT_LINE = 3
+
+# directions
+STOP = 0
+LEFT = 1
+RIGHT = 2
+FORWARD = 3
+BACKWARD = 4
+
+# Battery resistor ladder ratio
+BATTERY_COEFF = 2.25
+
+# Ultrasonic sensor calibration
+ULTRASONIC_OFFSET = 800
+
+# Servo timing
+MOTOR_LEFT_TUNING = 33
+MOTOR_RIGHT_TUNING = 33
+
+# Ultrasonic distance sensor
+echo = Pin(14, Pin.IN)
+trigger = Pin(27, Pin.OUT)
+
+# Motor PWM-s
+pwm_left = PWM(Pin(15), freq=50, duty=0)
+pwm_right = PWM(Pin(4), freq=50, duty=0)
+
+# bottom LED
+bottom_led = Pin(5, Pin.OUT)
+# bottom LED is in reverse polarity
+bottom_led.value(1)
+# sensor LEDs
+enemy_led = Pin(16, Pin.OUT)
+left_line_led = Pin(17, Pin.OUT)
+right_line_led = Pin(12, Pin.OUT)
+
+# Battery gauge
+adc_battery = ADC(Pin(32))
+
+# Optek sensors
+adc_line_left = ADC(Pin(34))
+adc_line_right = ADC(Pin(33))
+
+# Set reference voltage to 3.3V
+adc_battery.atten(ADC.ATTN_11DB)
+adc_line_left.atten(ADC.ATTN_11DB)
+adc_line_right.atten(ADC.ATTN_11DB)
+
+# Calibrate line sensors
+LINE_LEFT_THRESHOLD = adc_line_left.read()
+LINE_RIGHT_THRESHOLD = adc_line_right.read()
+
+def set_led(led, state):
+    if led == STATUS:
+        bottom_led.value(0 if state else 1)
+    elif led == ENEMY:
+        enemy_led.value(state)
+    elif led == LEFT_LINE:
+        left_line_led.value(state)
+    elif led == RIGHT_LINE:
+        right_line_led.value(state)
+
+def get_battery_voltage():
+    return round(BATTERY_COEFF * (adc_battery.read() * 3.3 / 4096), 2)
+
+enemy_score = 0
+def enemy_distance():
+    global enemy_score
+
+    trigger.value(0)
+    sleep_us(5)
+    trigger.value(1)
+    sleep_us(10)
+    trigger.value(0)
+    # wait for the pulse and calculate the distance
+    enemy_distance = (time_pulse_us(echo, 1, 30000) / 2) / 29.1
+
+    if enemy_distance < 60 and enemy_distance > 0:
+        if enemy_score < 5:
+            enemy_score += 1
+    else:
+        if enemy_score > 0:
+            enemy_score -= 1
+
+    # indicate enemy LED
+    set_led(ENEMY, True if enemy_score > 2 else False)
+
+    return True if enemy_score > 2 else False
+
+def line_left():
+    line = abs(adc_line_left.read() - LINE_LEFT_THRESHOLD) > 1000
+    set_led(LEFT_LINE, line)
+    return line
+
+def line_right():
+    line = abs(adc_line_right.read() - LINE_RIGHT_THRESHOLD) > 1000
+    set_led(RIGHT_LINE, line)
+    return line
+
+def detach_servos():
+    motor_left(0)
+    motor_right(0)
+
+prev_left_speed = 0
+def motor_left(speed):
+    global prev_left_speed
+    if speed == prev_left_speed:
+        return
+    prev_left_speed = speed
+    assert speed >= -100
+    assert speed <= 100
+    pwm_left.duty(int(33 + MOTOR_LEFT_TUNING + speed * 33 / 100)) # -100 ... 100 to 33 .. 102
+    if speed == 0:
+        pwm_left.duty(0)
+
+prev_right_speed = 0
+def motor_right(speed):
+    global prev_right_speed
+    if speed == prev_right_speed:
+        return
+    prev_right_speed = speed
+    assert speed >= -100
+    assert speed <= 100
+    pwm_right.duty(int(33 + MOTOR_RIGHT_TUNING + speed * 33 / 100)) # -100 ... 100 to 33 .. 102
+    if speed == 0:
+        pwm_right.duty(0)
+
+print("Battery voltage: %.2fV" % get_battery_voltage())
+print("Line sensor thresholds:", LINE_LEFT_THRESHOLD, LINE_RIGHT_THRESHOLD)