102 lines
2.7 KiB
Plaintext
102 lines
2.7 KiB
Plaintext
import time
|
|
from machine import Pin, ADC, PWM
|
|
|
|
light_sensor_1 = ADC(26)
|
|
light_sensor_2 = ADC(27)
|
|
servo_pwm = PWM(Pin(28, Pin.OUT))
|
|
servo_pwm.freq(50)
|
|
cur_deg = 90
|
|
reset_button = Pin(14, Pin.IN)
|
|
calibrate_button = Pin(15, Pin.IN)
|
|
led = Pin(25, Pin.OUT)
|
|
led.high()
|
|
run_avg = [0, 0, 0, 0, 0]
|
|
|
|
light_1_offset = 0
|
|
light_2_offset = 0
|
|
|
|
|
|
def servo(degrees):
|
|
if degrees > 180:
|
|
degrees = 180
|
|
if degrees < 0:
|
|
degrees = 0
|
|
max_duty = 2500000
|
|
min_duty = 500000
|
|
new_duty = min_duty + (max_duty - min_duty) * (degrees / 180)
|
|
servo_pwm.duty_ns(int(new_duty))
|
|
|
|
|
|
def shift_array(arr, num_to_add):
|
|
arr.pop(0)
|
|
arr.append(num_to_add)
|
|
return arr
|
|
|
|
|
|
def move_to_light():
|
|
print("Moving to light...")
|
|
global cur_deg
|
|
is_facing_light = False
|
|
while not is_facing_light:
|
|
offset = light_sensor_1.read_u16() - light_sensor_2.read_u16()
|
|
if offset > 75 and cur_deg > 0:
|
|
cur_deg = cur_deg - 1
|
|
servo(cur_deg)
|
|
elif offset < -75 and cur_deg < 180:
|
|
cur_deg = cur_deg + 1
|
|
servo(cur_deg)
|
|
else:
|
|
is_facing_light = True
|
|
time.sleep(.01)
|
|
print("Moved to light!")
|
|
|
|
|
|
servo(90)
|
|
while True:
|
|
if reset_button.value() == 1:
|
|
led.low()
|
|
cur_deg = 90
|
|
servo(90)
|
|
cal_threshold = 0
|
|
run_avg = [0, 0, 0, 0, 0]
|
|
while reset_button.value() == 1:
|
|
time.sleep(.1)
|
|
led.high()
|
|
print("Reset")
|
|
continue
|
|
if calibrate_button.value() == 1:
|
|
sens_1 = light_sensor_1.read_u16()
|
|
sens_2 = light_sensor_2.read_u16()
|
|
if sens_1 > sens_2:
|
|
light_1_offset = sens_1 - sens_2
|
|
elif sens_1 < sens_2:
|
|
light_2_offset = sens_2 - sens_1
|
|
while calibrate_button.value() == 1:
|
|
time.sleep(.1)
|
|
diff = light_sensor_1.read_u16() - light_sensor_2.read_u16() - (light_1_offset - light_2_offset)
|
|
run_avg = shift_array(run_avg, diff)
|
|
num_over = 0
|
|
first_iter = True
|
|
sign = True
|
|
for cur_num in run_avg:
|
|
if abs(cur_num) > 90:
|
|
if first_iter:
|
|
print("First iter")
|
|
if cur_num >= 0:
|
|
sign = True
|
|
else:
|
|
sign = False
|
|
first_iter = False
|
|
if (cur_num >= 0 and not sign) or (cur_num < 0 and sign):
|
|
print("Reset")
|
|
num_over = 0
|
|
first_iter = True
|
|
break
|
|
elif (cur_num >= 0 and sign) or (cur_num < 0 and not sign):
|
|
print("+1")
|
|
num_over += 1
|
|
if num_over == 5:
|
|
print("Try move")
|
|
move_to_light()
|
|
run_avg = [0, 0, 0, 0, 0]
|