Pi Pico Asystent do lutowania - ramię robotyczne DIY ze sterowaniem nożnym

[Lukasc]

Asystent do lutowania - ramię robotyczne DIY

 

 

Aby ułatwić sobie lutowanie, zakupiłem gotowe, stalowe  ramię robotyczne z aliexpress. Celem było stworzenie tzw. 3 ręki sterowanej za pomocą nóg (jak to brzmi ) . Jest to pierwszy etap projektu, który miał pokazać jaka jest praktyczność tego typu rozwiązania. 

Główne komponenty:

• Raspberry Pi Pico W (serce projektu)
• Serwomechanizmy typu DS3218MG (silne i precyzyjne serwa do manipulacji)
• Serwomechanizm obrotowy 360 stopni np PowerHD LF-MG-360
• Wyświetlacz LCD 4x20 z konwerterem I2C (do monitorowania stanu urządzenia)
• Dwa joysticki analogowe (do ręcznego sterowania ramieniem)
• Moduł pomiarowy napięcia i natężenia prądu (pomiar napięcia baterii oraz prądu pobieranego przez serwa)
• Bateria 18650 (zasilanie)
• Przełącznik do wyłączania zasilania
• Część własnoręcznie zaprojektowanych i wydrukowanych części z PETG
• Dwa nożne pedały

 

Projekt pozwala na dość precyzyjne pozycjonowanie ramienia robotycznego dzięki joystickom. Wyświetlacz LCD informuje użytkownika o aktualnej pozycji każdego serwa oraz kierunku obrotu serwa 360 stopni. Układ serw zapisywany jest w pamięci i ponowne uruchomienie ramienia powoduje nastawienie serw do zapisanych wartości. Napięcie baterii jest stale monitorowane, co pozwala mi określić optymalny moment ładowania baterii, natomiast pomiar prądu pobieranego przez serwa pozwala szybko reagować na ewentualne problemy podczas pracy urządzenia.

Największym rozczarowaniem okazał się dołączony do zestawu chwytak a właściwie jego połączenie z serwem. Najpierw próbowałem połączyć chwytanie z serwem 180 stopni ale odpowiedni ścisk, który stabilnie trzymał lutowany przewód generował wzrost wykorzystywanego prądu. Prąd wzrastał nawet do 2 amper i więcej. Dodatkowo ramię zaczynało zachowywać się niestabilnie. Uchwyt na zmianę popuszczał i zaciskał się losowo. Zamiana na serwo 360 stopni powodowało, że chwytu w ogóle nie było. Podanie stałego sygnału obrotu powodowało taki sam brak stabilności jak w przypadku poprzedniego serwomechanizmu. Zrezygnowałem więc z tej części i dodałem ręczny chwytak drukując do niego część specjalnie z wyprofilowanymi prowadnicami pod przewody. 

Aby ograniczyć ilość punktów obrotu i ustabilizować ruch zrezygnowałem także z innych części elementów robota minimalizując całą konstrukcję. Zaprojektowałem skrócone ramię, którym zastąpiłem dłuższe stalowe ramie, które nie było mi potrzebne. Dodatkowo, stworzyłem specjalną obudowę dla dolnego elementu obrotowego, zapewniając stabilność konstrukcji. Dwie płaszczyzny obudowy pokryte smarem umożliwiają płynny i stabilny obrót ramienia, eliminując obciążenie serwa przez dolną część ramienia, co znacząco redukuje drgania i odciąża główne serwo.

Całość przymocowana została do zwykłej deski. Przewody starałem się poprowadzić pod spodem. 

Aktualnie nie będąc do końca zadowolonym z precyzji przymierzam się do przejścia do 2 fazy projektu – zalutowanie wszystkiego i rozważenie dodania silników krokowych. Szukam także rozwiązania, które zamiast deski-podstawki będzie demontowanym uchwytem do biurka. 

Klasa do sterowania serwami Ramie_robota_klasy.py:

