pi_pico_w micropython(thonny)_4servoRobot

#PROGRAM BY: RANDY N. CAÑADA
#SUBSCRIBE: FB PAGES:
#Playlist.ph:
#https://www.facebook.com/profile.php?id=61573177858722

#Arduino:
#https://www.facebook.com/rhacan2?__tn__=R

#YOUTUBE:
#PlaylistPH
#https://www.youtube.com/@PlaylistPh-h2v

#ElectroCrafters:
#https://www.youtube.com/channel/UCu1aqzUdTqSnTN5BHNlbVgA



import network
import socket
import time
from machine import Pin, PWM

#ENTER YOUR WIFI CREDENTIALS
SSID = "YOUR WIFI"
PASSWORD = "YOUR PASSWORD"

PORT = 1234

PIN_JAW    = 16
PIN_ARM_L  = 17 #fwd_rev
PIN_ARM_R  = 18 #up_dwn
PIN_BASE   = 19 #bottom

def attach_servo(pin_num):
    pwm = PWM(Pin(pin_num))
    pwm.freq(50)
    return pwm

def angle_to_duty(angle):
    pulse_us = 500 + (angle / 180) * 1900
    return int((pulse_us / 20000) * 65535)

def smooth_move(servo, current_angle, target_angle, step=1, delay_ms=10):
    if servo is None:
        return target_angle
    direction = 1 if target_angle > current_angle else -1
    for angle in range(current_angle, target_angle + direction, direction * step):
        servo.duty_u16(angle_to_duty(angle))
        time.sleep_ms(delay_ms)
    return target_angle

servoJaw = attach_servo(PIN_JAW)
servoBase = attach_servo(PIN_BASE)
servoArmL = None
servoArmR = None
activeArmServo = None

angleJaw  = 80
angleArmL = 45
angleArmR = 90
angleBase = 90

servoJaw.duty_u16(angle_to_duty(angleJaw))
servoBase.duty_u16(angle_to_duty(angleBase))

last_update = {
    "Jaw": time.ticks_ms(),
    "ArmL": time.ticks_ms(),
    "ArmR": time.ticks_ms(),
    "Base": time.ticks_ms()
}
TIMEOUT_MS = 5000

def connect_wifi():
    wlan = network.WLAN(network.STA_IF)
    wlan.active(True)
    wlan.connect(SSID, PASSWORD)
    print("📡 Connecting to Wi-Fi...")
    while not wlan.isconnected():
        time.sleep(1)
        print(".", end="")
    ip = wlan.ifconfig()[0]
    print(f"\n✅ Connected! IP: {ip}")
    return ip

ip_address = connect_wifi()

server = socket.socket()
server.bind(('', PORT))
server.listen(1)
print(f"🧠 Listening on {ip_address}:{PORT}")

while True:
    client, addr = server.accept()
    ip_address = network.WLAN(network.STA_IF).ifconfig()[0]  # Refresh IP
    print(f"🎮 Controller connected from {addr}")
    print(f"🌐 Current IP: {ip_address}")

    try:
        while True:
            data = client.recv(1024)
            if not data:
                break

            input_str = data.decode().strip()
            messages = input_str.split('\n')

            for msg in messages:
                if msg.startswith("AXIS:"):
                    parts = msg.split(":")
                    if len(parts) != 3:
                        print(f"⚠️ Malformed input: {msg}")
                        continue

                    axis = parts[1]
                    try:
                        val = int(parts[2])
                    except ValueError:
                        print(f"⚠️ Invalid value: {parts[2]}")
                        continue

                    val = max(min(val, 32767), -32767)

                    if axis == "ABS_RX":
                        newJaw = int(((val + 32767) / 65534) * 20 + 70)
                        if newJaw != angleJaw:
                            if servoJaw is None:
                                servoJaw = attach_servo(PIN_JAW)
                                print("🔌 Jaw reattached")
                            angleJaw = smooth_move(servoJaw, angleJaw, newJaw)
                            last_update["Jaw"] = time.ticks_ms()
                            print(f"🦷 Jaw → {angleJaw}°")

                    elif axis == "ABS_X":
                        newArmL = int(((val + 32767) / 65534) * 90)
                        if newArmL != angleArmL:
                            if activeArmServo == "R" and servoArmR:
                                servoArmR.duty_u16(0)
                                servoArmR.deinit()
                                pin = Pin(PIN_ARM_R, Pin.OUT)
                                pin.value(0)
                                servoArmR = None
                                print("🛑 Left Arm deinitialized")

                            if servoArmL is None:
                                servoArmL = attach_servo(PIN_ARM_L)
                                print("🔌 Right Arm reattached")

                            angleArmL = smooth_move(servoArmL, angleArmL, newArmL)
                            activeArmServo = "L"
                            last_update["ArmL"] = time.ticks_ms()
                            print(f"🦿 Right Arm → {angleArmL}°")

                    elif axis == "ABS_Y":
                        newArmR = int(180 - ((val + 32767) / 65534) * 180)
                        if newArmR != angleArmR:
                            if activeArmServo == "L" and servoArmL:
                                servoArmL.duty_u16(0)
                                servoArmL.deinit()
                                pin = Pin(PIN_ARM_L, Pin.OUT)
                                pin.value(0)
                                servoArmL = None
                                print("🛑 Right Arm deinitialized")

                            if servoArmR is None:
                                servoArmR = attach_servo(PIN_ARM_R)
                                print("🔌 Left Arm reattached")

                            angleArmR = smooth_move(servoArmR, angleArmR, newArmR)
                            activeArmServo = "R"
                            last_update["ArmR"] = time.ticks_ms()
                            print(f"🦾 Left Arm → {angleArmR}°")

                    elif axis == "ABS_RY":
                        newBase = int(((val + 32767) / 65534) * 180)
                        if newBase != angleBase:
                            if servoBase is None:
                                servoBase = attach_servo(PIN_BASE)
                                print("🔌 Base reattached")
                            angleBase = smooth_move(servoBase, angleBase, newBase)
                            last_update["Base"] = time.ticks_ms()
                            print(f"🔄 Base → {angleBase}°")

            now = time.ticks_ms()

            if time.ticks_diff(now, last_update["Jaw"]) > TIMEOUT_MS and servoJaw:
                servoJaw.duty_u16(0)
                servoJaw.deinit()
                pin = Pin(PIN_JAW, Pin.OUT)
                pin.value(0)
                servoJaw = None
                print("🛑 Jaw servo deinitialized due to inactivity")

            if time.ticks_diff(now, last_update["ArmL"]) > TIMEOUT_MS and servoArmL:
                servoArmL.duty_u16(0)
                servoArmL.deinit()
                pin = Pin(PIN_ARM_L, Pin.OUT)
                pin.value(0)
                servoArmL = None
                print("🛑 Right Arm servo deinitialized due to inactivity")

            if time.ticks_diff(now, last_update["ArmR"]) > TIMEOUT_MS and servoArmR:
                servoArmR.duty_u16(0)
                servoArmR.deinit()
                pin = Pin(PIN_ARM_R, Pin.OUT)
                pin.value(0)
                servoArmR = None
                print("🛑 Left Arm servo deinitialized due to inactivity")

            if time.ticks_diff(now, last_update["Base"]) > TIMEOUT_MS and servoBase:
                servoBase.duty_u16(0)
                servoBase.deinit()
                pin = Pin(PIN_BASE, Pin.OUT)
                pin.value(0)
                servoBase = None
                print("🛑 Base servo deinitialized due to inactivity")

    except Exception as e:
        print(f"⚠️ Error: {e}")
    finally:
        client.close()
        print("⚡ Controller disconnected")