diff --git a/Micropython/MONA_puppet/ads7830.py b/Micropython/MONA_puppet/ads7830.py new file mode 100644 index 0000000..779455a --- /dev/null +++ b/Micropython/MONA_puppet/ads7830.py @@ -0,0 +1,178 @@ +""" +ADS7830 8-bit ADC Driver for MicroPython +Based on the Arduino library from ControlEverything.com +Ported for MONA ESP robot +""" + +from machine import I2C +import time + +# I2C Addresses +ADS7830_DEFAULT_ADDRESS = 0x48 # ADDR = GND +ADS7830_VDD_ADDRESS = 0x49 # ADDR = VDD +ADS7830_SDA_ADDRESS = 0x4A # ADDR = SDA +ADS7830_SCL_ADDRESS = 0x4B # ADDR = SCL + +# Conversion delay in milliseconds +ADS7830_CONVERSIONDELAY = 5 + +# Command byte register bits +# Single-Ended/Differential Inputs +ADS7830_REG_COMMAND_SD_DIFF = 0x00 # Differential Inputs +ADS7830_REG_COMMAND_SD_SINGLE = 0x80 # Single-Ended Inputs + +# Channel selection for single-ended mode (according to datasheet Table 2) +ADS7830_REG_COMMAND_CH_SINGLE_0 = 0x00 +ADS7830_REG_COMMAND_CH_SINGLE_2 = 0x10 +ADS7830_REG_COMMAND_CH_SINGLE_4 = 0x20 +ADS7830_REG_COMMAND_CH_SINGLE_6 = 0x30 +ADS7830_REG_COMMAND_CH_SINGLE_1 = 0x40 +ADS7830_REG_COMMAND_CH_SINGLE_3 = 0x50 +ADS7830_REG_COMMAND_CH_SINGLE_5 = 0x60 +ADS7830_REG_COMMAND_CH_SINGLE_7 = 0x70 + +# Channel selection for differential mode +ADS7830_REG_COMMAND_CH_DIFF_0_1 = 0x00 # P = CH0, N = CH1 +ADS7830_REG_COMMAND_CH_DIFF_2_3 = 0x10 # P = CH2, N = CH3 +ADS7830_REG_COMMAND_CH_DIFF_4_5 = 0x20 # P = CH4, N = CH5 +ADS7830_REG_COMMAND_CH_DIFF_6_7 = 0x30 # P = CH6, N = CH7 +ADS7830_REG_COMMAND_CH_DIFF_1_0 = 0x40 # P = CH1, N = CH0 +ADS7830_REG_COMMAND_CH_DIFF_3_2 = 0x50 # P = CH3, N = CH2 +ADS7830_REG_COMMAND_CH_DIFF_5_4 = 0x60 # P = CH5, N = CH4 +ADS7830_REG_COMMAND_CH_DIFF_7_6 = 0x70 # P = CH7, N = CH6 + +# Power-Down Selection +ADS7830_REG_COMMAND_PD_PDADCONV = 0x00 # Power Down Between Conversions +ADS7830_REG_COMMAND_PD_IROFF_ADON = 0x04 # Internal Ref OFF, ADC ON +ADS7830_REG_COMMAND_PD_IRON_ADOFF = 0x08 # Internal Ref ON, ADC OFF +ADS7830_REG_COMMAND_PD_IRON_ADON = 0x0C # Internal Ref ON, ADC ON + +# SD Mode enum-like constants +SDMODE_DIFF = ADS7830_REG_COMMAND_SD_DIFF +SDMODE_SINGLE = ADS7830_REG_COMMAND_SD_SINGLE + +# PD Mode enum-like constants +PDADCONV = ADS7830_REG_COMMAND_PD_PDADCONV +PDIROFF_ADON = ADS7830_REG_COMMAND_PD_IROFF_ADON +PDIRON_ADOFF = ADS7830_REG_COMMAND_PD_IRON_ADOFF +PDIRON_ADON = ADS7830_REG_COMMAND_PD_IRON_ADON + + +class ADS7830: + """ADS7830 8-bit ADC driver class""" + + # Single-ended channel mapping + _SINGLE_CHANNEL_MAP = { + 0: ADS7830_REG_COMMAND_CH_SINGLE_0, + 1: ADS7830_REG_COMMAND_CH_SINGLE_1, + 2: ADS7830_REG_COMMAND_CH_SINGLE_2, + 3: ADS7830_REG_COMMAND_CH_SINGLE_3, + 4: ADS7830_REG_COMMAND_CH_SINGLE_4, + 5: ADS7830_REG_COMMAND_CH_SINGLE_5, + 6: ADS7830_REG_COMMAND_CH_SINGLE_6, + 7: ADS7830_REG_COMMAND_CH_SINGLE_7, + } + + # Differential channel mapping (using channel pair as key) + _DIFF_CHANNEL_MAP = { + (0, 1): ADS7830_REG_COMMAND_CH_DIFF_0_1, + (1, 0): ADS7830_REG_COMMAND_CH_DIFF_1_0, + (2, 3): ADS7830_REG_COMMAND_CH_DIFF_2_3, + (3, 2): ADS7830_REG_COMMAND_CH_DIFF_3_2, + (4, 5): ADS7830_REG_COMMAND_CH_DIFF_4_5, + (5, 4): ADS7830_REG_COMMAND_CH_DIFF_5_4, + (6, 7): ADS7830_REG_COMMAND_CH_DIFF_6_7, + (7, 6): ADS7830_REG_COMMAND_CH_DIFF_7_6, + } + + def __init__(self, i2c: I2C, address: int = ADS7830_DEFAULT_ADDRESS): + """ + Initialize ADS7830 ADC + + Args: + i2c: I2C bus object + address: I2C address of the device + """ + self._i2c = i2c + self._address = address + self._conversion_delay = ADS7830_CONVERSIONDELAY + self._sd_mode = SDMODE_SINGLE + self._pd_mode = PDIROFF_ADON + + @property + def sd_mode(self) -> int: + """Get Single-Ended/Differential mode""" + return self._sd_mode + + @sd_mode.setter + def sd_mode(self, mode: int): + """Set Single-Ended/Differential mode""" + self._sd_mode = mode + + @property + def pd_mode(self) -> int: + """Get Power-Down mode""" + return self._pd_mode + + @pd_mode.setter + def pd_mode(self, mode: int): + """Set Power-Down mode""" + self._pd_mode = mode + + def _write_command(self, cmd: int): + """Write command byte to ADC""" + self._i2c.writeto(self._address, bytes([cmd])) + + def _read_result(self) -> int: + """Read conversion result from ADC""" + data = self._i2c.readfrom(self._address, 1) + return data[0] + + def measure_single_ended(self, channel: int) -> int: + """ + Read single-ended ADC value from specified channel + + Args: + channel: Channel number (0-7) + + Returns: + 8-bit unsigned ADC value (0-255) + """ + if channel < 0 or channel > 7: + return 0 + + # Build config byte + config = self._sd_mode | self._pd_mode | self._SINGLE_CHANNEL_MAP[channel] + + # Write config and read result + self._write_command(config) + time.sleep_ms(self._conversion_delay) + return self._read_result() + + def measure_differential(self, positive_ch: int, negative_ch: int) -> int: + """ + Read differential ADC value + + Args: + positive_ch: Positive input channel + negative_ch: Negative input channel + + Returns: + 8-bit signed ADC value + """ + channel_pair = (positive_ch, negative_ch) + if channel_pair not in self._DIFF_CHANNEL_MAP: + return 0 + + # Build config byte + config = self._sd_mode | self._pd_mode | self._DIFF_CHANNEL_MAP[channel_pair] + + # Write config and read result + self._write_command(config) + time.sleep_ms(self._conversion_delay) + + raw_adc = self._read_result() + # Convert to signed value + if raw_adc > 127: + return raw_adc - 256 + return raw_adc diff --git a/Micropython/MONA_puppet/main.py b/Micropython/MONA_puppet/main.py new file mode 100644 index 0000000..1f5a747 --- /dev/null +++ b/Micropython/MONA_puppet/main.py @@ -0,0 +1,2 @@ +from mona_udp_controller import main +main() diff --git a/Micropython/MONA_puppet/mcp23008.py b/Micropython/MONA_puppet/mcp23008.py new file mode 100644 index 0000000..496544a --- /dev/null +++ b/Micropython/MONA_puppet/mcp23008.py @@ -0,0 +1,133 @@ +""" +MCP23008 I2C GPIO Expander Driver for MicroPython +Simplified implementation for MONA ESP robot +""" + +from machine import I2C + +# Default I2C address +MCP23008_DEFAULT_ADDRESS = 0x20 + +# Register addresses +MCP23008_IODIR = 0x00 # I/O Direction Register +MCP23008_IPOL = 0x01 # Input Polarity Register +MCP23008_GPINTEN = 0x02 # Interrupt-on-Change Control Register +MCP23008_DEFVAL = 0x03 # Default Value Register +MCP23008_INTCON = 0x04 # Interrupt Control Register +MCP23008_IOCON = 0x05 # Configuration Register +MCP23008_GPPU = 0x06 # Pull-Up Resistor Configuration Register +MCP23008_INTF = 0x07 # Interrupt Flag Register +MCP23008_INTCAP = 0x08 # Interrupt Capture Register +MCP23008_GPIO = 0x09 # GPIO Port Register +MCP23008_OLAT = 0x0A # Output Latch Register + +# Pin modes +INPUT = 1 +OUTPUT = 0 + +# Pin states +HIGH = 1 +LOW = 0 + + +class MCP23008: + """MCP23008 8-bit I/O Expander driver class""" + + def __init__(self, i2c: I2C, address: int = MCP23008_DEFAULT_ADDRESS): + """ + Initialize MCP23008 + + Args: + i2c: I2C bus object + address: I2C address of the device + """ + self._i2c = i2c + self._address = address + self._iodir = 0xFF # All inputs by default + self._gpio = 0x00 # All low by default + + # Initialize device + self._write_register(MCP23008_IODIR, self._iodir) + self._write_register(MCP23008_GPIO, self._gpio) + + def _write_register(self, reg: int, value: int): + """Write a byte to register""" + self._i2c.writeto(self._address, bytes([reg, value])) + + def _read_register(self, reg: int) -> int: + """Read a byte from register""" + self._i2c.writeto(self._address, bytes([reg])) + data = self._i2c.readfrom(self._address, 1) + return data[0] + + def pin_mode(self, pin: int, mode: int): + """ + Set pin mode (INPUT or OUTPUT) + + Args: + pin: Pin number (0-7) + mode: INPUT (1) or OUTPUT (0) + """ + if pin < 0 or pin > 7: + return + + if mode == INPUT: + self._iodir |= (1 << pin) + else: + self._iodir &= ~(1 << pin) + + self._write_register(MCP23008_IODIR, self._iodir) + + def digital_write(self, pin: int, value: int): + """ + Write to output pin + + Args: + pin: Pin number (0-7) + value: HIGH (1) or LOW (0) + """ + if pin < 0 or pin > 7: + return + + if value: + self._gpio |= (1 << pin) + else: + self._gpio &= ~(1 << pin) + + self._write_register(MCP23008_GPIO, self._gpio) + + def digital_read(self, pin: int) -> int: + """ + Read from input pin + + Args: + pin: Pin number (0-7) + + Returns: + HIGH (1) or LOW (0) + """ + if pin < 0 or pin > 7: + return 0 + + gpio_val = self._read_register(MCP23008_GPIO) + return (gpio_val >> pin) & 0x01 + + def pull_up(self, pin: int, enable: bool): + """ + Enable/disable internal pull-up resistor + + Args: + pin: Pin number (0-7) + enable: True to enable, False to disable + """ + if pin < 0 or pin > 7: + return + + gppu = self._read_register(MCP23008_GPPU) + + if enable: + gppu |= (1 << pin) + else: + gppu &= ~(1 << pin) + + self._write_register(MCP23008_GPPU, gppu) diff --git a/Micropython/MONA_puppet/mona_esp_lib.py b/Micropython/MONA_puppet/mona_esp_lib.py new file mode 100644 index 0000000..784d921 --- /dev/null +++ b/Micropython/MONA_puppet/mona_esp_lib.py @@ -0,0 +1,357 @@ +""" +Mona_ESP_lib.py - MicroPython library for MONA ESP robot +Ported from Arduino C++ library by Bart Garcia +Created for MicroPython on ESP32 +""" + +from machine import Pin, PWM, I2C, ADC +import time + +from ads7830 import ADS7830, SDMODE_SINGLE, PDIROFF_ADON +from mcp23008 import MCP23008, INPUT, OUTPUT, HIGH, LOW +from neopixel_led import NeoPixelLED + +# ============== Pin Definitions ============== + +# Motor Control Pins +MOT_RIGHT_FORWARD = 19 +MOT_RIGHT_BACKWARD = 21 +MOT_LEFT_FORWARD = 4 +MOT_LEFT_BACKWARD = 18 + +# Motor Feedback (Encoder) Pins +MOT_RIGHT_FEEDBACK = 39 +MOT_RIGHT_FEEDBACK_2 = 23 +MOT_LEFT_FEEDBACK = 35 +MOT_LEFT_FEEDBACK_2 = 34 + +# I2C Pins +SDA_PIN = 32 +SCL_PIN = 33 + +# LED Pins +LED_RGB1_PIN = 22 +LED_RGB2_PIN = 15 + +# Battery Voltage Pin +BATT_VOL_PIN = 36 + +# IO Expander Pin Definitions +EXP_0 = 0 +EXP_1 = 1 +EXP_2 = 2 +EXP_3 = 3 +EXP_4 = 4 +EXP_5 = 5 +EXP_6 = 6 +EXP_7 = 7 + +# IR Enable pins (on IO Expander) +IR_ENABLE_1 = EXP_4 +IR_ENABLE_2 = EXP_3 +IR_ENABLE_3 = EXP_2 +IR_ENABLE_4 = EXP_1 +IR_ENABLE_5 = EXP_0 + +# IR Sensor ADC channels +IR1_SENSOR = 7 # Left IR +IR2_SENSOR = 6 # Left diagonal IR +IR3_SENSOR = 5 # Front IR +IR4_SENSOR = 4 # Right diagonal IR +IR5_SENSOR = 0 # Right IR + +# Motor PWM Settings +MOT_FREQ = 5000 +MOT_DUTY_MAX = 255 + +# I2C Device Addresses +IO_EXP_ADDRESS = 0x20 +ADC_ADDRESS = 0x48 + +# ============== Global Objects ============== + +# I2C bus +_i2c = None + +# IO Expander +_io_expander = None + +# ADC +_adc = None + +# RGB LEDs +_rgb1 = None +_rgb2 = None + +# Motor PWM objects +_mot_rf = None # Right forward +_mot_rb = None # Right backward +_mot_lf = None # Left forward +_mot_lb = None # Left backward + +# Battery ADC +_batt_adc = None + + +def mona_esp_init(): + """Initialize the MONA ESP robot hardware""" + global _i2c, _io_expander, _adc, _rgb1, _rgb2 + global _mot_rf, _mot_rb, _mot_lf, _mot_lb, _batt_adc + + # Initialize Motor PWM + _mot_rf = PWM(Pin(MOT_RIGHT_FORWARD), freq=MOT_FREQ, duty=0) + _mot_rb = PWM(Pin(MOT_RIGHT_BACKWARD), freq=MOT_FREQ, duty=0) + _mot_lf = PWM(Pin(MOT_LEFT_FORWARD), freq=MOT_FREQ, duty=0) + _mot_lb = PWM(Pin(MOT_LEFT_BACKWARD), freq=MOT_FREQ, duty=0) + + # Initialize I2C + _i2c = I2C(0, scl=Pin(SCL_PIN), sda=Pin(SDA_PIN), freq=400000) + + # Initialize IO Expander + _io_expander = MCP23008(_i2c, IO_EXP_ADDRESS) + + # Set IO Expander pin modes + _io_expander.pin_mode(IR_ENABLE_5, OUTPUT) + _io_expander.pin_mode(IR_ENABLE_4, OUTPUT) + _io_expander.pin_mode(IR_ENABLE_3, OUTPUT) + _io_expander.pin_mode(IR_ENABLE_2, OUTPUT) + _io_expander.pin_mode(IR_ENABLE_1, OUTPUT) + _io_expander.pin_mode(EXP_5, INPUT) + _io_expander.pin_mode(EXP_6, INPUT) + _io_expander.pin_mode(EXP_7, INPUT) + + # Turn off all IR LEDs + _io_expander.digital_write(IR_ENABLE_5, LOW) + _io_expander.digital_write(IR_ENABLE_4, LOW) + _io_expander.digital_write(IR_ENABLE_3, LOW) + _io_expander.digital_write(IR_ENABLE_2, LOW) + _io_expander.digital_write(IR_ENABLE_1, LOW) + + # Initialize ADC (ADS7830) + _adc = ADS7830(_i2c, ADC_ADDRESS) + _adc.sd_mode = SDMODE_SINGLE + _adc.pd_mode = PDIROFF_ADON + + # Initialize RGB LEDs + _rgb1 = NeoPixelLED(LED_RGB1_PIN, 1) + _rgb2 = NeoPixelLED(LED_RGB2_PIN, 1) + + # Initialize Battery Voltage ADC + _batt_adc = ADC(Pin(BATT_VOL_PIN)) + _batt_adc.atten(ADC.ATTN_0DB) # 0dB attenuation (up to ~800mV) + + print("MONA ESP initialized!") + + +# ============== Motor Control Functions ============== + +def _set_motor_speed(pwm_obj: PWM, speed: int): + """Set motor PWM duty cycle (0-255)""" + speed = min(max(speed, 0), 255) + # Convert 8-bit (0-255) to 10-bit duty cycle (0-1023) + duty = int(speed * 1023 / 255) + pwm_obj.duty(duty) + + +def right_mot_forward(speed: int): + """Move right motor forward""" + _set_motor_speed(_mot_rf, speed) + _set_motor_speed(_mot_rb, 0) + + +def right_mot_backward(speed: int): + """Move right motor backward""" + _set_motor_speed(_mot_rf, 0) + _set_motor_speed(_mot_rb, speed) + + +def right_mot_stop(): + """Stop right motor""" + _set_motor_speed(_mot_rf, 0) + _set_motor_speed(_mot_rb, 0) + + +def left_mot_forward(speed: int): + """Move left motor forward""" + _set_motor_speed(_mot_lf, speed) + _set_motor_speed(_mot_lb, 0) + + +def left_mot_backward(speed: int): + """Move left motor backward""" + _set_motor_speed(_mot_lf, 0) + _set_motor_speed(_mot_lb, speed) + + +def left_mot_stop(): + """Stop left motor""" + _set_motor_speed(_mot_lf, 0) + _set_motor_speed(_mot_lb, 0) + + +def motors_forward(speed: int): + """Move both motors forward""" + right_mot_forward(speed) + left_mot_forward(speed) + + +def motors_backward(speed: int): + """Move both motors backward""" + right_mot_backward(speed) + left_mot_backward(speed) + + +def motors_spin_left(speed: int): + """Spin robot to the left (right forward, left backward)""" + right_mot_forward(speed) + left_mot_backward(speed) + + +def motors_spin_right(speed: int): + """Spin robot to the right (right backward, left forward)""" + right_mot_backward(speed) + left_mot_forward(speed) + + +def motors_stop(): + """Stop both motors""" + right_mot_stop() + left_mot_stop() + + +# ============== IR Sensor Functions ============== + +# Mapping from IR number to enable pin on IO expander +_IR_ENABLE_MAP = { + 1: IR_ENABLE_1, + 2: IR_ENABLE_2, + 3: IR_ENABLE_3, + 4: IR_ENABLE_4, + 5: IR_ENABLE_5, +} + +# Mapping from IR number to ADC channel +_IR_SENSOR_MAP = { + 1: IR1_SENSOR, + 2: IR2_SENSOR, + 3: IR3_SENSOR, + 4: IR4_SENSOR, + 5: IR5_SENSOR, +} + + +def enable_ir(ir_number: int): + """Enable IR LED for specified sensor (1-5)""" + if ir_number in _IR_ENABLE_MAP: + _io_expander.digital_write(_IR_ENABLE_MAP[ir_number], HIGH) + + +def disable_ir(ir_number: int): + """Disable IR LED for specified sensor (1-5)""" + if ir_number in _IR_ENABLE_MAP: + _io_expander.digital_write(_IR_ENABLE_MAP[ir_number], LOW) + + +def read_ir(ir_number: int) -> int: + """ + Read raw ADC value from IR sensor (1-5) + + Returns: + 8-bit ADC value (0-255), or 0 if invalid sensor number + """ + if ir_number in _IR_SENSOR_MAP: + return _adc.measure_single_ended(_IR_SENSOR_MAP[ir_number]) + return 0 + + +def get_ir(ir_number: int) -> int: + """ + Get IR sensor reading (difference between dark and light values) + + This function reads the ambient light, turns on the IR LED, + reads again, then turns off the IR LED and returns the difference. + + Args: + ir_number: IR sensor number (1-5) + + Returns: + Difference value (0-255), or 0 if invalid sensor number + """ + if ir_number not in _IR_SENSOR_MAP: + return 0 + + # Read dark value (ambient) + dark_val = read_ir(ir_number) + + # Enable IR LED + enable_ir(ir_number) + time.sleep_ms(1) # Wait for IR LED to reach full brightness + + # Read light value + light_val = read_ir(ir_number) + + # Disable IR LED + disable_ir(ir_number) + + return abs(dark_val - light_val) + + +def detect_object(ir_number: int, threshold: int) -> bool: + """ + Detect if an object is present near the specified IR sensor + + Args: + ir_number: IR sensor number (1-5) + threshold: Detection threshold value + + Returns: + True if object detected, False otherwise + """ + if ir_number not in _IR_SENSOR_MAP: + return False + + ir_val = get_ir(ir_number) + return ir_val > threshold + + +# ============== Battery Voltage ============== + +def batt_vol() -> int: + """ + Get battery percentage (0-100) + + MONA's battery voltage ranges from 4.2V (full) to 3.3V (minimum working). + On-board resistors convert this to 0.869-0.630V. + Using 0dB attenuation, ADC range is approximately 3550-2750. + + Note: When USB is connected, this reads USB voltage instead. + + Returns: + Battery percentage (0-100) + """ + adc_val = _batt_adc.read() + + # Convert to percentage + # Offset 2750 (3.3V), range 800 (3.3V to 4.2V maps to 2750-3550) + bat_percentage = (adc_val - 2750) // 8 + + # Clamp to 0-100 + return max(0, min(100, bat_percentage)) + + +# ============== LED Control ============== + +def set_led(led_number: int, red: int, green: int, blue: int): + """ + Set RGB LED color + + Args: + led_number: LED number (1 or 2) + red: Red component (0-255) + green: Green component (0-255) + blue: Blue component (0-255) + """ + if led_number == 1: + _rgb1.fill(red, green, blue) + elif led_number == 2: + _rgb2.fill(red, green, blue) diff --git a/Micropython/MONA_puppet/mona_udp_controller.py b/Micropython/MONA_puppet/mona_udp_controller.py new file mode 100644 index 0000000..78e7511 --- /dev/null +++ b/Micropython/MONA_puppet/mona_udp_controller.py @@ -0,0 +1,521 @@ +import network +import socket +import time +from machine import Pin +import _thread + +from mona_esp_lib import ( + mona_esp_init, + motors_forward, + motors_backward, + motors_spin_left, + motors_spin_right, + motors_stop, + left_mot_forward, + right_mot_forward, + get_ir, + set_led, +) + +# ===================== WiFi / UDP 설정 ===================== +SSID = "InMOLab" +PASSWORD = "dlsahfoq104!" +LOCAL_PORT = 8080 + +# ===================== 상태 정의 ===================== +STATE_IDLE = 0 +STATE_TURNING = 1 +STATE_MOVING = 2 +STATE_AVOID = 3 +STATE_ESCAPING = 4 +STATE_EMERGENCY = 5 + +# ===================== 제어 상수 ===================== +PULSES_PER_MM = 18.0 +PULSES_PER_DEGREE = 12.8 +FWD_SPD = 100 +TURN_SPD = 100 + +UPDATE_THRESHOLD_PULSES = 900 +MIN_DIST_MM = 40.0 + +# IR/회피 임계값 +TH = 80 +TH_OUTER = 100 +DELTA = 15 + +# 엔코더 핀 +PIN_ENCODER_LEFT = 35 +PIN_ENCODER_RIGHT = 39 + +# 제어 관련 임계값 +ROTATION_DEADBAND_DEG = 5.0 +MIN_MOTOR_PWM = 60 + +# PI 제어 게인 +K_P = 0.5 +K_I = 0.002 + +# 적분 제한 추가 +INTEGRAL_LIMIT = 500 # 적분 오차 상한 + +# 비상 동작 +EMERGENCY_SPIN_SPD = 200 +ESCAPING_MS = 1000 +EMERGENCY_SPIN_MS = 1200 +BACK_MS = 120 +OSCILLATION_WINDOW_MS = 1200 +OSCILLATION_COUNT_THRESHOLD = 4 + +# ===================== 전역 변수 ===================== +state = STATE_IDLE + +# 엔코더 카운트 +left_encoder_count = 0 +right_encoder_count = 0 +_encoder_lock = _thread.allocate_lock() # 스레드 안전성 + +# IR 센서 값 +ir_values = [0, 0, 0, 0, 0] +_ir_lock = _thread.allocate_lock() + +# 모션 제어 변수 +start_left_count = 0 +start_right_count = 0 +target_turn_pulses = 0 +target_move_pulses = 0 +integral_error = 0.0 +turn_direction = 0 + +# 탈출/비상 타이머 +escaping_until_ms = 0 +emergency_back_until = 0 +emergency_spin_until = 0 + +# 진동 감지 +last_turn_direction = 0 +turn_change_count = 0 +oscillation_timer_start = 0 + +# UDP 소켓 +udp_socket = None + +# 엔코더 핀 객체 +encoder_left_pin = None +encoder_right_pin = None + +# 제어 루프 실행 플래그 +running = True + + +# ===================== 엔코더 인터럽트 (최적화) ===================== +def isr_left_encoder(pin): + global left_encoder_count + left_encoder_count += 1 + + +def isr_right_encoder(pin): + global right_encoder_count + right_encoder_count += 1 + + +def get_encoder_counts(): + """스레드 안전하게 엔코더 값 읽기""" + return left_encoder_count, right_encoder_count + + +# ===================== 유틸리티 함수 ===================== +def clear_motion_targets(): + global target_turn_pulses, target_move_pulses + global start_left_count, start_right_count, integral_error + + target_turn_pulses = 0 + target_move_pulses = 0 + l, r = get_encoder_counts() + start_left_count = l + start_right_count = r + integral_error = 0.0 + + +def enter_escaping(ms=ESCAPING_MS): + global escaping_until_ms, state + escaping_until_ms = time.ticks_ms() + ms + motors_forward(FWD_SPD) + state = STATE_ESCAPING + + +def start_emergency_left_spin(): + global emergency_back_until, emergency_spin_until, state + global turn_change_count, last_turn_direction, oscillation_timer_start + + now = time.ticks_ms() + emergency_back_until = now + BACK_MS + emergency_spin_until = emergency_back_until + EMERGENCY_SPIN_MS + + motors_backward(FWD_SPD) + state = STATE_EMERGENCY + + turn_change_count = 0 + last_turn_direction = -1 + oscillation_timer_start = now + + print("[EMERGENCY] back -> spin_left") + + +def check_oscillation_and_escape(current_direction): + global last_turn_direction, turn_change_count, oscillation_timer_start + + if last_turn_direction != 0 and current_direction != last_turn_direction: + now = time.ticks_ms() + if time.ticks_diff(now, oscillation_timer_start) > OSCILLATION_WINDOW_MS: + turn_change_count = 1 + oscillation_timer_start = now + else: + turn_change_count += 1 + + if turn_change_count >= OSCILLATION_COUNT_THRESHOLD: + start_emergency_left_spin() + return + + last_turn_direction = current_direction + + +def constrain(val, min_val, max_val): + return max(min_val, min(max_val, val)) + + +# ===================== WiFi 연결 ===================== +def connect_wifi(): + wlan = network.WLAN(network.STA_IF) + wlan.active(True) + + if not wlan.isconnected(): + print(f"Connecting to {SSID}...") + wlan.connect(SSID, PASSWORD) + + timeout = 20 + while not wlan.isconnected() and timeout > 0: + time.sleep(0.5) + print(".", end="") + timeout -= 1 + + if wlan.isconnected(): + print(f"\nWiFi connected!") + print(f"IP: {wlan.ifconfig()[0]}") + else: + print("\nWiFi connection failed!") + return False + + return True + + +def setup_udp(): + global udp_socket + udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + udp_socket.bind(('0.0.0.0', LOCAL_PORT)) + udp_socket.setblocking(False) + print(f"UDP listening on port {LOCAL_PORT}") + + +# ===================== 모션 제어 ===================== +def start_motion(angle_deg, dist_mm): + global state, target_turn_pulses, target_move_pulses + global start_left_count, start_right_count, integral_error, turn_direction + + if state in (STATE_AVOID, STATE_ESCAPING, STATE_EMERGENCY): + return + + l, r = get_encoder_counts() + start_left_count = l + start_right_count = r + integral_error = 0.0 + + target_turn_pulses = int(round(abs(angle_deg) * PULSES_PER_DEGREE)) + target_move_pulses = int(round(abs(dist_mm) * PULSES_PER_MM)) + + if target_turn_pulses > 0 and abs(angle_deg) > ROTATION_DEADBAND_DEG: + state = STATE_TURNING + if angle_deg > 0: + motors_spin_right(TURN_SPD) + turn_direction = 1 + else: + motors_spin_left(TURN_SPD) + turn_direction = -1 + elif target_move_pulses > 0: + state = STATE_MOVING + else: + state = STATE_IDLE + motors_stop() + + +# ===================== UDP 패킷 처리 ===================== +def handle_udp_packet(): + global state + + last_packet = None + while True: + try: + data, addr = udp_socket.recvfrom(255) + if data: + last_packet = data + except OSError: + break + + if last_packet is None: + return + + packet = last_packet.decode('utf-8').strip() + print(f"Received: {packet}") + + # STOP 명령 + if packet.upper().startswith("STOP"): + motors_stop() + clear_motion_targets() + state = STATE_IDLE + return + + # G 명령 + if packet.upper().startswith("G"): + try: + parts = packet[1:].strip().split() + if len(parts) >= 2: + angle = float(parts[0]) + dist = float(parts[1]) + + if dist < MIN_DIST_MM: + return + + # 이동 중이면 일정 거리 이상 이동 후에만 새 명령 수락 + if state == STATE_MOVING: + l, r = get_encoder_counts() + l_now = abs(l - start_left_count) + r_now = abs(r - start_right_count) + if ((l_now + r_now) // 2) < UPDATE_THRESHOLD_PULSES: + return + + start_motion(angle, dist) + except ValueError: + print("Invalid G command format") + + +# ===================== IR 센서 백그라운드 읽기 (Core 1) ===================== +def ir_sensor_thread(): + """별도 스레드에서 IR 센서를 읽어 메인 루프 속도 유지""" + global ir_values, running + + while running: + new_values = [0, 0, 0, 0, 0] + for i in range(5): + new_values[i] = get_ir(i + 1) + + with _ir_lock: + for i in range(5): + ir_values[i] = new_values[i] + + time.sleep_ms(20) # IR 읽기 주기 + + +def get_ir_values(): + """스레드 안전하게 IR 값 읽기""" + with _ir_lock: + return ir_values.copy() + + +# ===================== PI 제어 (개선) ===================== +def pi_control(l_now, r_now): + """개선된 PI 제어 - 적분 제한 및 데드밴드 적용""" + global integral_error + + err = l_now - r_now + + # 적분 오차 누적 (제한 적용) + integral_error += err + integral_error = constrain(integral_error, -INTEGRAL_LIMIT, INTEGRAL_LIMIT) + + # 작은 오차는 무시 (데드밴드) + if abs(err) < 5: + err = 0 + + u = (K_P * err) + (K_I * integral_error) + + left_pwm = constrain(int(round(FWD_SPD - u)), MIN_MOTOR_PWM, 255) + right_pwm = constrain(int(round(FWD_SPD + u)), MIN_MOTOR_PWM, 255) + + return left_pwm, right_pwm + + +# ===================== 제어 루프 ===================== +def control_loop(r1, r2, r3, r4, r5): + global state, start_left_count, start_right_count, integral_error + + # 장애물 감지 + obstacle = (r1 > TH_OUTER) or (r2 > TH) or (r3 > TH) or (r4 > TH) or (r5 > TH_OUTER) + + if obstacle: + if state not in (STATE_AVOID, STATE_ESCAPING, STATE_EMERGENCY): + motors_stop() + clear_motion_targets() + state = STATE_AVOID + + # 비상 상태 처리 + if state == STATE_EMERGENCY: + now = time.ticks_ms() + if time.ticks_diff(now, emergency_back_until) < 0: + motors_backward(FWD_SPD) + return + if time.ticks_diff(now, emergency_spin_until) < 0: + motors_spin_left(EMERGENCY_SPIN_SPD) + return + motors_stop() + state = STATE_IDLE + return + + # 엔코더 값 읽기 + l, r = get_encoder_counts() + l_now = abs(l - start_left_count) + r_now = abs(r - start_right_count) + avg = (l_now + r_now) // 2 + + # 상태별 처리 + if state == STATE_TURNING: + if avg >= target_turn_pulses: + motors_stop() + if target_move_pulses > 0: + start_left_count = l + start_right_count = r + integral_error = 0.0 + state = STATE_MOVING + else: + state = STATE_IDLE + + elif state == STATE_MOVING: + if avg >= target_move_pulses: + motors_stop() + state = STATE_IDLE + else: + # 개선된 PI 제어 사용 + left_pwm, right_pwm = pi_control(l_now, r_now) + left_mot_forward(left_pwm) + right_mot_forward(right_pwm) + + elif state == STATE_AVOID: + if (r1 <= TH_OUTER) and (r2 <= TH) and (r3 <= TH) and (r4 <= TH) and (r5 <= TH_OUTER): + enter_escaping(ESCAPING_MS) + return + + if r3 >= TH: + if abs(r2 - r4) <= DELTA or r2 < r4: + motors_spin_left(TURN_SPD) + check_oscillation_and_escape(-1) + else: + motors_spin_right(TURN_SPD) + check_oscillation_and_escape(1) + elif r1 >= TH_OUTER or r2 >= TH: + motors_spin_right(TURN_SPD) + check_oscillation_and_escape(1) + elif r4 >= TH or r5 >= TH_OUTER: + motors_spin_left(TURN_SPD) + check_oscillation_and_escape(-1) + + elif state == STATE_ESCAPING: + if obstacle: + motors_stop() + state = STATE_AVOID + return + if time.ticks_diff(time.ticks_ms(), escaping_until_ms) >= 0: + motors_stop() + state = STATE_IDLE + + +# ===================== LED 상태 표시 ===================== +def update_status_led(): + if state == STATE_IDLE: + set_led(1, 0, 30, 0) + set_led(2, 0, 30, 0) + elif state == STATE_TURNING: + set_led(1, 0, 0, 50) + set_led(2, 0, 0, 50) + elif state == STATE_MOVING: + set_led(1, 0, 50, 0) + set_led(2, 0, 50, 0) + elif state == STATE_AVOID: + set_led(1, 50, 50, 0) + set_led(2, 50, 50, 0) + elif state == STATE_ESCAPING: + set_led(1, 50, 25, 0) + set_led(2, 50, 25, 0) + elif state == STATE_EMERGENCY: + set_led(1, 50, 0, 0) + set_led(2, 50, 0, 0) + + +# ===================== 메인 ===================== +def main(): + global encoder_left_pin, encoder_right_pin, running + + print("Initializing MONA ESP...") + mona_esp_init() + + # 엔코더 인터럽트 설정 + encoder_left_pin = Pin(PIN_ENCODER_LEFT, Pin.IN) + encoder_right_pin = Pin(PIN_ENCODER_RIGHT, Pin.IN) + encoder_left_pin.irq(trigger=Pin.IRQ_RISING, handler=isr_left_encoder) + encoder_right_pin.irq(trigger=Pin.IRQ_RISING, handler=isr_right_encoder) + + # WiFi 연결 + if not connect_wifi(): + print("Failed to connect WiFi. Restarting...") + time.sleep(3) + import machine + machine.reset() + + # UDP 설정 + setup_udp() + + # IR 센서 스레드 시작 (Core 1에서 실행) + _thread.start_new_thread(ir_sensor_thread, ()) + + print("MONA UDP Controller ready!") + set_led(1, 0, 30, 0) + set_led(2, 0, 30, 0) + + led_update_counter = 0 + last_control_time = time.ticks_ms() + + try: + while True: + # UDP 패킷 처리 + handle_udp_packet() + + # IR 센서 값 가져오기 + ir = get_ir_values() + r1, r2, r3, r4, r5 = ir[0], ir[1], ir[2], ir[3], ir[4] + + # 제어 루프 + control_loop(r1, r2, r3, r4, r5) + + # LED 상태 업데이트 + led_update_counter += 1 + if led_update_counter >= 100: + update_status_led() + led_update_counter = 0 + + # 제어 주기 유지 (약 5ms 목표) + elapsed = time.ticks_diff(time.ticks_ms(), last_control_time) + if elapsed < 5: + time.sleep_ms(5 - elapsed) + last_control_time = time.ticks_ms() + + except KeyboardInterrupt: + print("\nStopping...") + running = False + motors_stop() + set_led(1, 0, 0, 0) + set_led(2, 0, 0, 0) + if udp_socket: + udp_socket.close() + print("MONA stopped.") + + +if __name__ == "__main__": + main() diff --git a/Micropython/MONA_puppet/neopixel_led.py b/Micropython/MONA_puppet/neopixel_led.py new file mode 100644 index 0000000..1dcf56d --- /dev/null +++ b/Micropython/MONA_puppet/neopixel_led.py @@ -0,0 +1,61 @@ +""" +WS2812B NeoPixel LED Driver for MicroPython +Uses the built-in neopixel module +""" + +from machine import Pin +from neopixel import NeoPixel + + +class NeoPixelLED: + """Simple NeoPixel LED wrapper class""" + + def __init__(self, pin: int, num_leds: int = 1): + """ + Initialize NeoPixel LED + + Args: + pin: GPIO pin number + num_leds: Number of LEDs in the strip + """ + self._pin = Pin(pin, Pin.OUT) + self._np = NeoPixel(self._pin, num_leds) + self._num_leds = num_leds + self.clear() + + def clear(self): + """Turn off all LEDs""" + for i in range(self._num_leds): + self._np[i] = (0, 0, 0) + self._np.write() + + def fill(self, r: int, g: int, b: int): + """ + Fill all LEDs with the same color + + Args: + r: Red component (0-255) + g: Green component (0-255) + b: Blue component (0-255) + """ + for i in range(self._num_leds): + self._np[i] = (r, g, b) + self._np.write() + + def set_pixel(self, index: int, r: int, g: int, b: int): + """ + Set a specific LED color + + Args: + index: LED index + r: Red component (0-255) + g: Green component (0-255) + b: Blue component (0-255) + """ + if 0 <= index < self._num_leds: + self._np[index] = (r, g, b) + self._np.write() + + def show(self): + """Update the LED strip (write changes)""" + self._np.write() diff --git a/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino b/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino new file mode 100644 index 0000000..4ba0246 --- /dev/null +++ b/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino @@ -0,0 +1,674 @@ +#include +#include +#include "Mona_ESP_lib.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ====== USER CONFIG ====== +const char* SSID = "Your SSID"; +const char* PASSWORD = "Your Password"; +const String SELF_ID = "11"; // Change this for each robot (0-11) +const uint16_t TCP_PORT = 8080; +const int UDP_PORT = 8080; // UDP: PC → ESP32 이동 명령 (TCP와 프로토콜 달라 포트 공유 가능) + +// JSON buffer size +const size_t JSON_SIZE = 2048; + +WiFiServer tcpServer(TCP_PORT); +std::vector clients; + +DynamicJsonDocument selfMessageDoc(JSON_SIZE); +std::map receivedJSON_MAP; +std::map CommRecvTime_MAP; +SemaphoreHandle_t mapLock; + +unsigned long lastBroadcast_ms = 0; +unsigned long lastTcpSend_ms = 0; +unsigned long lastMemoryCleanup_ms = 0; +unsigned long lastStatsPrint_ms = 0; + +unsigned long p2p_loopCount = 0; +unsigned long p2p_maxLoopTime_us = 0; +unsigned long p2p_minLoopTime_us = 999999; +unsigned long p2p_totalLoopTime_us = 0; + +size_t currentMemoryUsage_bytes = 0; +size_t peakMemoryUsage_bytes = 0; + +static char g_jsonBuf[JSON_SIZE]; + +// ============================================================ +// ====== Puppet Config (Core 0 - puppetTask) ================= +// ====== UDP 이동 명령 수신, 모터/IR/엔코더 제어 ============= +// ============================================================ + +WiFiUDP udp; +char packetBuffer[255]; + +// --- 상태 머신 --- +enum RobotState { + STATE_IDLE, + STATE_TURNING, + STATE_MOVING, + STATE_AVOID, + STATE_ESCAPING, + STATE_EMERGENCY +}; +volatile RobotState state = STATE_IDLE; + +const uint32_t BROADCAST_MIN_INTERVAL_MS = 100; +const uint32_t BROADCAST_MAX_INTERVAL_MS = 100; +const uint32_t PEER_LINK_DROP_MS = 900; +const uint32_t WIFI_RECONNECT_INTERVAL_MS = 300; +const uint32_t WIFI_TIMEOUT_MS = 10000; +const uint32_t WIFI_RETRY_DELAY_MS = 200; +const uint32_t TCP_MONITORING_INTERVAL_MS = 100; +const uint32_t MEMORY_CLEANUP_INTERVAL_MS = 5000; +const uint32_t STATS_PRINT_INTERVAL_MS = 5000; +const uint32_t SERIAL_STABILIZE_DELAY_MS = 1000; + +static const int ESPNOW_MAX_PAYLOAD = 1470; + +uint32_t currentBroadcastInterval_ms = BROADCAST_MIN_INTERVAL_MS; +const uint32_t Peer_LinkDrop_MS = 900; +static uint8_t g_pkt[ESPNOW_MAX_PAYLOAD]; + + +// 펄스/제어 상수 +static const float PULSES_PER_MM = 18.0f; +static const float PULSES_PER_DEGREE = 12.8f; +static const int FWD_SPD = 100; +static const int TURN_SPD = 100; + +// 900펄스마다 새로운 명령을 받음 +static const long UPDATE_THRESHOLD_PULSES = 900; +static const float MIN_DIST_MM = 40.0f; + +// IR/회피 +static const int TH = 80; +static const int TH_OUTER = 100; +static const int DELTA = 15; + +// 핀 번호 정의 +static const int PIN_ENCODER_LEFT = 35; +static const int PIN_ENCODER_RIGHT = 39; + +// 제어 관련 임계값 +static const float ROTATION_DEADBAND_DEG = 5.0f; // 미세 회전 무시 각도 +static const int MIN_MOTOR_PWM = 60; // 모터 구동 최소 출력 + +// PI 제어 게인 (주행 보정) +static const float K_P = 0.5f; +static const float K_I = 0.002f; +static const float INTEGRAL_LIMIT = 500.0f; +static const int ERROR_DEADBAND = 5; + +// 비상 동작 속도 +static const int EMERGENCY_SPIN_SPD = 200; +static const uint16_t ESCAPING_MS = 1000; +unsigned long escaping_until_ms = 0; + +static const unsigned long EMERGENCY_SPIN_MS = 1200; +static const unsigned long BACK_MS = 120; +static const unsigned long OSCILLATION_WINDOW_MS = 1200; +static const int OSCILLATION_COUNT_THRESHOLD = 4; + +int last_turn_direction = 0; +int turn_change_count = 0; +unsigned long oscillation_timer_start = 0; + +unsigned long emergency_back_until = 0; +unsigned long emergency_spin_until = 0; + +volatile long left_encoder_count = 0; +volatile long right_encoder_count = 0; + +void IRAM_ATTR isr_left_encoder() { left_encoder_count++; } +void IRAM_ATTR isr_right_encoder() { right_encoder_count++; } + +static long start_left_count = 0; +static long start_right_count = 0; +static long target_turn_pulses = 0; +static long target_move_pulses = 0; +static float integral_error = 0.0f; + +// --- 제어 주기 --- +static const unsigned long CONTROL_INTERVAL_MS = 5; // 5ms 이동 제어 +static const unsigned long LED_UPDATE_INTERVAL_MS = 100; // 100ms LED 갱신 + +// Puppet 태스크 핸들 +TaskHandle_t puppetTaskHandle = NULL; + +// ====== P2P 함수들 ========================================== + +bool update_Broadcast_recv_JSON_MAP(const String& senderID, const char* jsonBuf, size_t jsonLen) { + DynamicJsonDocument* doc = new DynamicJsonDocument(JSON_SIZE); + if (deserializeJson(*doc, jsonBuf, jsonLen)) { delete doc; return false; } + + // 콜백(인터럽트 컨텍스트)에서 오래 블록하지 않도록 즉시 실패 허용 + if (xSemaphoreTake(mapLock, 0) != pdTRUE) { delete doc; return false; } + + auto it = receivedJSON_MAP.find(senderID); + if (it != receivedJSON_MAP.end()) { + delete it->second; + it->second = doc; + } else { + receivedJSON_MAP[senderID] = doc; + currentMemoryUsage_bytes += JSON_SIZE; + } + CommRecvTime_MAP[senderID] = millis(); + xSemaphoreGive(mapLock); + return true; +} + +void onEspNowRecv(const esp_now_recv_info_t *recv_info, const uint8_t *incoming, int len) { + (void)recv_info; + if (len <= 1) return; + + uint8_t idLen = incoming[0]; + if (len < 1 + idLen) return; + + String senderID = String((const char*)(&incoming[1]), idLen); + if (senderID == SELF_ID) return; + + int jsonLen = len - (1 + idLen); + if (jsonLen <= 0) return; + update_Broadcast_recv_JSON_MAP(senderID, (const char*)(&incoming[1 + idLen]), (size_t)jsonLen); +} + +void ensureBroadcastPeer() { + uint8_t bcast[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + if (esp_now_is_peer_exist(bcast)) return; + + esp_now_peer_info_t p = {}; + memcpy(p.peer_addr, bcast, 6); + p.ifidx = WIFI_IF_STA; + p.channel = WiFi.channel(); + p.encrypt = false; + + esp_now_add_peer(&p); +} + +void broadcastSelfMessageIfDue() { + unsigned long now = millis(); + if ((unsigned long)(now - lastBroadcast_ms) < currentBroadcastInterval_ms) return; + lastBroadcast_ms += currentBroadcastInterval_ms; + currentBroadcastInterval_ms = random(BROADCAST_MIN_INTERVAL_MS, BROADCAST_MAX_INTERVAL_MS); + + if (selfMessageDoc.isNull()) return; + ensureBroadcastPeer(); + + size_t jsonLen = serializeJson(selfMessageDoc, g_jsonBuf, sizeof(g_jsonBuf)); + if (jsonLen == 0) return; + + const uint8_t idLen = (uint8_t)SELF_ID.length(); + const size_t total = 1 + (size_t)idLen + jsonLen; + if ((int)total > ESPNOW_MAX_PAYLOAD) return; + + g_pkt[0] = idLen; + memcpy(&g_pkt[1], SELF_ID.c_str(), idLen); + memcpy(&g_pkt[1 + idLen], g_jsonBuf, jsonLen); + + uint8_t bcast[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + esp_now_send(bcast, g_pkt, total); +} + +void sendTcpMonitoringIfDue() { + unsigned long now = millis(); + if ((unsigned long)(now - lastTcpSend_ms) < TCP_MONITORING_INTERVAL_MS) return; + lastTcpSend_ms += TCP_MONITORING_INTERVAL_MS; + + size_t activeRobots = receivedJSON_MAP.size(); + size_t monitorSize = JSON_SIZE * (activeRobots + 1); + DynamicJsonDocument monitor(monitorSize); + monitor["agent_id"] = SELF_ID; + JsonObject rx = monitor.createNestedObject("received_messages"); + + xSemaphoreTake(mapLock, portMAX_DELAY); + for (auto const& kv : receivedJSON_MAP) { + if ((unsigned long)(now - CommRecvTime_MAP[kv.first]) <= Peer_LinkDrop_MS) + rx[kv.first] = kv.second->as(); + } + xSemaphoreGive(mapLock); + + String out; + serializeJson(monitor, out); + out += "\n"; + for (auto& c : clients) + if (c && c.connected()) c.print(out); + + size_t tempMem = currentMemoryUsage_bytes + monitorSize; + if (tempMem > peakMemoryUsage_bytes) peakMemoryUsage_bytes = tempMem; +} + +void cleanupStaleEntries() { + unsigned long now = millis(); + if ((unsigned long)(now - lastMemoryCleanup_ms) < MEMORY_CLEANUP_INTERVAL_MS) return; + lastMemoryCleanup_ms += MEMORY_CLEANUP_INTERVAL_MS; + + xSemaphoreTake(mapLock, portMAX_DELAY); + auto it = receivedJSON_MAP.begin(); + int cleaned = 0; + while (it != receivedJSON_MAP.end()) { + if ((unsigned long)(now - CommRecvTime_MAP[it->first]) > (Peer_LinkDrop_MS * 2)) { + delete it->second; + CommRecvTime_MAP.erase(it->first); + it = receivedJSON_MAP.erase(it); + currentMemoryUsage_bytes -= JSON_SIZE; + cleaned++; + } else { ++it; } + } + xSemaphoreGive(mapLock); + + if (cleaned > 0) + Serial.printf("[P2P][MEM] Cleaned %d stale entries, current: %d bytes\n", + cleaned, currentMemoryUsage_bytes); +} + +void printTimingStats() { + unsigned long now = millis(); + if ((unsigned long)(now - lastStatsPrint_ms) < STATS_PRINT_INTERVAL_MS) return; + lastStatsPrint_ms += STATS_PRINT_INTERVAL_MS; + if (p2p_loopCount == 0) return; + + unsigned long avg = p2p_totalLoopTime_us / p2p_loopCount; + unsigned long jitter = p2p_maxLoopTime_us - p2p_minLoopTime_us; + Serial.printf("[P2P][TIMING] Loops:%lu Avg:%lu us Jitter:%lu us ActiveRobots:%d\n", + p2p_loopCount, avg, jitter, receivedJSON_MAP.size()); + Serial.printf("[P2P][MEM] Current:%d Peak:%d bytes\n", + currentMemoryUsage_bytes, peakMemoryUsage_bytes); + + p2p_loopCount = p2p_totalLoopTime_us = 0; + p2p_maxLoopTime_us = 0; + p2p_minLoopTime_us = 999999; +} + +// ============================================================ +// ====== Puppet 함수들 ======================================= +// ============================================================ + +static inline void clear_motion_targets() { + target_turn_pulses = 0; + target_move_pulses = 0; + start_left_count = left_encoder_count; + start_right_count = right_encoder_count; + integral_error = 0.0f; +} + +static inline void enter_escaping(uint16_t ms = ESCAPING_MS) { + escaping_until_ms = millis() + ms; + Motors_forward(FWD_SPD); + state = STATE_ESCAPING; +} + +static inline void start_emergency_left_spin() { + unsigned long now = millis(); + emergency_back_until = now + BACK_MS; + emergency_spin_until = emergency_back_until + EMERGENCY_SPIN_MS; + Motors_backward(FWD_SPD); + state = STATE_EMERGENCY; + turn_change_count = 0; + last_turn_direction = -1; + oscillation_timer_start = now; + Serial.println("[Puppet][EMERGENCY] back -> spin_left"); +} + +static inline void check_oscillation_and_escape(int dir) { + if (last_turn_direction != 0 && dir != last_turn_direction) { + unsigned long now = millis(); + if (now - oscillation_timer_start > OSCILLATION_WINDOW_MS) { + turn_change_count = 1; + oscillation_timer_start = now; + } else { turn_change_count++; } + if (turn_change_count >= OSCILLATION_COUNT_THRESHOLD) { + start_emergency_left_spin(); return; + } + } + last_turn_direction = dir; +} + +void pi_control(long l_now, long r_now, int* left_pwm, int* right_pwm) { + long err = l_now - r_now; + if (abs(err) < ERROR_DEADBAND) err = 0; + integral_error += (float)err; + if (integral_error > INTEGRAL_LIMIT) integral_error = INTEGRAL_LIMIT; + if (integral_error < -INTEGRAL_LIMIT) integral_error = -INTEGRAL_LIMIT; + float u = (K_P * (float)err) + (K_I * integral_error); + *left_pwm = constrain((int)lroundf(FWD_SPD - u), MIN_MOTOR_PWM, 255); + *right_pwm = constrain((int)lroundf(FWD_SPD + u), MIN_MOTOR_PWM, 255); +} + +void update_status_led() { + switch (state) { + case STATE_IDLE: Set_LED(1, 0, 30, 0); Set_LED(2, 0, 30, 0); break; + case STATE_TURNING: Set_LED(1, 0, 0, 50); Set_LED(2, 0, 0, 50); break; + case STATE_MOVING: Set_LED(1, 0, 50, 0); Set_LED(2, 0, 50, 0); break; + case STATE_AVOID: Set_LED(1,50, 50, 0); Set_LED(2,50, 50, 0); break; + case STATE_ESCAPING: Set_LED(1,50, 25, 0); Set_LED(2,50, 25, 0); break; + case STATE_EMERGENCY: Set_LED(1,50, 0, 0); Set_LED(2,50, 0, 0); break; + } +} + +void start_motion(float angle_deg, float dist_mm) { + if (state == STATE_AVOID || state == STATE_ESCAPING || state == STATE_EMERGENCY) return; + + start_left_count = left_encoder_count; + start_right_count = right_encoder_count; + integral_error = 0.0f; + + target_turn_pulses = (long)lroundf(fabsf(angle_deg) * PULSES_PER_DEGREE); + target_move_pulses = (long)lroundf(fabsf(dist_mm) * PULSES_PER_MM); + + if (target_turn_pulses > 0 && fabsf(angle_deg) > ROTATION_DEADBAND_DEG) { + state = STATE_TURNING; + if (angle_deg > 0) Motors_spin_right(TURN_SPD); + else Motors_spin_left(TURN_SPD); + } else if (target_move_pulses > 0) { + state = STATE_MOVING; + } else { + state = STATE_IDLE; + Motors_stop(); + } +} + +void handle_udp_packet() { + int packetSize = udp.parsePacket(); + if (!packetSize) return; + + // 가장 최신 패킷만 사용 (버퍼 플러시) + while (packetSize) { + int len = udp.read(packetBuffer, 255); + if (len > 0) packetBuffer[len] = 0; + packetSize = udp.parsePacket(); + } + + if (strncasecmp(packetBuffer, "STOP", 4) == 0) { + Motors_stop(); clear_motion_targets(); state = STATE_IDLE; return; + } + + if (packetBuffer[0] == 'G' || packetBuffer[0] == 'g') { + float angle = 0, dist = 0; + if (sscanf(packetBuffer + 1, "%f %f", &angle, &dist) == 2) { + if (dist < MIN_DIST_MM) return; + if (state == STATE_MOVING) { + long l_now = labs(left_encoder_count - start_left_count); + long r_now = labs(right_encoder_count - start_right_count); + if (((l_now + r_now) / 2) < UPDATE_THRESHOLD_PULSES) return; + } + start_motion(angle, dist); + } + } +} + +void control_loop(int r1, int r2, int r3, int r4, int r5) { + bool obstacle = (r1 > TH_OUTER) || (r2 > TH) || (r3 > TH) || (r4 > TH) || (r5 > TH_OUTER); + + if (obstacle) { + if (state != STATE_AVOID && state != STATE_ESCAPING && state != STATE_EMERGENCY) { + Motors_stop(); clear_motion_targets(); state = STATE_AVOID; + } + } + + if (state == STATE_EMERGENCY) { + unsigned long now = millis(); + if (now < emergency_back_until) { Motors_backward(FWD_SPD); return; } + if (now < emergency_spin_until) { Motors_spin_left(EMERGENCY_SPIN_SPD); return; } + Motors_stop(); state = STATE_IDLE; return; + } + + long l_now = labs(left_encoder_count - start_left_count); + long r_now = labs(right_encoder_count - start_right_count); + long avg = (l_now + r_now) / 2; + + switch (state) { + case STATE_TURNING: + if (avg >= target_turn_pulses) { + Motors_stop(); + if (target_move_pulses > 0) { + start_left_count = left_encoder_count; + start_right_count = right_encoder_count; + integral_error = 0.0f; + state = STATE_MOVING; + } else { state = STATE_IDLE; } + } + break; + + case STATE_MOVING: + if (avg >= target_move_pulses) { + Motors_stop(); state = STATE_IDLE; + } else { + int lp, rp; + pi_control(l_now, r_now, &lp, &rp); + Left_mot_forward(lp); + Right_mot_forward(rp); + } + break; + + case STATE_AVOID: + if ((r1<=TH_OUTER)&&(r2<=TH)&&(r3<=TH)&&(r4<=TH)&&(r5<=TH_OUTER)) { + enter_escaping(ESCAPING_MS); break; + } + if (r3 >= TH) { + if (abs(r2 - r4) <= DELTA || r2 < r4) { + Motors_spin_left(TURN_SPD); check_oscillation_and_escape(-1); + } else { + Motors_spin_right(TURN_SPD); check_oscillation_and_escape(+1); + } + } else if (r1 >= TH_OUTER || r2 >= TH) { + Motors_spin_right(TURN_SPD); check_oscillation_and_escape(+1); + } else if (r4 >= TH || r5 >= TH_OUTER) { + Motors_spin_left(TURN_SPD); check_oscillation_and_escape(-1); + } + break; + + case STATE_ESCAPING: + if (obstacle) { Motors_stop(); state = STATE_AVOID; break; } + if ((int32_t)(millis() - escaping_until_ms) >= 0) { Motors_stop(); state = STATE_IDLE; } + break; + + default: break; + } +} + + +// ============================================================ +// ====== Core 0: Puppet 태스크 =============================== +// ============================================================ +void puppetTask(void* parameter) { + // UDP 시작 (setup()에서 WiFi 연결 완료 후 이 태스크가 시작되므로 안전) + udp.begin(UDP_PORT); + Serial.printf("[Puppet][Core0] UDP server started on port %d\n", UDP_PORT); + + unsigned long last_control_time = 0; + unsigned long last_led_update = 0; + + while (true) { + unsigned long now = millis(); + + // UDP 명령 처리 (G command / STOP) + handle_udp_packet(); + + // 이동 제어 (5ms 주기) + if (now - last_control_time >= CONTROL_INTERVAL_MS) { + last_control_time = now; + int r1 = Get_IR(1), r2 = Get_IR(2), r3 = Get_IR(3); + int r4 = Get_IR(4), r5 = Get_IR(5); + control_loop(r1, r2, r3, r4, r5); + } + + // LED 갱신 (100ms 주기) + if (now - last_led_update >= LED_UPDATE_INTERVAL_MS) { + last_led_update = now; + update_status_led(); + } + + vTaskDelay(pdMS_TO_TICKS(1)); // 1ms yield → 다른 태스크에 CPU 양보 + } +} + +// ============================================================ +// ====== 공통 초기화 ========================================= +// ============================================================ +void setupNetwork() { + WiFi.mode(WIFI_STA); + WiFi.setSleep(false); + esp_wifi_set_ps(WIFI_PS_NONE); + WiFi.setAutoReconnect(true); + WiFi.persistent(true); + WiFi.begin(SSID, PASSWORD); + + Serial.print("[WiFi] Connecting"); + unsigned long t0 = millis(); + while (WiFi.status() != WL_CONNECTED) { + delay(WIFI_RECONNECT_INTERVAL_MS); Serial.print("."); + if (millis() - t0 > WIFI_TIMEOUT_MS) { + Serial.println("\n[WiFi] Timeout, retrying..."); + WiFi.disconnect(true, true); + delay(WIFI_RETRY_DELAY_MS); + WiFi.begin(SSID, PASSWORD); + t0 = millis(); Serial.print("[WiFi] Connecting"); + } + } + Serial.printf("\n[WiFi] Connected: %s\n", WiFi.localIP().toString().c_str()); + Serial.printf("[WiFi] SELF_ID: %s\n", SELF_ID.c_str()); + + uint8_t primary = 0; + wifi_second_chan_t second = WIFI_SECOND_CHAN_NONE; + esp_wifi_get_channel(&primary, &second); + Serial.printf("[WiFi] Channel: %d\n", primary); + + // TCP 서버 시작 (P2P용) + tcpServer.begin(); + Serial.printf("[P2P][Core1] TCP server started on port %d\n", TCP_PORT); + + // ESP-NOW 초기화 (P2P용) + if (esp_now_init() != ESP_OK) { + Serial.println("[ESPNOW] Init failed -> restart"); ESP.restart(); + } + esp_now_register_recv_cb(onEspNowRecv); + ensureBroadcastPeer(); + Serial.printf("[ESPNOW] Ready. Max payload=%d bytes\n", ESPNOW_MAX_PAYLOAD); +} + +void setup() { + Serial.begin(115200); + delay(SERIAL_STABILIZE_DELAY_MS); + + randomSeed(esp_random()); + mapLock = xSemaphoreCreateMutex(); + + // P2P 타이밍/메모리 초기화 + currentMemoryUsage_bytes = JSON_SIZE; // selfMessageDoc 크기 + peakMemoryUsage_bytes = JSON_SIZE; + unsigned long now = millis(); + lastBroadcast_ms = now; + lastTcpSend_ms = now; + lastMemoryCleanup_ms = now; + lastStatsPrint_ms = now; + currentBroadcastInterval_ms = random(BROADCAST_MIN_INTERVAL_MS, BROADCAST_MAX_INTERVAL_MS); + + // MONA 하드웨어 초기화 (Puppet용) + Mona_ESP_init(); + pinMode(PIN_ENCODER_LEFT, INPUT); + pinMode(PIN_ENCODER_RIGHT, INPUT); + attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_LEFT), isr_left_encoder, RISING); + attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_RIGHT), isr_right_encoder, RISING); + Set_LED(1, 0, 30, 0); + Set_LED(2, 0, 30, 0); + + // WiFi + TCP + ESP-NOW 초기화 + setupNetwork(); + + // ★ Core 0에 Puppet 태스크 생성 + xTaskCreatePinnedToCore( + puppetTask, // 태스크 함수 + "PuppetTask", // 태스크 이름 + 8192, // 스택 크기 (bytes) + NULL, // 파라미터 + 2, // 우선순위 (loop()의 1보다 높게 → 모터 제어 우선) + &puppetTaskHandle, // 태스크 핸들 + 0 // Core 0 고정 + ); + + Serial.println("[SYSTEM] Dual-core ready!"); + Serial.println("[SYSTEM] Core 0 → Puppet (UDP motion control)"); + Serial.println("[SYSTEM] Core 1 → P2P (ESP-NOW + TCP CBBA)"); +} + + +// ============================================================ +// ====== Core 1: P2P loop() ================================== +// ============================================================ +void loop() { + unsigned long loopStart_us = micros(); + + // WiFi 재연결 체크 + if (WiFi.status() != WL_CONNECTED) { + for (auto& c : clients) c.stop(); + clients.clear(); + WiFi.reconnect(); + yield(); return; + } + + // TCP 클라이언트 수락 + WiFiClient newcomer = tcpServer.available(); + if (newcomer) { + newcomer.setTimeout(10); + newcomer.setNoDelay(true); + bool placed = false; + for (auto& c : clients) { + if (!c || !c.connected()) { c.stop(); c = newcomer; placed = true; break; } + } + if (!placed) clients.push_back(newcomer); + } + + // TCP 수신: PC → selfMessageDoc (CBBA 데이터) + for (auto& c : clients) { + if (c && c.connected() && c.available()) { + String line = c.readStringUntil('\n'); + (void)deserializeJson(selfMessageDoc, line); + } + } + + // 1. ESP-NOW 브로드캐스트 (100ms 주기) + broadcastSelfMessageIfDue(); + + // 2. TCP 모니터링 송신 (50ms 주기) + sendTcpMonitoringIfDue(); + + // 3. 메모리 정리 (5000ms 주기) + cleanupStaleEntries(); + + // 4. 통계 출력 (5000ms 주기) + printTimingStats(); + + // TCP 클라이언트 정리 + for (auto& c : clients) + if (c && !c.connected()) c.stop(); + clients.erase( + std::remove_if(clients.begin(), clients.end(), + [](WiFiClient& c){ return !c.connected(); }), + clients.end() + ); + + // 루프 시간 측정 + unsigned long dur = micros() - loopStart_us; + p2p_loopCount++; + p2p_totalLoopTime_us += dur; + if (dur > p2p_maxLoopTime_us) p2p_maxLoopTime_us = dur; + if (dur < p2p_minLoopTime_us) p2p_minLoopTime_us = dur; + + yield(); +} \ No newline at end of file diff --git a/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino b/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino new file mode 100644 index 0000000..8feaf6a --- /dev/null +++ b/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino @@ -0,0 +1,433 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ====== USER CONFIG ====== +const char* SSID = "InMOLab"; +const char* PASSWORD = "dlsahfoq104!"; +const String SELF_ID = "Your SELF_ID"; +const uint16_t SERVER_PORT = 8080; + +// JSON 버퍼는 넉넉하게(단, 실제 ESPNOW payload는 1470 제한) +const size_t JSON_SIZE = 2048; +const int TOTAL_ROBOTS = 12; + +// ====== TIMING CONSTANTS (누적 오차 방지 방식) ====== +const uint32_t BROADCAST_MIN_INTERVAL_MS = 100; +const uint32_t BROADCAST_MAX_INTERVAL_MS = 100; +const uint32_t TCP_MONITORING_INTERVAL_MS = 100; +const uint32_t MEMORY_CLEANUP_INTERVAL_MS = 5000; +const uint32_t STATS_PRINT_INTERVAL_MS = 5000; + +const uint32_t Peer_LinkDrop_MS = 900; +const uint32_t WIFI_RECONNECT_INTERVAL_MS = 300; +const uint32_t WIFI_TIMEOUT_MS = 10000; +const uint32_t WIFI_RETRY_DELAY_MS = 200; +const uint32_t SERIAL_STABILIZE_DELAY_MS = 1000; + +// ESPNOW v2 payload 최대 +static const int ESPNOW_MAX_PAYLOAD = 1470; + +WiFiServer server(SERVER_PORT); +std::vector clients; + +DynamicJsonDocument selfMessageDoc(JSON_SIZE); +std::map receivedJSON_MAP; +std::map CommRecvTime_MAP; + +SemaphoreHandle_t mapLock; + +// ====== 누적 오차 방지를 위한 타이밍 변수 ====== +unsigned long lastBroadcast_ms = 0; +unsigned long lastTcpSend_ms = 0; +unsigned long lastMemoryCleanup_ms = 0; +unsigned long lastStatsPrint_ms = 0; +uint32_t currentBroadcastInterval_ms = BROADCAST_MIN_INTERVAL_MS; + +// ====== TIMING MONITORING ====== +unsigned long loopCount = 0; +unsigned long maxLoopTime_us = 0; +unsigned long minLoopTime_us = 999999; +unsigned long totalLoopTime_us = 0; + +// ====== MEMORY MONITORING ====== +size_t currentMemoryUsage_bytes = 0; +size_t peakMemoryUsage_bytes = 0; + +static char g_jsonBuf[JSON_SIZE]; +static uint8_t g_pkt[ESPNOW_MAX_PAYLOAD]; + +// ------------------------- +// Memory cleanup +// ------------------------- +void cleanupStaleEntries() { + unsigned long now = millis(); + + // 누적 오차 방지: 정확히 5초마다 실행 + if ((unsigned long)(now - lastMemoryCleanup_ms) < MEMORY_CLEANUP_INTERVAL_MS) { + return; + } + + lastMemoryCleanup_ms += MEMORY_CLEANUP_INTERVAL_MS; + + xSemaphoreTake(mapLock, portMAX_DELAY); + + auto it = receivedJSON_MAP.begin(); + int cleanedCount = 0; + + while (it != receivedJSON_MAP.end()) { + if ((unsigned long)(now - CommRecvTime_MAP[it->first]) > (Peer_LinkDrop_MS * 2)) { + delete it->second; + CommRecvTime_MAP.erase(it->first); + it = receivedJSON_MAP.erase(it); + + currentMemoryUsage_bytes -= JSON_SIZE; + cleanedCount++; + } else { + ++it; + } + } + + xSemaphoreGive(mapLock); + + if (cleanedCount > 0) { + Serial.printf("[MEMORY] Cleaned %d stale entries, current: %d bytes\n", + cleanedCount, currentMemoryUsage_bytes); + } +} + +// ------------------------- +// Update map +// ------------------------- +bool update_Broadcast_recv_JSON_MAP(const String& senderID, const char* jsonBuf, size_t jsonLen) { + DynamicJsonDocument* doc = new DynamicJsonDocument(JSON_SIZE); + DeserializationError err = deserializeJson(*doc, jsonBuf, jsonLen); + if (err) { + delete doc; + return false; + } + + if (xSemaphoreTake(mapLock, 0) != pdTRUE) { + delete doc; + return false; + } + + auto it = receivedJSON_MAP.find(senderID); + if (it != receivedJSON_MAP.end()) { + delete it->second; + it->second = doc; + } else { + receivedJSON_MAP[senderID] = doc; + currentMemoryUsage_bytes += JSON_SIZE; + } + CommRecvTime_MAP[senderID] = millis(); + + xSemaphoreGive(mapLock); + return true; +} + +void onEspNowRecv(const esp_now_recv_info_t *recv_info, const uint8_t *incoming, int len) { + (void)recv_info; + + if (len <= 1) return; + + uint8_t idLen = incoming[0]; + if (len < 1 + idLen) return; + + String senderID = String((const char*)(&incoming[1]), idLen); + if (senderID == SELF_ID) return; + + int jsonLen = len - (1 + idLen); + if (jsonLen <= 0) return; + + update_Broadcast_recv_JSON_MAP(senderID, + (const char*)(&incoming[1 + idLen]), + (size_t)jsonLen); +} + +void ensureBroadcastPeer() { + uint8_t bcast[6] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; + if (esp_now_is_peer_exist(bcast)) return; + + esp_now_peer_info_t p = {}; + memcpy(p.peer_addr, bcast, 6); + p.ifidx = WIFI_IF_STA; + p.channel = WiFi.channel(); + p.encrypt = false; + + esp_err_t rc = esp_now_add_peer(&p); + if (rc != ESP_OK && rc != ESP_ERR_ESPNOW_EXIST) { + } +} + +void broadcastSelfMessageIfDue() { + unsigned long now = millis(); + + // 누적 오차 방지: (now - lastBroadcast_ms) 사용 + if ((unsigned long)(now - lastBroadcast_ms) < currentBroadcastInterval_ms) { + return; + } + + // 누적 오차 방지: lastBroadcast_ms += interval + lastBroadcast_ms += currentBroadcastInterval_ms; + + // 다음 간격 설정 (충돌 회피를 위한 랜덤화) + currentBroadcastInterval_ms = random(BROADCAST_MIN_INTERVAL_MS, BROADCAST_MAX_INTERVAL_MS); + + if (selfMessageDoc.isNull()) return; + + ensureBroadcastPeer(); + + size_t jsonLen = serializeJson(selfMessageDoc, g_jsonBuf, sizeof(g_jsonBuf)); + if (jsonLen == 0) return; + + const uint8_t idLen = (uint8_t)SELF_ID.length(); + const size_t total = 1 + (size_t)idLen + jsonLen; + + if ((int)total > ESPNOW_MAX_PAYLOAD) { + return; + } + + g_pkt[0] = idLen; + memcpy(&g_pkt[1], SELF_ID.c_str(), idLen); + memcpy(&g_pkt[1 + idLen], g_jsonBuf, jsonLen); + + uint8_t bcast[6] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; + esp_err_t rc = esp_now_send(bcast, g_pkt, total); + if (rc != ESP_OK) { + //Serial.printf("[ESP-NOW TX] send failed: %d\n", (int)rc); + } +} + +// ------------------------- +// TCP Monitoring (누적 오차 방지) +// ------------------------- +void sendTcpMonitoringIfDue() { + unsigned long now = millis(); + + // 누적 오차 방지 + if ((unsigned long)(now - lastTcpSend_ms) < TCP_MONITORING_INTERVAL_MS) { + return; + } + + lastTcpSend_ms += TCP_MONITORING_INTERVAL_MS; + + // 동적 메모리 할당 (실제 필요한 크기만) + size_t activeRobots = receivedJSON_MAP.size(); + size_t monitorSize = JSON_SIZE * (activeRobots + 1); + + DynamicJsonDocument monitor(monitorSize); + monitor["agent_id"] = SELF_ID; + JsonObject rx = monitor.createNestedObject("received_messages"); + + xSemaphoreTake(mapLock, portMAX_DELAY); + for (auto const &kv : receivedJSON_MAP) { + if ((unsigned long)(now - CommRecvTime_MAP[kv.first]) <= Peer_LinkDrop_MS) { + rx[kv.first] = kv.second->as(); + } + } + xSemaphoreGive(mapLock); + + String out; + serializeJson(monitor, out); + out += "\n"; + + for (auto &c : clients) { + if (c && c.connected()) { + c.print(out); + } + } + + // 피크 메모리 추적 + size_t tempMemory = currentMemoryUsage_bytes + monitorSize; + if (tempMemory > peakMemoryUsage_bytes) { + peakMemoryUsage_bytes = tempMemory; + } +} + +// ------------------------- +// Statistics printing +// ------------------------- +void printTimingStats() { + unsigned long now = millis(); + + // 누적 오차 방지 + if ((unsigned long)(now - lastStatsPrint_ms) < STATS_PRINT_INTERVAL_MS) { + return; + } + + lastStatsPrint_ms += STATS_PRINT_INTERVAL_MS; + + if (loopCount == 0) return; + + unsigned long avgLoopTime_us = totalLoopTime_us / loopCount; + unsigned long jitter_us = maxLoopTime_us - minLoopTime_us; + + Serial.printf("[TIMING] Loops: %lu, Avg: %lu us, Min: %lu us, Max: %lu us\n", + loopCount, avgLoopTime_us, minLoopTime_us, maxLoopTime_us); + Serial.printf("[TIMING] Jitter: %lu us\n", jitter_us); + Serial.printf("[MEMORY] Current: %d bytes, Peak: %d bytes, Active robots: %d\n", + currentMemoryUsage_bytes, peakMemoryUsage_bytes, receivedJSON_MAP.size()); + + // 통계 리셋 + loopCount = 0; + totalLoopTime_us = 0; + maxLoopTime_us = 0; + minLoopTime_us = 999999; +} + +void setupNetwork() { + WiFi.mode(WIFI_STA); + WiFi.setSleep(false); + esp_wifi_set_ps(WIFI_PS_NONE); + WiFi.setAutoReconnect(true); + WiFi.persistent(true); + + WiFi.begin(SSID, PASSWORD); + + Serial.print("[WiFi] Connecting to AP"); + unsigned long t0 = millis(); + while (WiFi.status() != WL_CONNECTED) { + delay(WIFI_RECONNECT_INTERVAL_MS); + Serial.print("."); + if (millis() - t0 > WIFI_TIMEOUT_MS) { + Serial.println("\n[WiFi] connect timeout -> retry"); + WiFi.disconnect(true, true); + delay(WIFI_RETRY_DELAY_MS); + WiFi.begin(SSID, PASSWORD); + t0 = millis(); + Serial.print("[WiFi] Connecting to AP"); + } + } + + Serial.println("\n[WiFi] Connected!"); + Serial.printf("[WiFi] IP Address: %s\n", WiFi.localIP().toString().c_str()); + Serial.printf("[WiFi] Board ID (SELF_ID): %s\n", SELF_ID.c_str()); + + uint8_t primary = 0; + wifi_second_chan_t second = WIFI_SECOND_CHAN_NONE; + esp_wifi_get_channel(&primary, &second); + Serial.printf("[WiFi] Channel: %d\n", primary); + + server.begin(); + + if (esp_now_init() != ESP_OK) { + Serial.println("[ESPNOW] init failed -> restart"); + ESP.restart(); + } + + esp_now_register_recv_cb(onEspNowRecv); + ensureBroadcastPeer(); + + // Serial.printf("[ESPNOW] Ready. Max payload=%d bytes\n", ESPNOW_MAX_PAYLOAD); + // Serial.printf("[CONFIG] JSON_SIZE=%d bytes, Broadcast interval=%d~%dms\n", + // JSON_SIZE, BROADCAST_MIN_INTERVAL_MS, BROADCAST_MAX_INTERVAL_MS); +} + +void setup() { + Serial.begin(115200); + delay(SERIAL_STABILIZE_DELAY_MS); + + randomSeed(esp_random()); + mapLock = xSemaphoreCreateMutex(); + + // 초기 메모리 설정 + currentMemoryUsage_bytes = JSON_SIZE; // selfMessageDoc + peakMemoryUsage_bytes = JSON_SIZE; + + // 타이밍 초기화 (millis()로 현재 시간 설정) + unsigned long now = millis(); + lastBroadcast_ms = now; + lastTcpSend_ms = now; + lastMemoryCleanup_ms = now; + lastStatsPrint_ms = now; + + // 첫 브로드캐스트 간격 랜덤화 + currentBroadcastInterval_ms = random(BROADCAST_MIN_INTERVAL_MS, BROADCAST_MAX_INTERVAL_MS); + + setupNetwork(); + + Serial.println("[SYSTEM] Using drift-free timing (누적 오차 방지)"); +} + +void loop() { + unsigned long loopStart_us = micros(); + + // ========== WiFi 재연결 체크 ========== + if (WiFi.status() != WL_CONNECTED) { + for (auto &c : clients) c.stop(); + clients.clear(); + WiFi.reconnect(); + yield(); + return; + } + + // ========== TCP 클라이언트 수락 ========== + WiFiClient newcomer = server.available(); + if (newcomer) { + newcomer.setTimeout(10); + newcomer.setNoDelay(true); + + bool placed = false; + for (auto &c : clients) { + if (!c || !c.connected()) { + c.stop(); + c = newcomer; + placed = true; + break; + } + } + if (!placed) clients.push_back(newcomer); + } + + // ========== TCP 데이터 수신 (PC -> ESP32) ========== + for (auto &c : clients) { + if (c && c.connected() && c.available()) { + String line = c.readStringUntil('\n'); + (void)deserializeJson(selfMessageDoc, line); + } + } + + // ========== 핵심 작업 (누적 오차 방지 방식) ========== + + // 1. ESP-NOW 브로드캐스트 (가장 중요!) + broadcastSelfMessageIfDue(); + + // 2. TCP 모니터링 송신 + sendTcpMonitoringIfDue(); + + // 3. 메모리 정리 (주기적) + cleanupStaleEntries(); + + // 4. 통계 출력 (주기적) + printTimingStats(); + + // ========== TCP 클라이언트 정리 ========== + for (auto &c : clients) { + if (c && !c.connected()) c.stop(); + } + clients.erase(std::remove_if(clients.begin(), clients.end(), + [](WiFiClient& c){ return !c.connected(); }), clients.end()); + + // ========== 루프 시간 측정 ========== + unsigned long loopEnd_us = micros(); + unsigned long loopDuration_us = loopEnd_us - loopStart_us; + + loopCount++; + totalLoopTime_us += loopDuration_us; + + if (loopDuration_us > maxLoopTime_us) maxLoopTime_us = loopDuration_us; + if (loopDuration_us < minLoopTime_us) minLoopTime_us = loopDuration_us; + + // ========== CPU 양보 (다른 태스크에게 시간 할애) ========== + yield(); +} diff --git a/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino b/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino new file mode 100644 index 0000000..f63b34b --- /dev/null +++ b/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino @@ -0,0 +1,454 @@ +#include +#include +#include "Mona_ESP_lib.h" +#include +#include // strncasecmp를 위한 헤더 추가 (Core 3.x 대응) + +// ===================== WiFi / UDP ===================== +const char* ssid = "InMOLab"; +const char* password = "dlsahfoq104!"; +WiFiUDP udp; +const int localPort = 8080; +char packetBuffer[255]; + +// ===================== 상태 정의 ===================== +enum RobotState { + STATE_IDLE, + STATE_TURNING, + STATE_MOVING, + STATE_AVOID, + STATE_ESCAPING, + STATE_EMERGENCY +}; +volatile RobotState state = STATE_IDLE; + +// 펄스/제어 상수 +static const float PULSES_PER_MM = 18.0f; +static const float PULSES_PER_DEGREE = 12.8f; +static const int FWD_SPD = 100; +static const int TURN_SPD = 100; + +// 900펄스마다 새로운 명령을 받음 +static const long UPDATE_THRESHOLD_PULSES = 900; +static const float MIN_DIST_MM = 40.0f; + +// IR/회피 +static const int TH = 80; +static const int TH_OUTER = 100; +static const int DELTA = 15; + +// 핀 번호 정의 +static const int PIN_ENCODER_LEFT = 35; +static const int PIN_ENCODER_RIGHT = 39; + +// 제어 관련 임계값 +static const float ROTATION_DEADBAND_DEG = 5.0f; // 미세 회전 무시 각도 +static const int MIN_MOTOR_PWM = 60; // 모터 구동 최소 출력 + +// ===================== PI 제어 게인 (개선) ===================== +static const float K_P = 0.5f; // 비례 게인 감소 (0.95 → 0.5) +static const float K_I = 0.002f; // 적분 게인 대폭 감소 (0.01 → 0.002) + +// 적분 제한 추가 +static const float INTEGRAL_LIMIT = 500.0f; + +// 오차 데드밴드 +static const int ERROR_DEADBAND = 5; + +// 비상 동작 속도 +static const int EMERGENCY_SPIN_SPD = 200; + +static const uint16_t ESCAPING_MS = 1000; +unsigned long escaping_until_ms = 0; + +static const unsigned long EMERGENCY_SPIN_MS = 1200; +static const unsigned long BACK_MS = 120; +static const unsigned long OSCILLATION_WINDOW_MS = 1200; +static const int OSCILLATION_COUNT_THRESHOLD = 4; + +int last_turn_direction = 0; +int turn_change_count = 0; +unsigned long oscillation_timer_start = 0; + +unsigned long emergency_back_until = 0; +unsigned long emergency_spin_until = 0; + +// ===================== 엔코더 ===================== +volatile long left_encoder_count = 0; +volatile long right_encoder_count = 0; + +// Core 3.x에서는 인터럽트 핸들러에 IRAM_ATTR이 필수입니다. +void IRAM_ATTR isr_left_encoder() { left_encoder_count++; } +void IRAM_ATTR isr_right_encoder() { right_encoder_count++; } + +static long start_left_count = 0; +static long start_right_count = 0; +static long target_turn_pulses = 0; +static long target_move_pulses = 0; +static float integral_error = 0.0f; + +// ===================== IR 센서 캐시 (멀티코어용) ===================== +volatile int cached_ir[5] = {0, 0, 0, 0, 0}; +volatile bool ir_data_ready = false; +static const unsigned long IR_READ_INTERVAL_MS = 20; + +// ===================== 제어 주기 관리 ===================== +static const unsigned long CONTROL_INTERVAL_MS = 5; // 5ms 제어 주기 +unsigned long last_control_time = 0; + +// ===================== LED 상태 표시 ===================== +static const unsigned long LED_UPDATE_INTERVAL_MS = 100; +unsigned long last_led_update = 0; + +// ===================== 유틸리티 ===================== +static inline void clear_motion_targets() { + target_turn_pulses = 0; + target_move_pulses = 0; + start_left_count = left_encoder_count; + start_right_count = right_encoder_count; + integral_error = 0.0f; +} + +static inline void enter_escaping(uint16_t ms = ESCAPING_MS) { + escaping_until_ms = millis() + ms; + Motors_forward(FWD_SPD); + state = STATE_ESCAPING; +} + +static inline void start_emergency_left_spin() { + unsigned long now = millis(); + emergency_back_until = now + BACK_MS; + emergency_spin_until = emergency_back_until + EMERGENCY_SPIN_MS; + + Motors_backward(FWD_SPD); + state = STATE_EMERGENCY; + + turn_change_count = 0; + last_turn_direction = -1; + oscillation_timer_start = now; + + Serial.println("[EMERGENCY] back -> spin_left"); +} + +static inline void check_oscillation_and_escape(int current_direction) { + if (last_turn_direction != 0 && current_direction != last_turn_direction) { + unsigned long now = millis(); + if (now - oscillation_timer_start > OSCILLATION_WINDOW_MS) { + turn_change_count = 1; + oscillation_timer_start = now; + } else { + turn_change_count++; + } + + if (turn_change_count >= OSCILLATION_COUNT_THRESHOLD) { + start_emergency_left_spin(); + return; + } + } + last_turn_direction = current_direction; +} + +// ===================== 개선된 PI 제어 ===================== +void pi_control(long l_now, long r_now, int* left_pwm, int* right_pwm) { + long err = l_now - r_now; + + // 데드밴드 적용 - 작은 오차는 무시 + if (abs(err) < ERROR_DEADBAND) { + err = 0; + } + + // 적분 오차 누적 (제한 적용) + integral_error += (float)err; + if (integral_error > INTEGRAL_LIMIT) integral_error = INTEGRAL_LIMIT; + if (integral_error < -INTEGRAL_LIMIT) integral_error = -INTEGRAL_LIMIT; + + float u = (K_P * (float)err) + (K_I * integral_error); + + *left_pwm = constrain((int)lroundf(FWD_SPD - u), MIN_MOTOR_PWM, 255); + *right_pwm = constrain((int)lroundf(FWD_SPD + u), MIN_MOTOR_PWM, 255); +} + +// ===================== LED 상태 표시 ===================== +void update_status_led() { + switch (state) { + case STATE_IDLE: + Set_LED(1, 0, 30, 0); // 녹색: 대기 + Set_LED(2, 0, 30, 0); + break; + case STATE_TURNING: + Set_LED(1, 0, 0, 50); // 파란색: 회전 + Set_LED(2, 0, 0, 50); + break; + case STATE_MOVING: + Set_LED(1, 0, 50, 0); // 밝은 녹색: 이동 + Set_LED(2, 0, 50, 0); + break; + case STATE_AVOID: + Set_LED(1, 50, 50, 0); // 노란색: 회피 + Set_LED(2, 50, 50, 0); + break; + case STATE_ESCAPING: + Set_LED(1, 50, 25, 0); // 주황색: 탈출 + Set_LED(2, 50, 25, 0); + break; + case STATE_EMERGENCY: + Set_LED(1, 50, 0, 0); // 빨간색: 비상 + Set_LED(2, 50, 0, 0); + break; + } +} + +// ===================== Core 0: IR 센서 읽기 태스크 ===================== +TaskHandle_t irTaskHandle = NULL; + +void ir_sensor_task(void* parameter) { + while (true) { + int new_ir[5]; + for (int i = 0; i < 5; i++) { + new_ir[i] = Get_IR(i + 1); + } + + // 캐시 업데이트 (atomic하게) + noInterrupts(); + for (int i = 0; i < 5; i++) { + cached_ir[i] = new_ir[i]; + } + ir_data_ready = true; + interrupts(); + + vTaskDelay(pdMS_TO_TICKS(IR_READ_INTERVAL_MS)); + } +} + +// ===================== IR 값 가져오기 ===================== +void get_cached_ir(int* r1, int* r2, int* r3, int* r4, int* r5) { + noInterrupts(); + *r1 = cached_ir[0]; + *r2 = cached_ir[1]; + *r3 = cached_ir[2]; + *r4 = cached_ir[3]; + *r5 = cached_ir[4]; + interrupts(); +} + +// ===================== 함수 선언 ===================== +void start_motion(float angle_deg, float dist_mm); +void handle_udp_packet(); +void control_loop(int r1, int r2, int r3, int r4, int r5); + +void setup() { + Serial.begin(115200); + + Mona_ESP_init(); + + // Core 3.x 대응: 인터럽트 설정 + pinMode(PIN_ENCODER_LEFT, INPUT); + pinMode(PIN_ENCODER_RIGHT, INPUT); + attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_LEFT), isr_left_encoder, RISING); + attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_RIGHT), isr_right_encoder, RISING); + + // WiFi 연결 + WiFi.mode(WIFI_STA); + WiFi.begin(ssid, password); + while (WiFi.status() != WL_CONNECTED) { + delay(500); Serial.print("."); + } + Serial.println("\nWiFi connected"); + Serial.println(WiFi.localIP()); + + udp.begin(localPort); + + // Core 0에서 IR 센서 읽기 태스크 시작 + xTaskCreatePinnedToCore( + ir_sensor_task, // 태스크 함수 + "IR_Task", // 태스크 이름 + 4096, // 스택 크기 + NULL, // 파라미터 + 1, // 우선순위 + &irTaskHandle, // 태스크 핸들 + 0 // Core 0에서 실행 + ); + + // 초기 LED 상태 + Set_LED(1, 0, 30, 0); + Set_LED(2, 0, 30, 0); + + Serial.println("MONA ready with dual-core optimization!"); +} + +void loop() { + unsigned long now = millis(); + + // UDP 패킷 처리 + handle_udp_packet(); + + // 제어 주기 확인 (5ms 간격) + if (now - last_control_time >= CONTROL_INTERVAL_MS) { + last_control_time = now; + + // 캐시된 IR 값 사용 + int r1, r2, r3, r4, r5; + get_cached_ir(&r1, &r2, &r3, &r4, &r5); + + control_loop(r1, r2, r3, r4, r5); + } + + // LED 업데이트 (100ms 간격) + if (now - last_led_update >= LED_UPDATE_INTERVAL_MS) { + last_led_update = now; + update_status_led(); + } +} + +void start_motion(float angle_deg, float dist_mm) { + if (state == STATE_AVOID || state == STATE_ESCAPING || state == STATE_EMERGENCY) { + return; + } + + start_left_count = left_encoder_count; + start_right_count = right_encoder_count; + integral_error = 0.0f; + + target_turn_pulses = (long)lroundf(fabsf(angle_deg) * PULSES_PER_DEGREE); + target_move_pulses = (long)lroundf(fabsf(dist_mm) * PULSES_PER_MM); + + if (target_turn_pulses > 0 && fabsf(angle_deg) > (ROTATION_DEADBAND_DEG)) { + state = STATE_TURNING; + if (angle_deg > 0) Motors_spin_right(TURN_SPD); + else Motors_spin_left(TURN_SPD); + } else if (target_move_pulses > 0) { + state = STATE_MOVING; + } else { + state = STATE_IDLE; + Motors_stop(); + } +} + +void handle_udp_packet() { + int packetSize = udp.parsePacket(); + if (!packetSize) return; + + // 버퍼 플러시 + while (packetSize) { + int len = udp.read(packetBuffer, 255); + if (len > 0) packetBuffer[len] = 0; + packetSize = udp.parsePacket(); + } + + if (strncasecmp(packetBuffer, "STOP", 4) == 0) { + Motors_stop(); + clear_motion_targets(); + state = STATE_IDLE; + return; + } + + if (packetBuffer[0] == 'G' || packetBuffer[0] == 'g') { + float angle = 0, dist = 0; + if (sscanf(packetBuffer + 1, "%f %f", &angle, &dist) == 2) { + if (dist < MIN_DIST_MM) return; + + if (state == STATE_MOVING) { + long l_now = labs(left_encoder_count - start_left_count); + long r_now = labs(right_encoder_count - start_right_count); + if (((l_now + r_now) / 2) < UPDATE_THRESHOLD_PULSES) return; + } + start_motion(angle, dist); + } + } +} + +void control_loop(int r1, int r2, int r3, int r4, int r5) { + bool obstacle = (r1 > TH_OUTER) || (r2 > TH) || (r3 > TH) || (r4 > TH) || (r5 > TH_OUTER); + + if (obstacle) { + if (state != STATE_AVOID && state != STATE_ESCAPING && state != STATE_EMERGENCY) { + Motors_stop(); + clear_motion_targets(); + state = STATE_AVOID; + } + } + + if (state == STATE_EMERGENCY) { + unsigned long now = millis(); + if (now < emergency_back_until) { + Motors_backward(FWD_SPD); + return; + } + if (now < emergency_spin_until) { + Motors_spin_left(EMERGENCY_SPIN_SPD); + return; + } + Motors_stop(); + state = STATE_IDLE; + return; + } + + long l_now = labs(left_encoder_count - start_left_count); + long r_now = labs(right_encoder_count - start_right_count); + long avg = (l_now + r_now) / 2; + + switch (state) { + case STATE_TURNING: + if (avg >= target_turn_pulses) { + Motors_stop(); + if (target_move_pulses > 0) { + start_left_count = left_encoder_count; + start_right_count = right_encoder_count; + integral_error = 0.0f; + state = STATE_MOVING; + } else { + state = STATE_IDLE; + } + } + break; + + case STATE_MOVING: + if (avg >= target_move_pulses) { + Motors_stop(); + state = STATE_IDLE; + } else { + // 개선된 PI 제어 사용 + int left_pwm, right_pwm; + pi_control(l_now, r_now, &left_pwm, &right_pwm); + Left_mot_forward(left_pwm); + Right_mot_forward(right_pwm); + } + break; + + case STATE_AVOID: + if ((r1 <= TH_OUTER) && (r2 <= TH) && (r3 <= TH) && (r4 <= TH) && (r5 <= TH_OUTER)) { + enter_escaping(ESCAPING_MS); + break; + } + if (r3 >= TH) { + if (abs(r2 - r4) <= DELTA || r2 < r4) { + Motors_spin_left(TURN_SPD); + check_oscillation_and_escape(-1); + } else { + Motors_spin_right(TURN_SPD); + check_oscillation_and_escape(+1); + } + } else if (r1 >= TH_OUTER || r2 >= TH) { + Motors_spin_right(TURN_SPD); + check_oscillation_and_escape(+1); + } else if (r4 >= TH || r5 >= TH_OUTER) { + Motors_spin_left(TURN_SPD); + check_oscillation_and_escape(-1); + } + break; + + case STATE_ESCAPING: + if (obstacle) { + Motors_stop(); + state = STATE_AVOID; + break; + } + if ((int32_t)(millis() - escaping_until_ms) >= 0) { + Motors_stop(); + state = STATE_IDLE; + } + break; + + default: break; + } +}