RoboCarFIXME.py

#***********************************************
#  ロボットカーの制御プログラム
#  MIT APP Inventorで作成したタブレットから制御
#  超音波距離センサで対象物回避
#***********************************************
from machine import PWM, Pin
import utime
import network
import socket
import re
import _thread

# Wi-Fi設定データ定義
ssid = 'YOUR SSID'
password = 'YOUR PASSWORD'

# 動作パラメータ
DIR_FORWARD = 0
DIR_BACKWARD = 1
DIR_STOP = 3

MIN_DISTANCE_CM = 15.0
DISTANCE_LOOP_SEC = 0.2
SPEED_PARSE_PATTERNS = (
    r"^/speed\?value=(\d{1,5})$",
    r"^/speed/(\d{1,5})$",
    r"^/speed=(\d{1,5})$",
    r"^/speed(\d{1,5})$",
)

# 各インスタンス生成
MA2PWM = PWM(Pin(16), freq=10000)
MA1PWM = PWM(Pin(17), freq=10000)
MB2PWM = PWM(Pin(18), freq=10000)
MB1PWM = PWM(Pin(19), freq=10000)

# 距離測定用ピン設定
Trigger = Pin(14, Pin.OUT)
Echo = Pin(15, Pin.IN)

# ロック作成
lock = _thread.allocate_lock()

# グローバル変数定義
Direction = DIR_STOP
Duty1 = 0
Duty2 = 0
MotionEnabled = False
CurrentState = "BOOT"


def log(level, message):
    t = utime.ticks_ms()
    print("[{0:>10}][{1}] {2}".format(t, level, message))


def set_state(new_state):
    global CurrentState
    if CurrentState != new_state:
        log("STATE", "{0} -> {1}".format(CurrentState, new_state))
        CurrentState = new_state


def clamp_duty(value):
    if value < 0:
        return 0
    if value > 65535:
        return 65535
    return value


#**** モータ制御関数 ******
def Backward(duty1, duty2):  # 後進
    MA2PWM.duty_u16(0)
    MA1PWM.duty_u16(duty1)
    MB2PWM.duty_u16(0)
    MB1PWM.duty_u16(duty2)


def Forward(duty1, duty2):  # 前進
    MA1PWM.duty_u16(0)
    MA2PWM.duty_u16(duty1)
    MB1PWM.duty_u16(0)
    MB2PWM.duty_u16(duty2)


def Brake():  # 停止
    MA1PWM.duty_u16(0)
    MA2PWM.duty_u16(0)
    MB1PWM.duty_u16(0)
    MB2PWM.duty_u16(0)


def apply_speed_locked():
    if Direction == DIR_FORWARD:
        Forward(Duty1, Duty2)
        log("MOTOR", "FORWARD L={0} R={1}".format(Duty1, Duty2))
    elif Direction == DIR_BACKWARD:
        Backward(Duty1, Duty2)
        log("MOTOR", "BACKWARD L={0} R={1}".format(Duty1, Duty2))
    else:
        Brake()
        log("MOTOR", "STOP")


def stop_locked(reason):
    global MotionEnabled, Direction
    MotionEnabled = False
    Direction = DIR_STOP
    Brake()
    log("SAFE", "stop reason={0}".format(reason))


def measure_distance_cm(timeout_us=30000):
    # Echoが変化しないケースで無限ループしないようにタイムアウトを入れる
    Trigger.low()
    utime.sleep_us(2)
    Trigger.high()
    utime.sleep_us(10)
    Trigger.low()

    start_wait = utime.ticks_us()
    while Echo.value() == 0:
        if utime.ticks_diff(utime.ticks_us(), start_wait) > timeout_us:
            return None

    pulse_start = utime.ticks_us()
    while Echo.value() == 1:
        if utime.ticks_diff(utime.ticks_us(), pulse_start) > timeout_us:
            return None

    pulse_end = utime.ticks_us()
    pulse_width = utime.ticks_diff(pulse_end, pulse_start)
    return (pulse_width * 0.0343) / 2


def parse_path(request_bytes):
    # 1行目: GET /path HTTP/1.1
    request_text = request_bytes.decode('utf-8', 'ignore')
    first_line = request_text.split('\r\n', 1)[0]
    parts = first_line.split(' ')
    if len(parts) < 2:
        return None
    return parts[1]


def extract_speed_duty(path):
    for pattern in SPEED_PARSE_PATTERNS:
        m = re.match(pattern, path)
        if m:
            return clamp_duty(int(m.group(1)))
    return None


