import time
import init as pyRTOS
from machine import Pin, PWM, ADC

MIN = 1638
MAX = 8192
resolution = (MAX - MIN) / (2**16 - 1)

pwm_out = 0

def POTTask(self):
    POT = ADC(Pin(26))

    yield
    while True:
        time.sleep_ms(10)

        global pwm_out
        pwm_out = int(POT.read_u16() * resolution) + MIN

        yield [pyRTOS.timeout_ns(50000000)]  # Sleep for 50 ms

def ServoTask(self):
    Servo = PWM(Pin(20), freq=50)

    yield
    while True:
        time.sleep_ms(10)
            
        global pwm_out
        Servo.duty_u16(pwm_out)

        yield [pyRTOS.timeout_ns(20000000)]  # Sleep for 20 ms

# Define tasks
def task1(self):
    led1 = Pin(2, Pin.OUT)
    led1.off()
    yield
    while True:
        time.sleep_ms(10)
        led1.toggle()
        print("Task 1 is running")
        yield [pyRTOS.timeout_ns(1000000000)]  # Sleep for 1 second

def task2(self):
    led2 = Pin(7, Pin.OUT)
    led2.off()
    yield
    while True:
        time.sleep_ms(10)
        led2.toggle()
        print("Task 2 is running")
        yield [pyRTOS.timeout_ns(3000000000)]  # Sleep for 1.5 seconds

# Create tasks
pyRTOS.add_task(pyRTOS.Task(task1,name='task1'))
pyRTOS.add_task(pyRTOS.Task(task2,name='task2'))
pyRTOS.add_task(pyRTOS.Task(POTTask,name='POTTask'))
pyRTOS.add_task(pyRTOS.Task(ServoTask,name='ServoTask'))

# Start the scheduler
pyRTOS.start()

while True:
    time.sleep_ms(10)
    pass