import machine
import time

POT_PIN = 34
sens = 0.80 #deacceleration detection sensivitity
NUM_READINGS = 100
SERVO1_PIN = 14  # Adjust to your servo 1 pin
SERVO2_PIN = 15  # Adjust to your servo 2 pin

potentiometer = machine.ADC(machine.Pin(POT_PIN))
readings = [0] * NUM_READINGS
index = 0

servo1 = machine.PWM(machine.Pin(SERVO1_PIN), freq=50)
servo2 = machine.PWM(machine.Pin(SERVO2_PIN), freq=50)

def map_range(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

def set_servo_angle(servo, angle):
    duty = int(map_range(angle, 0, 180, 2000, 9000))  # Adjust duty cycle range
    servo.duty_u16(duty)

while True:
    pot_value = potentiometer.read()
    readings[index] = pot_value

    average_value = sum(readings) / NUM_READINGS
    voltage = (average_value / 4095) * 5.00
    print(pot_value, average_value)

    if pot_value <= (average_value*sens):
        readings = [pot_value] * NUM_READINGS


    # Map pot values and averages to servo angles (0-180 degrees)
    servo1_angle = int(map_range(pot_value, 0, 4095, 0, 180))
    servo2_angle = int(map_range(average_value, 0, 4095, 0, 180))

    # Set servo angles
    set_servo_angle(servo1, servo1_angle)
    set_servo_angle(servo2, servo2_angle)

    index = (index + 1) % NUM_READINGS
    time.sleep_ms(50)