#******* 距離測定のCore1側スレッド ********
def mes_dist():
    global Direction, Duty1, Duty2, MotionEnabled

    log("THREAD", "distance thread started")
    while True:
        try:
            distance = measure_distance_cm()
            if distance is None:
                log("WARN", "distance timeout")
                utime.sleep(DISTANCE_LOOP_SEC)
                continue

            # 前進中のみ障害物回避を有効化
            should_avoid = False
            with lock:
                if MotionEnabled and Direction == DIR_FORWARD and (Duty1 > 0 or Duty2 > 0):
                    if distance < MIN_DISTANCE_CM:
                        should_avoid = True
                        log("SENSOR", "distance={0:.1f}cm < {1}cm".format(distance, MIN_DISTANCE_CM))

            if should_avoid:
                with lock:
                    set_state("AVOID")
                    Brake()
                    utime.sleep(0.5)
                    Direction = DIR_BACKWARD
                    Backward(50000, 0)  # 右旋回
                    log("MOTOR", "avoid turn-right")
                utime.sleep(1.2)
                with lock:
                    Direction = DIR_FORWARD
                    if MotionEnabled:
                        Forward(Duty1, Duty2)
                        log("MOTOR", "resume L={0} R={1}".format(Duty1, Duty2))
                    else:
                        Brake()
                    set_state("RUN")

            utime.sleep(DISTANCE_LOOP_SEC)
        except Exception as e:
            log("ERROR", "mes_dist exception: {0}".format(e))
            utime.sleep(0.5)


#******** Core0側のメインスレッド **********
def server():
    global Direction, Duty1, Duty2, MotionEnabled

    #**** ソケットの設定とサーバ動作開始 IPv4 TCP/IP
    addr = socket.getaddrinfo('0.0.0.0', 80)[0][-1]
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    s.bind(addr)
    s.listen(5)
    s.settimeout(2)

    set_state("LISTEN")
    log("NET", "listening on {0}".format(addr))

    while True:
        conn = None
        client_ip = "unknown"
        try:
            conn, caddr = s.accept()
            client_ip = caddr[0]
            log("NET", "connected {0}".format(client_ip))

            # 不要端末からのアクセス抑制
            if client_ip == '192.168.11.1':
                log("WARN", "blocked client {0}".format(client_ip))
                conn.close()
                continue

            request = conn.recv(1024)
            if not request:
                log("WARN", "empty request")
                conn.close()
                continue

            path = parse_path(request)
            log("CMD", "path={0}".format(path))

            with lock:
                if path == '/forward':
                    Direction = DIR_FORWARD
                    MotionEnabled = True
                    set_state("RUN")
                    apply_speed_locked()
                elif path == '/backward':
                    Direction = DIR_BACKWARD
                    MotionEnabled = True
                    set_state("RUN")
                    apply_speed_locked()
                elif path == '/turnleft':
                    # 進行方向に応じて旋回
                    MotionEnabled = True
                    set_state("RUN")
                    if Direction == DIR_BACKWARD:
                        Backward(60000, 50000)
                        log("MOTOR", "turnleft backward")
                    else:
                        Direction = DIR_FORWARD
                        Forward(60000, 50000)
                        log("MOTOR", "turnleft forward")
                elif path == '/turnright':
                    MotionEnabled = True
                    set_state("RUN")
                    if Direction == DIR_BACKWARD:
                        Backward(50000, 60000)
                        log("MOTOR", "turnright backward")
                    else:
                        Direction = DIR_FORWARD
                        Forward(50000, 60000)
                        log("MOTOR", "turnright forward")
                elif path == '/stop':
                    stop_locked("remote stop")
                    set_state("STOPPED")
                elif path is not None and path.startswith('/speed'):
                    duty = extract_speed_duty(path)
                    if duty is not None:
                        Duty1 = duty
                        Duty2 = duty
                        log("SPEED", "set duty={0}".format(duty))
                        if MotionEnabled and Direction in (DIR_FORWARD, DIR_BACKWARD):
                            apply_speed_locked()
                    else:
                        log("WARN", "invalid speed path: {0}".format(path))
                else:
                    log("WARN", "unknown path: {0}".format(path))

            res = "HTTP/1.1 200 OK\r\nContent-Type: text/plain\r\n\r\nOK"
            conn.send(res)

        except OSError:
            # accept timeout
            pass
        except Exception as e:
            log("ERROR", "server exception({0}): {1}".format(client_ip, e))
            if conn is not None:
                try:
                    conn.send("HTTP/1.1 500 Internal Server Error\r\nContent-Type: text/plain\r\n\r\nERROR")
                except Exception:
                    pass
        finally:
            if conn is not None:
                try:
                    conn.close()
                except Exception:
                    pass


#**** アクセスポイントと接続、サーバ動作開始
try:
    set_state("WIFI_CONNECT")
    log("BOOT", "motor safe stop on startup")
    Brake()

    wlan = network.WLAN(network.STA_IF)
    wlan.active(True)
    wlan.connect(ssid, password)

    wait_count = 0
    while not wlan.isconnected():
        wait_count += 1
        if wait_count % 20 == 0:
            log("WIFI", "connecting...")
        utime.sleep(0.2)

    status = wlan.ifconfig()
    log("WIFI", "connected IP={0}".format(status[0]))

    set_state("THREAD_START")
    _thread.start_new_thread(mes_dist, ())

    set_state("SERVER_START")
    server()
except Exception as e:
    log("FATAL", "startup exception: {0}".format(e))
    with lock:
        stop_locked("fatal error")