from machine import PWM, Pin
from time import sleep
from servo import Servo
from encoder import RotaryEncoder

servo1 = Servo(12)
servo2 = Servo(14)

# Define the sensor pin
sensor_pin = Pin(34, Pin.IN)

def rotation_handler(direction):
    if direction == RotaryEncoder.ROT_CW:
        servo1.rotate(9)
        servo2.rotate(-9)
    else:
        servo1.rotate(-9)
        servo2.rotate(9)

myEncoder = RotaryEncoder(26, 27, rotation_handler)

def main():
    # Read the sensor value
    sensor_value = sensor_pin.value()
    if sensor_value == 1:  # Sensor is pressed
        rotation_handler(RotaryEncoder.ROT_CW)
        sleep(1)
        rotation_handler(RotaryEncoder.ROT_CCW)
    sleep(0.1)

print("Hello, ESP32!")
if __name__ == '__main__':
    while True:
        main()

        

main.py

diagram.json

from machine import PWM, Pin
from time import sleep

class Servo:
    def __init__(self, servo_pin):
        self.__pwm = PWM(Pin(servo_pin, Pin.OUT), freq = 50)

        self.current_angle = 0

        self.min_angle = 0
        self.max_angle = 180
        self.min_time = 0.5
        self.max_time = 2.5
        self.motor_speed = 60 / 0.1

    def move(self, angle):
        if not self.min_angle <= angle <= self.max_angle:
            return
        slope = (self.max_time - self.min_time) / (self.max_angle - self.min_angle)
        time_per_angle = (angle * slope) + self.min_time
        duty_cycle = time_per_angle / 20
        pwm_value = duty_cycle * (2**10 - 1)
        self.__pwm.duty(int(pwm_value))

        delta_angle = abs(angle - self.current_angle)
        time_delay = delta_angle / self.motor_speed
        time_delay = max(time_delay, 0.02)
        
        self.current_angle = angle
        sleep(time_delay)

    def rotate(self, angle):
        target_angle = (self.current_angle + angle) % (self.max_angle + 1)
        self.move(target_angle)


servo.py

from machine import Pin

class RotaryEncoder:

    ROT_CW = 0
    ROT_CCW = 1
    
    def rotary_change(self, pin):
        if self.clk.value() == 0:
            if self.dt.value() == 1:
                self.handler_func(self.ROT_CW)
            else:
                self.handler_func(self.ROT_CCW)

    def __init__(self, clk, dt, handler_func):
        self.clk = Pin(clk, Pin.IN, Pin.PULL_UP)
        self.dt = Pin(dt, Pin.IN, Pin.PULL_UP)
        self.handler_func = handler_func

        self.clk.irq(handler=self.rotary_change, trigger=Pin.IRQ_FALLING)