from machine import Pin, PWM, I2C, ADC
import ustruct
import _thread

from time import sleep_ms, ticks_ms, ticks_diff
# import ssd1306
from pico_i2c_lcd import I2cLcd
import utime

# Funkcja mapująca zakres wartości
def map_range(value, in_min, in_max, out_min, out_max):
    return (value - in_min) * (out_max - out_min) // (in_max - in_min) + out_min

# Klasa dla serwa DS3218MG (0-180 stopni)
class DS3218MG:
    def __init__(self, pin, alias, name, reset_angle=90, min_angle=0, max_angle=180, reverse=False):
        self.servo = PWM(Pin(pin, mode=Pin.OUT))
        self.servo.freq(50)
        self.reset_angle = reset_angle
        self.min_angle = min_angle
        self.max_angle = max_angle
        self.angle = reset_angle
        self.alias = alias
        self.name = name
        self.reverse = reverse  # Dodana flaga odwrócenia osi

    def set_angle(self, angle):
        if angle < self.min_angle or angle > self.max_angle:
            raise ValueError("Angle must be between 0 and 180 degrees")
        min_duty = 1638
        max_duty = 8192
        duty = int(min_duty + (max_duty - min_duty) * (angle / 180.0))
        self.servo.duty_u16(duty)
        self.angle = angle
        #print(f"Set angle to: {angle}°")

    def move_by_step(self, step):
        if self.reverse:
            step = -step
        new_angle = self.angle + step
        if new_angle < self.min_angle:
            new_angle = self.min_angle
        elif new_angle > self.max_angle:
            new_angle = self.max_angle
        self.set_angle(new_angle)

    def reset_position(self):
        self.set_angle(self.reset_angle)

# Klasa dla serwa MG996R 360 stopni
class MG996R:
    def __init__(self, pin, alias, name, reverse=False):
        self.servo = PWM(Pin(pin, mode=Pin.OUT))
        self.servo.freq(50)
        self.moving = False
        self.start_time = None
        self.alias = alias
        self.name = name
        self.is_gripping = False
        self.reverse = reverse  # Dodana flaga odwrócenia osi
        self.set_speed(0)  # Ustawienie serwa w neutralnej pozycji

    def set_speed(self, speed):
        if self.reverse:
            speed = -speed
        if speed < -100 or speed > 100:
            raise ValueError("Speed must be between -100 and 100")
        mid_duty = 4915
        if speed == 0:
            duty = mid_duty
        elif speed > 0:
            duty = int(mid_duty + (mid_duty * (speed / 100.0)))
        else:
            duty = int(mid_duty - (mid_duty * (abs(speed) / 100.0)))
        self.servo.duty_u16(duty)

    def start(self, speed):
        if not self.moving:
            self.set_speed(speed)
            self.moving = True
            self.start_time = ticks_ms()
            direction = 'Right' if speed > 0 else 'Left'
            #print(f"Started: Direction {direction}, Speed {speed}%")

    def stop(self):
        if self.moving:
            self.set_speed(0)
            self.moving = False
            print("Stopped")

    def grip(self, speed):
        #if not self.is_gripping:
            #self.start(speed)
            #utime.sleep(0.5)  
        self.set_speed(speed)  
        self.is_gripping = True
        print('Gripped')

    def release(self):
        self.start(-100)
        utime.sleep(0.5)  
        self.stop()
        print('GRIP')
        self.is_gripping = False
        print('Puszczenie chwytaka')

    def monitor_current(self):
        if self.moving:
            current_time = ticks_ms()
            elapsed_time = ticks_diff(current_time, self.start_time)
            if elapsed_time > 1000:
                self.stop()
                print("Stopped due to resistance")

# Klasa do obsługi enkodera obrotowego z przyciskiem
class RotaryEncoder:
    def __init__(self, clk_pin, dt_pin, button_pin, settings, min_value=0, max_value=100):
        self.pin_clk = Pin(clk_pin, Pin.IN, Pin.PULL_UP)
        self.pin_dt = Pin(dt_pin, Pin.IN, Pin.PULL_UP)
        self.pin_button = Pin(button_pin, Pin.IN, Pin.PULL_UP)
        self.min_value = min_value
        self.max_value = max_value
        self.position = 0
        self.last_clk = self.pin_clk.value()
        self.last_button = self.pin_button.value()
        self.last_time = ticks_ms()
        self.settings = settings

        self.pin_clk.irq(trigger=Pin.IRQ_RISING | Pin.IRQ_FALLING, handler=self.handle_clk)
        self.pin_button.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=self.handle_button)

    def handle_clk(self, pin):
        settings = self.settings
        current_clk = self.pin_clk.value()
        current_dt = self.pin_dt.value()

        if current_clk != self.last_clk:
            if current_clk == current_dt:
                if settings.encoder.special_mode:
                    settings.encoder.selected_servo = (settings.encoder.selected_servo % 6) + 1
                else:
                    self.position += 1
            else:
                if settings.encoder.special_mode:
                    settings.encoder.selected_servo = (settings.encoder.selected_servo - 2) % 6 + 1
                else:
                    self.position -= 1
            # Ogranicz pozycję do zdefiniowanego zakresu
            if self.position < self.min_value:
                self.position = self.min_value
            elif self.position > self.max_value:
                self.position = self.max_value
            settings.encoder.menu_nr = self.position  # Aktualizacja zmiennej globalnej
        self.last_clk = current_clk

    def handle_button(self, pin):
        settings = self.settings
        current_time = ticks_ms()
        
        if ticks_diff(current_time, self.last_time) > 200:  # Debounce
            # Sprawdzenie czy przycisk jest wciśnięty
            if pin.value() == 0:
                settings.encoder.last_button_press_time = current_time  # Zapisujemy czas wciśnięcia przycisku
                print("Button pressed")

            # Jeżeli przycisk został zwolniony
            if pin.value() == 1:
                button_press_duration = ticks_diff(current_time, settings.encoder.last_button_press_time)
                print('Czas przytrzymania: ', button_press_duration)
                
                # Jeżeli czas wciśnięcia był dłuższy niż 3 sekundy, uruchamiamy tryb specjalny
                if button_press_duration > 3000:
                    self.handle_long_button_press()
                else:
                    # Jeżeli czas wciśnięcia był krótszy, to traktujemy to jako zwykłe kliknięcie
                    if settings.encoder.special_mode:
                        settings.encoder.special_mode = False
                    else:
                        settings.encoder.menu_ok = 1

            self.last_time = current_time

    def handle_long_button_press(self):
        settings = self.settings
        print("Long button press detected, entering special mode")
        settings.encoder.special_mode = True



# Klasa do obsługi joysticka
class JoystickHandler:
    def __init__(self, vrx_pin, vry_pin, sw_pin, servo1, servo2, servo3, servo4, servo5, servo6, foot_pin1, foot_pin2, settings, buzzer_pin):
        self.vrx = ADC(Pin(vrx_pin))
        self.vry = ADC(Pin(vry_pin))
        self.sw = Pin(sw_pin, Pin.IN, Pin.PULL_UP)
        self.buzzer = Pin(buzzer_pin, Pin.OUT)
        self.servo1 = servo1
        self.servo2 = servo2
        self.servo3 = servo3
        self.servo4 = servo4
        self.servo5 = servo5
        self.servo6 = servo6

        self.servo_x = servo1
        self.servo_y = servo2
        self.dead_zone = 2000
        self.last_button_state = 1
        self.hysteresis = 500  # Wartość histerezy dla odczytów joysticka

        self.last_x = self.vrx.read_u16()
        self.last_y = self.vry.read_u16()

        # Inicjalizacja footswitchy
        self.footswitch1 = Pin(foot_pin1, Pin.IN, Pin.PULL_UP)
        self.footswitch2 = Pin(foot_pin2, Pin.IN, Pin.PULL_UP)
        self.last_footswitch1_state = 1
        self.last_footswitch2_state = 1
        self.settings = settings

    def read_joystick(self):
        x = self.vrx.read_u16()
        y = self.vry.read_u16()
        button = not self.sw.value()

        if abs(x - self.last_x) < self.hysteresis:
            x = self.last_x
        if abs(y - self.last_y) < self.hysteresis:
            y = self.last_y

        self.last_x = x
        self.last_y = y

        return x, y, button

    def update(self):
        settings = self.settings
        _, _, button = self.read_joystick()
        move_delay = 20
        # Zmień tryb po naciśnięciu przycisku
        if button and self.last_button_state == 0:
            self.settings.encoder.mode = (self.settings.encoder.mode % self.settings.encoder.mode_max) + 1
            self.settings.servo.servo_mode = self.settings.encoder.mode
            print(f"Current mode: {self.settings.encoder.mode}")
            self.buzzer.value(1)
            sleep_ms(10)
            self.buzzer.value(0)
        if self.settings.encoder.mode == 1:
            self.servo_x = self.servo1
            self.servo_y = self.servo2
        elif self.settings.encoder.mode == 2:
            self.servo_x = self.servo3
            self.servo_y = self.servo4
        elif self.settings.encoder.mode == 3:
            self.servo_x = self.servo5
            self.servo_y = self.servo6

        self.last_button_state = button
        self.settings.servo.servo_x_alias = self.servo_x.alias
        self.settings.servo.servo_y_alias = self.servo_y.alias
        
        # Stan footswitchy
        footswitch1_state = not self.footswitch1.value()
        footswitch2_state = not self.footswitch2.value()

        if footswitch1_state:
            print('Foot lewa')
            if self.settings.encoder.selected_servo == 1:
                self.servo1.move_by_step(1)
                sleep_ms(move_delay)
                self.settings.servo.s1_value = self.servo1.angle
            elif self.settings.encoder.selected_servo == 2:
                self.servo2.move_by_step(1)
                self.settings.servo.s2_value = self.servo2.angle
                sleep_ms(move_delay)
            elif self.settings.encoder.selected_servo == 3:
                self.servo3.move_by_step(1)
                self.settings.servo.s3_value = self.servo3.angle
                sleep_ms(move_delay)
            elif self.settings.encoder.selected_servo == 4:
                self.servo4.move_by_step(1)
                self.settings.servo.s4_value = self.servo4.angle
                sleep_ms(move_delay)
            elif self.settings.encoder.selected_servo == 5:
                self.servo5.start(14)
            elif self.settings.encoder.selected_servo == 6:
                self.servo6.start(14)
        if footswitch2_state:
            if self.settings.encoder.selected_servo == 1:
                self.servo1.move_by_step(-1)
                self.settings.servo.s1_value = self.servo1.angle
                sleep_ms(move_delay)
            elif self.settings.encoder.selected_servo == 2:
                self.servo2.move_by_step(-1)
                self.settings.servo.s2_value = self.servo2.angle
                sleep_ms(move_delay)
            elif self.settings.encoder.selected_servo == 3:
                self.servo3.move_by_step(-1)
                self.settings.servo.s3_value = self.servo3.angle
                sleep_ms(move_delay)
            elif self.settings.encoder.selected_servo == 4:
                self.servo4.move_by_step(-1)
                self.settings.servo.s4_value = self.servo4.angle
                sleep_ms(move_delay)
            elif self.settings.encoder.selected_servo == 5:
                self.servo5.start(-14)
            elif self.settings.encoder.selected_servo == 6:
                self.servo6.start(-14)
            
            print('Foot prawa')

        if not footswitch1_state and self.last_footswitch1_state == 1:
            if self.settings.encoder.selected_servo == 5:
                self.servo5.stop()
            elif self.settings.encoder.selected_servo == 6:
                self.servo6.stop()
        if not footswitch2_state and self.last_footswitch2_state == 1:
            if self.settings.encoder.selected_servo == 5:
                self.servo5.stop()
            elif self.settings.encoder.selected_servo == 6:
                self.servo6.stop()

        self.last_footswitch1_state = footswitch1_state
        self.last_footswitch2_state = footswitch2_state

    def control_servos(self):
        settings = self.settings

        x, y, button = self.read_joystick()
        updated = False
        if settings.encoder.mode < 4:
            if isinstance(self.servo_x, MG996R):
                if abs(x - 32768) > self.dead_zone:
                    updated = True
                    speed = map_range(abs(x - 32768), 0, 32768, 14, 30)
                    if x < (32768 - self.dead_zone):
                        self.servo_x.start(-speed)
                        self.settings.servo.change = f'Left {speed}'
                    elif x > (32768 + self.dead_zone):
                        self.servo_x.start(speed)
                        self.settings.servo.change = f'Right {speed}'
                    self.settings.servo.current_servo = self.servo_x.name
                else:
                    if self.servo_x == settings.servo.s6_360 and not settings.servo.grip_in_use:
                        self.servo_x.stop()
                        print('war1')
                    elif self.servo_x != settings.servo.s6_360:
                        self.servo_x.stop()
                        print('war2')
            elif isinstance(self.servo_x, DS3218MG):
                if abs(x - 32768) > self.dead_zone:
                    updated = True
                    delay = map_range(abs(x - 32768), 0, 32768, settings.servo.delay_max, settings.servo.delay_min)
                    if x < (32768 - self.dead_zone):
                        self.servo_x.move_by_step(-1)
                        self.settings.servo.change = f'{self.servo_x.angle}°'
                    elif x > (32768 + self.dead_zone):
                        self.servo_x.move_by_step(1)
                        self.settings.servo.change = f'{self.servo_x.angle}°'
                    self.settings.servo.current_servo = self.servo_x.name
                    sleep_ms(delay)
            if isinstance(self.servo_y, MG996R):
                if abs(y - 32768) > self.dead_zone:
                    updated = True
                    speed = map_range(abs(y - 32768), 0, 32768, 14, 50) # 2 ostatnie wartosci to min i max sila. 14 to minimul jakie porusza serwo
                    if y < (32768 - self.dead_zone):
                        self.servo_y.start(-speed)
                        self.settings.servo.change = f'Left {speed}'
                    elif y > (32768 + self.dead_zone):
                        self.servo_y.start(speed)
                        self.settings.servo.change = f'Right {speed}'
                    self.settings.servo.current_servo = f"{self.servo_y.name}"
                else:
                    if self.servo_y == settings.servo.s6_360 and not settings.servo.grip_in_use:
                        self.servo_y.stop()
                    elif self.servo_y != settings.servo.s6_360:
                        self.servo_y.stop()
            elif isinstance(self.servo_y, DS3218MG):
                if abs(y - 32768) > self.dead_zone:
                    updated = True
                    delay = map_range(abs(y - 32768), 0, 32768, settings.servo.delay_max, settings.servo.delay_min)
                    if y < (32768 - self.dead_zone):
                        self.servo_y.move_by_step(-1)
                        self.settings.servo.change = f' {self.servo_y.angle}°'
                    elif y > (32768 + self.dead_zone):
                        self.servo_y.move_by_step(1)
                        self.settings.servo.change = f' {self.servo_y.angle}°'
                    self.settings.servo.current_servo = f"{self.servo_y.name}"
                    sleep_ms(delay)

            if updated:
                self.settings.servo.servo_x_alias = self.servo_x.alias
                self.settings.servo.servo_y_alias = self.servo_y.alias
                self.settings.servo.s1_value = self.servo1.angle
                self.settings.servo.s2_value = self.servo2.angle
                self.settings.servo.s3_value = self.servo3.angle
                self.settings.servo.s4_value = self.servo4.angle
                self.settings.servo.s5_value = self.servo5.servo.duty_u16()  # Zakładając, że przechowuje prędkość
                self.settings.servo.s6_value = self.servo6.servo.duty_u16()  # Zakładając, że przechowuje prędkość
        elif settings.encoder.mode == 4:
            if abs(x - 32768) > self.dead_zone:
                if x < (32768 - self.dead_zone):
                    if settings.encoder.menu_nr < 2:
                        settings.encoder.menu_nr += 1
                        sleep_ms(300)
                        print('x')
                elif x > (32768 + self.dead_zone):
                    if settings.encoder.menu_nr >= 1:
                        settings.encoder.menu_nr -= 1
                        sleep_ms(300)
                    
            if abs(y - 32768) > self.dead_zone:
                if y < (32768 - self.dead_zone):
                    if settings.encoder.menu_nr < 2:
                        settings.encoder.menu_nr += 1
                        sleep_ms(300)
                elif y > (32768 + self.dead_zone):
                    if settings.encoder.menu_nr >= 1:
                        settings.encoder.menu_nr -= 1
                        sleep_ms(300)

                    
# Klasa do obsługi nowego joysticka
class CommonJoystickHandler:
    def __init__(self, vrx_pin, sw_pin, servo, settings):
        self.vrx = ADC(Pin(vrx_pin))
        self.sw = Pin(sw_pin, Pin.IN, Pin.PULL_UP)
        self.servo = servo
        self.dead_zone = 2000
        self.last_button_state = 1
        self.settings = settings

    def read_joystick(self):
        x = self.vrx.read_u16()
        button = not self.sw.value()
        return x, button

    def update(self):

        x, button = self.read_joystick()

        if abs(x - 32768) > self.dead_zone:
            speed = map_range(abs(x - 32768), 0, 32768, 14, self.settings.servo.grip_power)
            if x < (32768 - self.dead_zone):
                self.servo.start(-speed)
            elif x > (32768 + self.dead_zone):
                self.servo.start(speed)
        else:
            if self.settings.servo.grip == 0 and not self.settings.servo.grip_in_use:
                self.servo.stop()

        if button and self.last_button_state == 0:
            if not self.servo.is_gripping:
                self.servo.grip(self.settings.servo.grip_power) #mocno zaciska
                self.settings.servo.grip = 1
                self.settings.servo.grip_in_use = True
                #grip_in_use_led.value(1)
                print('Zacisk')
            else:
                self.servo.release()
                self.settings.servo.grip = 0
                self.settings.servo.grip_in_use = False
                #grip_in_use_led.value(0)
                print('Puszczenie')

        self.last_button_state = button
        
class JoystickHandlerExt:
    def __init__(self, servo_x, servo_y, settings):
        self.servo_x = servo_x
        self.servo_y = servo_y
        self.settings = settings
        self.velocity_180 = 600 #czas w ms, ktory bedzie dzielony przez wartosc joysticka - decyduje o predkosci ruchu
        self.velocity_360 = 15 # maksymalna predkosc obrotu serwa 360 (minimum to 14 przy ktorym serwo w ogole sie porusza)
        

    def update(self):
        settings = self.settings

        x = settings.external.joystick3_x
        y = settings.external.joystick3_y
        button = settings.external.joystick3_btn

        # Obsługa osi X
        if isinstance(self.servo_x, MG996R):
            if x != 0:
                speed = map_range(abs(x), 0, 10, 14, self.velocity_360)
                if x < 0:
                    self.servo_x.start(-speed)
                    settings.servo.change = f'Left {speed}'
                else:
                    self.servo_x.start(speed)
                    settings.servo.change = f'Right {speed}'
                settings.servo.current_servo = self.servo_x.name
            else:
                self.servo_x.stop()
        elif isinstance(self.servo_x, DS3218MG):
            if x != 0:
                step = 1 if x > 0 else -1
                self.servo_x.move_by_step(step)
                settings.servo.change = f'{self.servo_x.angle}°'
                settings.servo.current_servo = self.servo_x.name
                sleep_ms(self.velocity_180 // abs(x))  # Delay proportional to speed

        # Obsługa osi Y
        if isinstance(self.servo_y, MG996R):
            if y != 0:
                speed = map_range(abs(y), 0, 10, 14, self.velocity_360)
                if y < 0:
                    self.servo_y.start(-speed)
                    settings.servo.change = f'Left {speed}'
                else:
                    self.servo_y.start(speed)
                    settings.servo.change = f'Right {speed}'
                settings.servo.current_servo = f"{self.servo_y.name}"
            else:
                self.servo_y.stop()
        elif isinstance(self.servo_y, DS3218MG):
            if y != 0:
                step = 1 if y > 0 else -1
                self.servo_y.move_by_step(step)
                settings.servo.change = f' {self.servo_y.angle}°'
                settings.servo.current_servo = f"{self.servo_y.name}"
                sleep_ms(self.velocity_180 // abs(y))  # Delay proportional to speed

        # Obsługa przycisku joysticka
        if button:
            print('przycisk')

        # Aktualizacja aliasów serw i wartości w settings
        settings.servo.servo_x_alias = self.servo_x.alias
        settings.servo.servo_y_alias = self.servo_y.alias
        settings.servo.s1_value = self.settings.servo.s1_value
        settings.servo.s2_value = self.settings.servo.s2_value
        settings.servo.s3_value = self.settings.servo.s3_value
        settings.servo.s4_value = self.settings.servo.s4_value
        settings.servo.s5_value = self.settings.servo.s5_value # Zakładając, że przechowuje prędkość
        settings.servo.s6_value = self.settings.servo.s6_value # Zakładając, że przechowuje prędkość

 

Klasa funkcjonalna Ramie_robota_funkcje.py:

# robot_functions.py
from time import sleep_ms, ticks_ms, ticks_diff
def smooth_move_servo(servo, start_angle, end_angle, step=1, delay=10):
    if start_angle < end_angle:
        for angle in range(start_angle, end_angle + 1, step):
            servo.set_angle(angle)
            sleep_ms(delay)
    else:
        for angle in range(start_angle, end_angle - 1, -step):
            servo.set_angle(angle)
            sleep_ms(delay)

# Funkcja do obsługi kliknięcia przycisku i zmiany pozycji serwomechanizmów
def handle_menu_click(settings, s1_180, s2_180, s3_180, s4_180):
    menu_nr = settings.encoder.menu_nr
    s1_value = settings.servo.s1_value
    s2_value = settings.servo.s2_value
    s3_value = settings.servo.s3_value
    s4_value = settings.servo.s4_value
    
    if menu_nr == 0:  # Normal mode
        target_angles = [90, 90, 90, 40]
        smooth_move_servo(s1_180, s1_value, target_angles[0])
        smooth_move_servo(s2_180, s2_value, target_angles[1])
        smooth_move_servo(s3_180, s3_value, target_angles[2])
        smooth_move_servo(s4_180, s4_value, target_angles[3])
    elif menu_nr == 1:  # Low mode
        target_angles = [90, 0, 35, 145]
        smooth_move_servo(s3_180, s3_value, target_angles[2])
        smooth_move_servo(s4_180, s4_value, target_angles[3])
        smooth_move_servo(s1_180, s1_value, target_angles[0])
        smooth_move_servo(s2_180, s2_value, target_angles[1])
    elif menu_nr == 2:  # UpDown
        target_angles = [90, 0, 0, 180]
        print('1 ruch - s2_180 wyrownanie')
        smooth_move_servo(s2_180, s2_value, 76)
        
        s2_value = 76
        print('2 ruch - s3_180')
        smooth_move_servo(s3_180, s3_value, target_angles[2])
        print('3 ruch - s4_180')
        smooth_move_servo(s4_180, s4_value, target_angles[3])
        print('4 ruch - s1_180')
        smooth_move_servo(s1_180, s1_value, target_angles[0])
        print('4 ruch - s2_180')
        smooth_move_servo(s2_180, s2_value, target_angles[1])
    else:
        return
    
    # Update global values to new angles
    settings.servo.s1_value, settings.servo.s2_value, settings.servo.s3_value, settings.servo.s4_value = target_angles

 

settings.py

class DisplaySettings:
    #Zmienne globalne
    def __init__(self):
        self.last_lcd_update = 0
        self.lcd_update_interval = 400  # Czas odświeżania LCD w ms
        self.lcd_update_flag = False
        self.lcd_update_data = None

class ServoSettings:
    def __init__(self):
        self.s1_value = 0
        self.s2_value = 0
        self.s3_value = 0
        self.s4_value = 0
        self.s5_value = 0
        self.s6_value = 0
        self.current_servo = ""
        self.change = ""
        self.servo_x_alias = ""
        self.servo_y_alias = ""
        self.grip_in_use = False
        self.grip = 0
        self.grip_power = 25
        self.delay_min = 10
        self.delay_max = 200

class EncoderSettings:
    def __init__(self):
        self.menu_nr = 0
        self.menu_ok = 0
        self.selected_servo = 5 # wybor serwa pod sterowanie wlacznikami noznymi 1-6
        self.mode_max = 4
        self.mode = 1
        self.servo_mode = 1
        self.special_mode = 1
        self.last_button_press_time = 0
        self.last_activity_time = 0
        
        
class External_devices:
    def __init__(self):
        self.joystick3_x = 0
        self.joystick3_y = 0
        self.joystick3_btn = 0
        
        

class SensorData:
    def __init__(self):
        self.data = []


class Settings:
    def __init__(self):
        self.display = DisplaySettings()
        self.servo = ServoSettings()
        self.encoder = EncoderSettings()
        self.external = External_devices()
        self.sensor = SensorData()
        

 

Edytowano Marzec 24, 2025 przez Lukasc
dodany kod

[aimeiz]

Fajny projekt.

Będziesz też publikował kod źródłowy?

Co do chwytania i ściskania, to z doświadczeń dociskania pióra w plotterze, sprawdza się dociskanie przez sprężyną. Chwytak będzie chwytał na początku w miarę lekko, a docisk będzie się zwiększał poprzez dalszą zmianę pozycji przez serwo czy silnik krokowy.

[Lukasc]

6 godzin temu, aimeiz napisał:

Fajny projekt.

Będziesz też publikował kod źródłowy?

Co do chwytania i ściskania, to z doświadczeń dociskania pióra w plotterze, sprawdza się dociskanie przez sprężyną. Chwytak będzie chwytał na początku w miarę lekko, a docisk będzie się zwiększał poprzez dalszą zmianę pozycji przez serwo czy silnik krokowy.

dzięki. Spróbuję zaprojektować coś pod sprężyny. Co do kodu - mam mnóstwo wersji oprogramowania. Eksperymentowałem z C++, Pythonem z thread, bez thread, z klasami i enkoderem obrotywym itd. Obecnie używam uproszczonej wersji tymczasowej. Ale moje klasy udostępnię. Można ich użyć aby sterować serwami 180 i 360. 

[ethanak]

Jestem ciekaw czy coś takiego pomogłoby osobie niepełnosprawnej (sprawna tylko jedna ręka).

[Lukasc]

21 godzin temu, ethanak napisał:

Jestem ciekaw czy coś takiego pomogłoby osobie niepełnosprawnej (sprawna tylko jedna ręka).

moim zdaniem to ciekawa koncepcja. Zawsze można ewentualnie dodać nawet trzeci pedał nożny (ja kupiłem na allegro bo na Botland nie było) - jednym przełączać serwa, a dwoma nimi sterować. Można dodać nawet drugie ramię. Wtedy wszystko jest bez użycia rąk. Tylko serwa jak się okazało nie są super precyzyjne.