From f73a08196938cc2b1c4b9b011d7a3fb015713b49 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 7 Jan 2026 13:20:59 +0900 Subject: [PATCH 01/10] SPACE_MONA_puppet example with UDP control and obstacle avoidance --- .../SPACE_MONA_puppet/SPACE_MONA_puppet.ino | 326 ++++++++++++++++++ 1 file changed, 326 insertions(+) create mode 100644 examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino 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..ccd62b5 --- /dev/null +++ b/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino @@ -0,0 +1,326 @@ +#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 +}; +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.95f; // 비례 게인 +static const float K_I = 0.01f; // 적분 게인 + +// 비상 동작 속도 +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 유지) ===================== +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; + +// ===================== 유틸리티 ===================== +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; +} + +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() 내부에 구형 LEDC 코드가 있다면 여기서 충돌이 날 수 있음 + 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.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); +} + +void loop() { + handle_udp_packet(); + + int r1 = Get_IR(1); + int r2 = Get_IR(2); + int r3 = Get_IR(3); + int r4 = Get_IR(4); + int r5 = Get_IR(5); + + control_loop(r1, r2, r3, r4, r5); + + delay(2); +} + +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 { + long err = l_now - r_now; + integral_error += err; + float u = (K_P * (float)err) + (K_I * (float)integral_error); + int left_pwm = constrain((int)lroundf(FWD_SPD - u), MIN_MOTOR_PWM, 255); + int right_pwm = constrain((int)lroundf(FWD_SPD + u), MIN_MOTOR_PWM, 255); + 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; + } +} From c8a2147869bf04f9deae1a2a03126ddfe6527efe Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 7 Jan 2026 13:23:03 +0900 Subject: [PATCH 02/10] SPACE_MONA_p2p example with ESP-NOW broadcast and TCP bridge --- examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino | 296 +++++++++++++++++++++ 1 file changed, 296 insertions(+) create mode 100644 examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino 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..0d2e95c --- /dev/null +++ b/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino @@ -0,0 +1,296 @@ +#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 = "11"; +const uint16_t SERVER_PORT = 8080; + +// JSON 버퍼는 넉넉하게(단, 실제 ESPNOW payload는 1470 제한) +const size_t JSON_SIZE = 2048; +const int TOTAL_ROBOTS = 12; +const uint32_t Min_Broadcast_MS = 50; +const uint32_t Max_Broadcast_MS = 100; +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 MONITORING_SEND_INTERVAL_MS = 50; +const uint32_t INITIAL_BROADCAST_INTERVAL_MS = 40; +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 = 0; + +// 큰 버퍼는 스택이 아니라 전역(static)로 (스택오버플로 방지) +static char g_jsonBuf[JSON_SIZE]; + +// ESPNOW 송신 패킷 고정 버퍼 +static uint8_t g_pkt[ESPNOW_MAX_PAYLOAD]; + +// ------------------------- +// Update map (safe & fast) +// ------------------------- +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; + } + CommRecvTime_MAP[senderID] = millis(); + + xSemaphoreGive(mapLock); + return true; +} + +// ===================================================== +// Core 3.x 변경: recv callback 시그니처 +// ===================================================== +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); + + // Core 3.x에서 권장: 인터페이스/채널 명시 + p.ifidx = WIFI_IF_STA; + p.channel = WiFi.channel(); // AP 연결된 채널과 동일해야 함 + p.encrypt = false; + + esp_err_t rc = esp_now_add_peer(&p); + if (rc != ESP_OK && rc != ESP_ERR_ESPNOW_EXIST) { + //Serial.printf("[ESPNOW] add_peer(bcast) failed: %d\n", (int)rc); + } +} + +void broadcastSelfMessageIfDue() { + static uint32_t nextInterval = (INITIAL_BROADCAST_INTERVAL_MS); + if (millis() - lastBroadcast < nextInterval) return; + + lastBroadcast = millis(); + nextInterval = random(Min_Broadcast_MS, Max_Broadcast_MS); + + if (selfMessageDoc.isNull()) return; + + ensureBroadcastPeer(); + + // JSON 직렬화 + 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; + + // v2 payload 한도(1470) 기준으로 체크 + if ((int)total > ESPNOW_MAX_PAYLOAD) { + return; + } + + // 고정 버퍼에 패킷 구성 [idLen][id][json] + 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); + } +} + +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()); + + // 실제 채널 출력 (ESPNOW는 이 채널과 동일해야 함) + 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(); + + // ESPNOW init (레거시 C API) + if (esp_now_init() != ESP_OK) { + Serial.println("[ESPNOW] init failed -> restart"); + ESP.restart(); + } + + // Core 3.x 콜백 등록 + esp_now_register_recv_cb(onEspNowRecv); + + ensureBroadcastPeer(); + + Serial.printf("[ESPNOW] Ready. Using max payload=%d (v2 confirmed in your environment)\n", ESPNOW_MAX_PAYLOAD); +} + +void setup() { + Serial.begin(115200); + delay(SERIAL_STABILIZE_DELAY_MS); + + randomSeed(esp_random()); + mapLock = xSemaphoreCreateMutex(); + + setupNetwork(); +} + +void loop() { + // Wi-Fi 끊김 복구 + if (WiFi.status() != WL_CONNECTED) { + for (auto &c : clients) c.stop(); + clients.clear(); + WiFi.reconnect(); + delay(10); + return; + } + + // 새 클라이언트 수락 (끊긴 슬롯 재사용) + 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); // 실패해도 조용히 무시(필요하면 로그 추가) + } + } + + // ESPNOW 브로드캐스트 + broadcastSelfMessageIfDue(); + + // TCP 모니터 송신 + static unsigned long lastTcpSend = 0; + if (millis() - lastTcpSend > (MONITORING_SEND_INTERVAL_MS)) { + DynamicJsonDocument monitor(JSON_SIZE * TOTAL_ROBOTS); + monitor["agent_id"] = SELF_ID; + JsonObject rx = monitor.createNestedObject("received_messages"); + + unsigned long now = millis(); + xSemaphoreTake(mapLock, portMAX_DELAY); + for (auto const &kv : receivedJSON_MAP) { + if (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); + } + } + + lastTcpSend = millis(); + } + + // 끊긴 클라이언트 정리 + 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()); + + delay(1); +} From 375cc9952213fbda8ec89f27e819543455173b46 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 7 Jan 2026 13:27:10 +0900 Subject: [PATCH 03/10] Baseline from puppet and p2p --- .../SPACE_MONA_offboard.ino | 624 ++++++++++++++++++ 1 file changed, 624 insertions(+) create mode 100644 examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino 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..5c9ac20 --- /dev/null +++ b/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino @@ -0,0 +1,624 @@ +#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 +}; +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.95f; // 비례 게인 +static const float K_I = 0.01f; // 적분 게인 + +// 비상 동작 속도 +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 유지) ===================== +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; + +// ===================== 유틸리티 ===================== +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; +} + +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() 내부에 구형 LEDC 코드가 있다면 여기서 충돌이 날 수 있음 + 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.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); +} + +void loop() { + handle_udp_packet(); + + int r1 = Get_IR(1); + int r2 = Get_IR(2); + int r3 = Get_IR(3); + int r4 = Get_IR(4); + int r5 = Get_IR(5); + + control_loop(r1, r2, r3, r4, r5); + + delay(2); +} + +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 { + long err = l_now - r_now; + integral_error += err; + float u = (K_P * (float)err) + (K_I * (float)integral_error); + int left_pwm = constrain((int)lroundf(FWD_SPD - u), MIN_MOTOR_PWM, 255); + int right_pwm = constrain((int)lroundf(FWD_SPD + u), MIN_MOTOR_PWM, 255); + 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; + } +} + +#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 = "11"; +const uint16_t SERVER_PORT = 8080; + +// JSON 버퍼는 넉넉하게(단, 실제 ESPNOW payload는 1470 제한) +const size_t JSON_SIZE = 2048; +const int TOTAL_ROBOTS = 12; +const uint32_t Min_Broadcast_MS = 50; +const uint32_t Max_Broadcast_MS = 100; +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 MONITORING_SEND_INTERVAL_MS = 50; +const uint32_t INITIAL_BROADCAST_INTERVAL_MS = 40; +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 = 0; + +// 큰 버퍼는 스택이 아니라 전역(static)로 (스택오버플로 방지) +static char g_jsonBuf[JSON_SIZE]; + +// ESPNOW 송신 패킷 고정 버퍼 +static uint8_t g_pkt[ESPNOW_MAX_PAYLOAD]; + +// ------------------------- +// Update map (safe & fast) +// ------------------------- +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; + } + CommRecvTime_MAP[senderID] = millis(); + + xSemaphoreGive(mapLock); + return true; +} + +// ===================================================== +// Core 3.x 변경: recv callback 시그니처 +// ===================================================== +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); + + // Core 3.x에서 권장: 인터페이스/채널 명시 + p.ifidx = WIFI_IF_STA; + p.channel = WiFi.channel(); // AP 연결된 채널과 동일해야 함 + p.encrypt = false; + + esp_err_t rc = esp_now_add_peer(&p); + if (rc != ESP_OK && rc != ESP_ERR_ESPNOW_EXIST) { + //Serial.printf("[ESPNOW] add_peer(bcast) failed: %d\n", (int)rc); + } +} + +void broadcastSelfMessageIfDue() { + static uint32_t nextInterval = (INITIAL_BROADCAST_INTERVAL_MS); + if (millis() - lastBroadcast < nextInterval) return; + + lastBroadcast = millis(); + nextInterval = random(Min_Broadcast_MS, Max_Broadcast_MS); + + if (selfMessageDoc.isNull()) return; + + ensureBroadcastPeer(); + + // JSON 직렬화 + 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; + + // v2 payload 한도(1470) 기준으로 체크 + if ((int)total > ESPNOW_MAX_PAYLOAD) { + return; + } + + // 고정 버퍼에 패킷 구성 [idLen][id][json] + 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); + } +} + +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()); + + // 실제 채널 출력 (ESPNOW는 이 채널과 동일해야 함) + 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(); + + // ESPNOW init (레거시 C API) + if (esp_now_init() != ESP_OK) { + Serial.println("[ESPNOW] init failed -> restart"); + ESP.restart(); + } + + // Core 3.x 콜백 등록 + esp_now_register_recv_cb(onEspNowRecv); + + ensureBroadcastPeer(); + + Serial.printf("[ESPNOW] Ready. Using max payload=%d (v2 confirmed in your environment)\n", ESPNOW_MAX_PAYLOAD); +} + +void setup() { + Serial.begin(115200); + delay(SERIAL_STABILIZE_DELAY_MS); + + randomSeed(esp_random()); + mapLock = xSemaphoreCreateMutex(); + + setupNetwork(); +} + +void loop() { + // Wi-Fi 끊김 복구 + if (WiFi.status() != WL_CONNECTED) { + for (auto &c : clients) c.stop(); + clients.clear(); + WiFi.reconnect(); + delay(10); + return; + } + + // 새 클라이언트 수락 (끊긴 슬롯 재사용) + 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); // 실패해도 조용히 무시(필요하면 로그 추가) + } + } + + // ESPNOW 브로드캐스트 + broadcastSelfMessageIfDue(); + + // TCP 모니터 송신 + static unsigned long lastTcpSend = 0; + if (millis() - lastTcpSend > (MONITORING_SEND_INTERVAL_MS)) { + DynamicJsonDocument monitor(JSON_SIZE * TOTAL_ROBOTS); + monitor["agent_id"] = SELF_ID; + JsonObject rx = monitor.createNestedObject("received_messages"); + + unsigned long now = millis(); + xSemaphoreTake(mapLock, portMAX_DELAY); + for (auto const &kv : receivedJSON_MAP) { + if (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); + } + } + + lastTcpSend = millis(); + } + + // 끊긴 클라이언트 정리 + 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()); + + delay(1); +} From 5c647eda301bfd787dcd910a758f0fce1cb0e160 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Thu, 8 Jan 2026 13:23:56 +0900 Subject: [PATCH 04/10] Implement dual-core architecture and enhance ESP-NOW communication --- .../SPACE_MONA_offboard.ino | 700 ++++++++++-------- 1 file changed, 401 insertions(+), 299 deletions(-) diff --git a/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino b/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino index 5c9ac20..5533d91 100644 --- a/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino +++ b/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino @@ -2,14 +2,29 @@ #include #include "Mona_ESP_lib.h" #include -#include // strncasecmp를 위한 헤더 추가 (Core 3.x 대응) +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -// ===================== WiFi / UDP ===================== -const char* ssid = "InMOLab"; -const char* password = "dlsahfoq104!"; -WiFiUDP udp; -const int localPort = 8080; -char packetBuffer[255]; +// ====== 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 uint16_t UDP_PORT = 8080; + +// JSON buffer size +const size_t JSON_SIZE = 2048; // ===================== 상태 정의 ===================== enum RobotState { @@ -22,6 +37,21 @@ enum RobotState { }; RobotState state = STATE_IDLE; +// ESP-NOW settings +const int TOTAL_ROBOTS = 12; +const uint32_t MIN_BROADCAST_MS = 50; +const uint32_t MAX_BROADCAST_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 MONITORING_SEND_INTERVAL_MS = 50; +const uint32_t INITIAL_BROADCAST_INTERVAL_MS = 40; +const uint32_t SERIAL_STABILIZE_DELAY_MS = 1000; + +static const int ESPNOW_MAX_PAYLOAD = 1470; + + // 펄스/제어 상수 static const float PULSES_PER_MM = 18.0f; static const float PULSES_PER_DEGREE = 12.8f; @@ -67,7 +97,36 @@ unsigned long oscillation_timer_start = 0; unsigned long emergency_back_until = 0; unsigned long emergency_spin_until = 0; -// ===================== 엔코더 (volatile 유지) ===================== +// ===== Network (Core 0) ===== +WiFiServer tcpServer(TCP_PORT); +WiFiUDP udp; +std::vector tcpClients; + +// ===== CBBA Communication (Core 0) ===== +DynamicJsonDocument selfMessageDoc(JSON_SIZE); +std::map receivedJSON_MAP; +std::map CommRecvTime_MAP; +SemaphoreHandle_t mapLock; +SemaphoreHandle_t selfMsgLock; // NEW: selfMessageDoc 보호 + +unsigned long lastBroadcast = 0; +volatile bool dirtySelf = false; +volatile bool dirtyNeighbors = false; + +// ===== ESP-NOW counters ===== +volatile uint32_t espnow_rx_bytes = 0; +volatile uint32_t espnow_tx_bytes = 0; + +// Global JSON buffer +static char g_jsonBuf[JSON_SIZE]; +static uint8_t g_pkt[ESPNOW_MAX_PAYLOAD]; + +// ===== Motion Control (Core 1) - Atomic variables for cross-core sharing ===== +volatile float targetAngleDeg = 0; +volatile float targetDistMm = 0; +volatile bool newMotionCommand = false; // Core 0 → Core 1 signal +volatile bool stopRequested = false; + volatile long left_encoder_count = 0; volatile long right_encoder_count = 0; @@ -81,7 +140,9 @@ static long target_turn_pulses = 0; static long target_move_pulses = 0; static float integral_error = 0.0f; -// ===================== 유틸리티 ===================== +TaskHandle_t commTaskHandle = NULL; +TaskHandle_t motionTaskHandle = NULL; + static inline void clear_motion_targets() { target_turn_pulses = 0; target_move_pulses = 0; @@ -129,47 +190,265 @@ static inline void check_oscillation_and_escape(int current_direction) { last_turn_direction = current_direction; } -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); +// ============================================================================= +// ESP-NOW COMMUNICATION (Core 0) +// ============================================================================= +bool update_Broadcast_recv_JSON_MAP(const String& senderID, const char* jsonBuf, size_t jsonLen) { + if (jsonLen < 2) return false; -void setup() { - Serial.begin(115200); + // 콜백에서 풀 파싱은 부담 큼 -> 아주 가벼운 형태 체크만 + const char first = jsonBuf[0]; + const char last = jsonBuf[jsonLen - 1]; + if (!((first == '{' && last == '}') || (first == '[' && last == ']'))) { + return false; + } + + // 수신 콜백에서 오래 잠그지 않기: 즉시 락 실패 시 드랍 + if (xSemaphoreTake(mapLock, 0) != pdTRUE) { + return false; + } + + // 기존 String capacity 재사용(힙 단편화 완화) + String& dst = receivedJSON_MAP[senderID]; + dst.remove(0); + dst.reserve(jsonLen + 1); + dst.concat(jsonBuf, jsonLen); + + 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; + + if (update_Broadcast_recv_JSON_MAP(senderID, (const char*)(&incoming[1 + idLen]), (size_t)jsonLen)) { + espnow_rx_bytes += (uint32_t)len; + dirtyNeighbors = true; + } +} + +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() { + static uint32_t nextInterval = 40; + if (millis() - lastBroadcast < nextInterval) return; + + lastBroadcast = millis(); + nextInterval = random(MIN_BROADCAST_MS, MAX_BROADCAST_MS); + + // Lock selfMessageDoc for reading + if (xSemaphoreTake(selfMsgLock, pdMS_TO_TICKS(5)) != pdTRUE) return; - // Mona_ESP_init() 내부에 구형 LEDC 코드가 있다면 여기서 충돌이 날 수 있음 - Mona_ESP_init(); + if (selfMessageDoc.isNull() || selfMessageDoc.size() == 0) { + xSemaphoreGive(selfMsgLock); + return; + } - // 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); + ensureBroadcastPeer(); - WiFi.mode(WIFI_STA); - WiFi.begin(ssid, password); - while (WiFi.status() != WL_CONNECTED) { - delay(500); Serial.print("."); + size_t jsonLen = serializeJson(selfMessageDoc, g_jsonBuf, sizeof(g_jsonBuf)); + xSemaphoreGive(selfMsgLock); + + if (jsonLen == 0) return; + + uint8_t idLen = (uint8_t)SELF_ID.length(); + 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); + espnow_tx_bytes += (uint32_t)total; +} + +// ============================================================================= +// TCP COMMUNICATION (Core 0) +// ============================================================================= +void handleTcpClients() { + WiFiClient newcomer = tcpServer.available(); + if (newcomer) { + newcomer.setTimeout(10); + newcomer.setNoDelay(true); + + bool placed = false; + for (auto &c : tcpClients) { + if (!c || !c.connected()) { + c.stop(); + c = newcomer; + placed = true; + break; + } + } + if (!placed) tcpClients.push_back(newcomer); } - Serial.println("\nWiFi connected"); - Serial.println(WiFi.localIP()); - udp.begin(localPort); + for (auto &c : tcpClients) { + if (c && c.connected() && c.available()) { + String line = c.readStringUntil('\n'); + + // Lock selfMessageDoc for writing + if (xSemaphoreTake(selfMsgLock, pdMS_TO_TICKS(10)) == pdTRUE) { + if (deserializeJson(selfMessageDoc, line) == DeserializationError::Ok) { + dirtySelf = true; + } + xSemaphoreGive(selfMsgLock); + } + } + } } -void loop() { - handle_udp_packet(); +void sendMonitorToTcpClients() { + static unsigned long lastTcpSend = 0; + if (millis() - lastTcpSend < 50) return; + + // [FIX #1] 큰 DynamicJsonDocument 제거: + // mapLock을 짧게 잡고 스냅샷만 뜬 뒤, 락 풀고 String으로 한 줄 JSON 구성 + std::vector> snapshot; + snapshot.reserve(16); - int r1 = Get_IR(1); - int r2 = Get_IR(2); - int r3 = Get_IR(3); - int r4 = Get_IR(4); - int r5 = Get_IR(5); + const unsigned long now = millis(); - control_loop(r1, r2, r3, r4, r5); + if (xSemaphoreTake(mapLock, pdMS_TO_TICKS(5)) == pdTRUE) { + for (auto const &kv : receivedJSON_MAP) { + auto itT = CommRecvTime_MAP.find(kv.first); + if (itT != CommRecvTime_MAP.end() && (now - itT->second <= PEER_LINK_DROP_MS)) { + snapshot.push_back({kv.first, kv.second}); // (senderID, raw json) + } + } + xSemaphoreGive(mapLock); + } + + // 한 줄 JSON: {"agent_id":"11","received_messages":{"03":{...},"04":{...}}} + size_t est = 64; + for (auto &kv : snapshot) est += 6 + kv.first.length() + kv.second.length(); + + String out; + out.reserve(est); + out += "{\"agent_id\":\""; + out += SELF_ID; + out += "\",\"received_messages\":{"; + + bool first = true; + for (auto &kv : snapshot) { + if (!first) out += ","; + first = false; + + out += "\""; + out += kv.first; + out += "\":"; + out += kv.second; // raw JSON object/array + } + + out += "}}\n"; + + for (auto &c : tcpClients) { + if (c && c.connected()) { + c.print(out); + } + } - delay(2); + dirtySelf = dirtyNeighbors = false; + lastTcpSend = millis(); + + // Cleanup disconnected clients + for (auto &c : tcpClients) { + if (c && !c.connected()) c.stop(); + } + tcpClients.erase(std::remove_if(tcpClients.begin(), tcpClients.end(), + [](WiFiClient& c) { return !c.connected(); }), tcpClients.end()); } +// ============================================================================= +// UDP MOTION COMMANDS (Core 0 receives, Core 1 executes) +// ============================================================================= +void handleUdpPacket() { + static char udpBuffer[256]; + + int packetSize = udp.parsePacket(); + if (!packetSize) return; + + // Keep only latest packet + while (packetSize) { + int len = udp.read(udpBuffer, 255); + if (len > 0) udpBuffer[len] = 0; + packetSize = udp.parsePacket(); + } + + // STOP command + if (strncasecmp(udpBuffer, "STOP", 4) == 0) { + stopRequested = true; + return; + } + + // G command: "G " + if (udpBuffer[0] == 'G' || udpBuffer[0] == 'g') { + float angle = 0, dist = 0; + if (sscanf(udpBuffer + 1, "%f %f", &angle, &dist) == 2) { + if (dist >= MIN_DIST_MM) { + targetAngleDeg = angle; + targetDistMm = dist; + newMotionCommand = true; // Signal to Core 1 + } + } + } +} + +// ============================================================================= +// COMMUNICATION TASK (Core 0) - HIGH PRIORITY +// ============================================================================= +void commTask(void* parameter) { + Serial.println("[Core 0] Communication task started"); + + for (;;) { + // WiFi reconnection + if (WiFi.status() != WL_CONNECTED) { + for (auto &c : tcpClients) c.stop(); + tcpClients.clear(); + WiFi.reconnect(); + vTaskDelay(pdMS_TO_TICKS(100)); + continue; + } + + // ===== All communication handling ===== + handleTcpClients(); // TCP: PC <-> Robot + broadcastSelfMessageIfDue(); // ESP-NOW: Robot <-> Robot + sendMonitorToTcpClients(); // Send monitor to PC + handleUdpPacket(); // UDP: Motion commands + + // Short delay to prevent watchdog trigger + vTaskDelay(pdMS_TO_TICKS(1)); + } +} + +// ============================================================================= +// MOTION CONTROL TASK (Core 1) - INDEPENDENT +// ============================================================================= void start_motion(float angle_deg, float dist_mm) { if (state == STATE_AVOID || state == STATE_ESCAPING || state == STATE_EMERGENCY) { return; @@ -194,39 +473,6 @@ void start_motion(float angle_deg, float dist_mm) { } } -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); @@ -325,161 +571,45 @@ void control_loop(int r1, int r2, int r3, int r4, int r5) { } } -#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 = "11"; -const uint16_t SERVER_PORT = 8080; - -// JSON 버퍼는 넉넉하게(단, 실제 ESPNOW payload는 1470 제한) -const size_t JSON_SIZE = 2048; -const int TOTAL_ROBOTS = 12; -const uint32_t Min_Broadcast_MS = 50; -const uint32_t Max_Broadcast_MS = 100; -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 MONITORING_SEND_INTERVAL_MS = 50; -const uint32_t INITIAL_BROADCAST_INTERVAL_MS = 40; -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 = 0; - -// 큰 버퍼는 스택이 아니라 전역(static)로 (스택오버플로 방지) -static char g_jsonBuf[JSON_SIZE]; - -// ESPNOW 송신 패킷 고정 버퍼 -static uint8_t g_pkt[ESPNOW_MAX_PAYLOAD]; - -// ------------------------- -// Update map (safe & fast) -// ------------------------- -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; - } - CommRecvTime_MAP[senderID] = millis(); - - xSemaphoreGive(mapLock); - return true; -} - -// ===================================================== -// Core 3.x 변경: recv callback 시그니처 -// ===================================================== -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); - - // Core 3.x에서 권장: 인터페이스/채널 명시 - p.ifidx = WIFI_IF_STA; - p.channel = WiFi.channel(); // AP 연결된 채널과 동일해야 함 - p.encrypt = false; - - esp_err_t rc = esp_now_add_peer(&p); - if (rc != ESP_OK && rc != ESP_ERR_ESPNOW_EXIST) { - //Serial.printf("[ESPNOW] add_peer(bcast) failed: %d\n", (int)rc); - } -} - -void broadcastSelfMessageIfDue() { - static uint32_t nextInterval = (INITIAL_BROADCAST_INTERVAL_MS); - if (millis() - lastBroadcast < nextInterval) return; - - lastBroadcast = millis(); - nextInterval = random(Min_Broadcast_MS, Max_Broadcast_MS); - - if (selfMessageDoc.isNull()) return; - - ensureBroadcastPeer(); - - // JSON 직렬화 - size_t jsonLen = serializeJson(selfMessageDoc, g_jsonBuf, sizeof(g_jsonBuf)); - if (jsonLen == 0) return; +void motionTask(void* parameter) { + Serial.println("[Core 1] Motion control task started"); + + for (;;) { + // Check for STOP command from Core 0 + if (stopRequested) { + Motors_stop(); + clear_motion_targets(); + state = STATE_IDLE; + stopRequested = false; + } - const uint8_t idLen = (uint8_t)SELF_ID.length(); - const size_t total = 1 + (size_t)idLen + jsonLen; + // Check for new motion command from Core 0 + if (newMotionCommand) { + // Only accept if not in obstacle avoidance + 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) { + start_motion(targetAngleDeg, targetDistMm); + } + } else if (state == STATE_IDLE || state == STATE_TURNING) { + start_motion(targetAngleDeg, targetDistMm); + } + newMotionCommand = false; + } - // v2 payload 한도(1470) 기준으로 체크 - if ((int)total > ESPNOW_MAX_PAYLOAD) { - return; - } + // Read IR sensors (this is slow, but doesn't block Core 0 now!) + int r1 = Get_IR(1); + int r2 = Get_IR(2); + int r3 = Get_IR(3); + int r4 = Get_IR(4); + int r5 = Get_IR(5); - // 고정 버퍼에 패킷 구성 [idLen][id][json] - g_pkt[0] = idLen; - memcpy(&g_pkt[1], SELF_ID.c_str(), idLen); - memcpy(&g_pkt[1 + idLen], g_jsonBuf, jsonLen); + // Run motion control + control_loop(r1, r2, r3, r4, r5); - 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); + // Motion task runs at ~50Hz (20ms period) + vTaskDelay(pdMS_TO_TICKS(50)); } } @@ -519,9 +649,15 @@ void setupNetwork() { esp_wifi_get_channel(&primary, &second); Serial.printf("[WiFi] Channel: %d\n", primary); - server.begin(); + // Start TCP server + tcpServer.begin(); + Serial.printf("[TCP] Server on port %d\n", TCP_PORT); + + // Start UDP + udp.begin(UDP_PORT); + Serial.printf("[UDP] Server on port %d\n", UDP_PORT); - // ESPNOW init (레거시 C API) + // Initialize ESP-NOW if (esp_now_init() != ESP_OK) { Serial.println("[ESPNOW] init failed -> restart"); ESP.restart(); @@ -541,84 +677,50 @@ void setup() { randomSeed(esp_random()); mapLock = xSemaphoreCreateMutex(); + selfMsgLock = xSemaphoreCreateMutex(); - setupNetwork(); -} - -void loop() { - // Wi-Fi 끊김 복구 - if (WiFi.status() != WL_CONNECTED) { - for (auto &c : clients) c.stop(); - clients.clear(); - WiFi.reconnect(); - delay(10); - return; - } - - // 새 클라이언트 수락 (끊긴 슬롯 재사용) - 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); // 실패해도 조용히 무시(필요하면 로그 추가) - } - } - - // ESPNOW 브로드캐스트 - broadcastSelfMessageIfDue(); - - // TCP 모니터 송신 - static unsigned long lastTcpSend = 0; - if (millis() - lastTcpSend > (MONITORING_SEND_INTERVAL_MS)) { - DynamicJsonDocument monitor(JSON_SIZE * TOTAL_ROBOTS); - monitor["agent_id"] = SELF_ID; - JsonObject rx = monitor.createNestedObject("received_messages"); - - unsigned long now = millis(); - xSemaphoreTake(mapLock, portMAX_DELAY); - for (auto const &kv : receivedJSON_MAP) { - if (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); - } - } + // Initialize Mona robot hardware + Mona_ESP_init(); - lastTcpSend = millis(); - } + // Setup encoders + 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); - // 끊긴 클라이언트 정리 - 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()); + setupNetwork(); - delay(1); + Serial.println("================================="); + Serial.println("MONA Firmware v2 - Dual Core"); + Serial.println("- Core 0: Communication (TCP/ESP-NOW/UDP)"); + Serial.println("- Core 1: Motion Control (IR/Motors)"); + Serial.println("================================="); + + // Create Communication Task on Core 0 (PRO_CPU) + // Higher priority (2) than default Arduino loop + xTaskCreatePinnedToCore( + commTask, // Task function + "CommTask", // Name + 8192, // Stack size + NULL, // Parameters + 2, // Priority (higher = more important) + &commTaskHandle, // Task handle + 0 // Core 0 + ); + + // Create Motion Task on Core 1 (APP_CPU) + // Lower priority (1), independent from communication + xTaskCreatePinnedToCore( + motionTask, // Task function + "MotionTask", // Name + 4096, // Stack size + NULL, // Parameters + 1, // Priority + &motionTaskHandle, // Task handle + 1 // Core 1 + ); } + +void loop() { + vTaskDelay(pdMS_TO_TICKS(1000)); +} \ No newline at end of file From 645f6117d44f66cb19eb67d9b932a00680c49bbe Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Sun, 18 Jan 2026 15:27:00 +0900 Subject: [PATCH 05/10] Add MONA_puppet micropython scripts --- Micropython/MONA_puppet/ads7830.py | 178 ++++++ Micropython/MONA_puppet/main.py | 2 + Micropython/MONA_puppet/mcp23008.py | 133 +++++ Micropython/MONA_puppet/mona_esp_lib.py | 357 ++++++++++++ .../MONA_puppet/mona_udp_controller.py | 521 ++++++++++++++++++ Micropython/MONA_puppet/neopixel_led.py | 61 ++ 6 files changed, 1252 insertions(+) create mode 100644 Micropython/MONA_puppet/ads7830.py create mode 100644 Micropython/MONA_puppet/main.py create mode 100644 Micropython/MONA_puppet/mcp23008.py create mode 100644 Micropython/MONA_puppet/mona_esp_lib.py create mode 100644 Micropython/MONA_puppet/mona_udp_controller.py create mode 100644 Micropython/MONA_puppet/neopixel_led.py 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() From 002deef559820bc2e9a8f29c556046bfa0aa5e11 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Mon, 19 Jan 2026 18:02:59 +0900 Subject: [PATCH 06/10] Implement dual-core architecture and optimize control loop --- .../SPACE_MONA_puppet/SPACE_MONA_puppet.ino | 168 +++++++++++++++--- 1 file changed, 148 insertions(+), 20 deletions(-) diff --git a/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino b/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino index ccd62b5..f63b34b 100644 --- a/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino +++ b/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino @@ -20,7 +20,7 @@ enum RobotState { STATE_ESCAPING, STATE_EMERGENCY }; -RobotState state = STATE_IDLE; +volatile RobotState state = STATE_IDLE; // 펄스/제어 상수 static const float PULSES_PER_MM = 18.0f; @@ -45,9 +45,15 @@ 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.95f; // 비례 게인 -static const float K_I = 0.01f; // 적분 게인 +// ===================== 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; @@ -67,7 +73,7 @@ unsigned long oscillation_timer_start = 0; unsigned long emergency_back_until = 0; unsigned long emergency_spin_until = 0; -// ===================== 엔코더 (volatile 유지) ===================== +// ===================== 엔코더 ===================== volatile long left_encoder_count = 0; volatile long right_encoder_count = 0; @@ -81,6 +87,19 @@ 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; @@ -129,6 +148,90 @@ static inline void check_oscillation_and_escape(int current_direction) { 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); @@ -136,7 +239,6 @@ void control_loop(int r1, int r2, int r3, int r4, int r5); void setup() { Serial.begin(115200); - // Mona_ESP_init() 내부에 구형 LEDC 코드가 있다면 여기서 충돌이 날 수 있음 Mona_ESP_init(); // Core 3.x 대응: 인터럽트 설정 @@ -145,6 +247,7 @@ void setup() { 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) { @@ -154,20 +257,47 @@ void setup() { 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(); - int r1 = Get_IR(1); - int r2 = Get_IR(2); - int r3 = Get_IR(3); - int r4 = Get_IR(4); - int r5 = Get_IR(5); - - control_loop(r1, r2, r3, r4, r5); - - delay(2); + // 제어 주기 확인 (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) { @@ -277,11 +407,9 @@ void control_loop(int r1, int r2, int r3, int r4, int r5) { Motors_stop(); state = STATE_IDLE; } else { - long err = l_now - r_now; - integral_error += err; - float u = (K_P * (float)err) + (K_I * (float)integral_error); - int left_pwm = constrain((int)lroundf(FWD_SPD - u), MIN_MOTOR_PWM, 255); - int right_pwm = constrain((int)lroundf(FWD_SPD + u), MIN_MOTOR_PWM, 255); + // 개선된 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); } From 1552a740cfbf133a6025d1e4e0da0780d306d154 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 11 Feb 2026 16:26:29 +0900 Subject: [PATCH 07/10] =?UTF-8?q?=EB=88=84=EC=A0=81=20=EC=98=A4=EC=B0=A8?= =?UTF-8?q?=20=EB=B0=A9=EC=A7=80=20=EC=B6=94=EA=B0=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino | 238 +++++++++++++++++---- 1 file changed, 193 insertions(+), 45 deletions(-) diff --git a/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino b/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino index 0d2e95c..35972ba 100644 --- a/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino +++ b/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino @@ -20,14 +20,18 @@ const uint16_t SERVER_PORT = 8080; // JSON 버퍼는 넉넉하게(단, 실제 ESPNOW payload는 1470 제한) const size_t JSON_SIZE = 2048; const int TOTAL_ROBOTS = 12; -const uint32_t Min_Broadcast_MS = 50; -const uint32_t Max_Broadcast_MS = 100; + +// ====== TIMING CONSTANTS (누적 오차 방지 방식) ====== +const uint32_t BROADCAST_MIN_INTERVAL_MS = 20; +const uint32_t BROADCAST_MAX_INTERVAL_MS = 40; +const uint32_t TCP_MONITORING_INTERVAL_MS = 50; +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 MONITORING_SEND_INTERVAL_MS = 50; -const uint32_t INITIAL_BROADCAST_INTERVAL_MS = 40; const uint32_t SERIAL_STABILIZE_DELAY_MS = 1000; // ESPNOW v2 payload 최대 @@ -42,16 +46,69 @@ std::map CommRecvTime_MAP; SemaphoreHandle_t mapLock; -unsigned long lastBroadcast = 0; +// ====== 누적 오차 방지를 위한 타이밍 변수 ====== +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)로 (스택오버플로 방지) static char g_jsonBuf[JSON_SIZE]; // ESPNOW 송신 패킷 고정 버퍼 static uint8_t g_pkt[ESPNOW_MAX_PAYLOAD]; // ------------------------- -// Update map (safe & fast) +// 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); @@ -73,6 +130,7 @@ bool update_Broadcast_recv_JSON_MAP(const String& senderID, const char* jsonBuf, it->second = doc; } else { receivedJSON_MAP[senderID] = doc; + currentMemoryUsage_bytes += JSON_SIZE; } CommRecvTime_MAP[senderID] = millis(); @@ -121,11 +179,18 @@ void ensureBroadcastPeer() { } void broadcastSelfMessageIfDue() { - static uint32_t nextInterval = (INITIAL_BROADCAST_INTERVAL_MS); - if (millis() - lastBroadcast < nextInterval) return; + unsigned long now = millis(); + + // 누적 오차 방지: (now - lastBroadcast_ms) 사용 + if ((unsigned long)(now - lastBroadcast_ms) < currentBroadcastInterval_ms) { + return; + } - lastBroadcast = millis(); - nextInterval = random(Min_Broadcast_MS, Max_Broadcast_MS); + // 누적 오차 방지: lastBroadcast_ms += interval + lastBroadcast_ms += currentBroadcastInterval_ms; + + // 다음 간격 설정 (충돌 회피를 위한 랜덤화) + currentBroadcastInterval_ms = random(BROADCAST_MIN_INTERVAL_MS, BROADCAST_MAX_INTERVAL_MS); if (selfMessageDoc.isNull()) return; @@ -155,6 +220,83 @@ void broadcastSelfMessageIfDue() { } } +// ------------------------- +// 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); @@ -171,7 +313,7 @@ void setupNetwork() { while (WiFi.status() != WL_CONNECTED) { delay(WIFI_RECONNECT_INTERVAL_MS); Serial.print("."); - if (millis() - t0 > (WIFI_TIMEOUT_MS)) { + if (millis() - t0 > WIFI_TIMEOUT_MS) { Serial.println("\n[WiFi] connect timeout -> retry"); WiFi.disconnect(true, true); delay(WIFI_RETRY_DELAY_MS); @@ -214,16 +356,31 @@ void setup() { 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(); } void loop() { // Wi-Fi 끊김 복구 + unsigned long loopStart_us = micros(); if (WiFi.status() != WL_CONNECTED) { for (auto &c : clients) c.stop(); clients.clear(); WiFi.reconnect(); - delay(10); + yield(); return; } @@ -255,42 +412,33 @@ void loop() { // ESPNOW 브로드캐스트 broadcastSelfMessageIfDue(); - - // TCP 모니터 송신 - static unsigned long lastTcpSend = 0; - if (millis() - lastTcpSend > (MONITORING_SEND_INTERVAL_MS)) { - DynamicJsonDocument monitor(JSON_SIZE * TOTAL_ROBOTS); - monitor["agent_id"] = SELF_ID; - JsonObject rx = monitor.createNestedObject("received_messages"); - - unsigned long now = millis(); - xSemaphoreTake(mapLock, portMAX_DELAY); - for (auto const &kv : receivedJSON_MAP) { - if (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); - } - } - - lastTcpSend = millis(); - } - - // 끊긴 클라이언트 정리 + + // 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()); - delay(1); + // ========== 루프 시간 측정 ========== + 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(); } From 1a4cffe7ba25ca79b7b7724bd7e916da6f87d54c Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Mon, 23 Feb 2026 19:39:46 +0900 Subject: [PATCH 08/10] =?UTF-8?q?Broadcast,=20TCP=20=EC=A3=BC=EA=B8=B0=20?= =?UTF-8?q?=EB=B3=80=EA=B2=BD=20=EB=B0=8F=20=EC=A3=BC=EC=84=9D=20=EC=A0=9C?= =?UTF-8?q?=EA=B1=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino | 49 +++++++++------------- 1 file changed, 19 insertions(+), 30 deletions(-) diff --git a/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino b/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino index 35972ba..8feaf6a 100644 --- a/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino +++ b/examples/SPACE_MONA_p2p/SPACE_MONA_p2p.ino @@ -14,7 +14,7 @@ // ====== USER CONFIG ====== const char* SSID = "InMOLab"; const char* PASSWORD = "dlsahfoq104!"; -const String SELF_ID = "11"; +const String SELF_ID = "Your SELF_ID"; const uint16_t SERVER_PORT = 8080; // JSON 버퍼는 넉넉하게(단, 실제 ESPNOW payload는 1470 제한) @@ -22,9 +22,9 @@ const size_t JSON_SIZE = 2048; const int TOTAL_ROBOTS = 12; // ====== TIMING CONSTANTS (누적 오차 방지 방식) ====== -const uint32_t BROADCAST_MIN_INTERVAL_MS = 20; -const uint32_t BROADCAST_MAX_INTERVAL_MS = 40; -const uint32_t TCP_MONITORING_INTERVAL_MS = 50; +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; @@ -64,8 +64,6 @@ size_t currentMemoryUsage_bytes = 0; size_t peakMemoryUsage_bytes = 0; static char g_jsonBuf[JSON_SIZE]; - -// ESPNOW 송신 패킷 고정 버퍼 static uint8_t g_pkt[ESPNOW_MAX_PAYLOAD]; // ------------------------- @@ -118,7 +116,6 @@ bool update_Broadcast_recv_JSON_MAP(const String& senderID, const char* jsonBuf, return false; } - // 수신 콜백에서 오래 잠그지 않기: 즉시 락 실패 시 드랍 if (xSemaphoreTake(mapLock, 0) != pdTRUE) { delete doc; return false; @@ -138,9 +135,6 @@ bool update_Broadcast_recv_JSON_MAP(const String& senderID, const char* jsonBuf, return true; } -// ===================================================== -// Core 3.x 변경: recv callback 시그니처 -// ===================================================== void onEspNowRecv(const esp_now_recv_info_t *recv_info, const uint8_t *incoming, int len) { (void)recv_info; @@ -166,15 +160,12 @@ void ensureBroadcastPeer() { esp_now_peer_info_t p = {}; memcpy(p.peer_addr, bcast, 6); - - // Core 3.x에서 권장: 인터페이스/채널 명시 p.ifidx = WIFI_IF_STA; - p.channel = WiFi.channel(); // AP 연결된 채널과 동일해야 함 + 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) { - //Serial.printf("[ESPNOW] add_peer(bcast) failed: %d\n", (int)rc); } } @@ -196,19 +187,16 @@ void broadcastSelfMessageIfDue() { ensureBroadcastPeer(); - // JSON 직렬화 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; - // v2 payload 한도(1470) 기준으로 체크 if ((int)total > ESPNOW_MAX_PAYLOAD) { return; } - // 고정 버퍼에 패킷 구성 [idLen][id][json] g_pkt[0] = idLen; memcpy(&g_pkt[1], SELF_ID.c_str(), idLen); memcpy(&g_pkt[1 + idLen], g_jsonBuf, jsonLen); @@ -299,8 +287,6 @@ void printTimingStats() { void setupNetwork() { WiFi.mode(WIFI_STA); - - // 절전/모뎀슬립 비활성화 + 자동 재연결 WiFi.setSleep(false); esp_wifi_set_ps(WIFI_PS_NONE); WiFi.setAutoReconnect(true); @@ -327,7 +313,6 @@ void setupNetwork() { 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()); - // 실제 채널 출력 (ESPNOW는 이 채널과 동일해야 함) uint8_t primary = 0; wifi_second_chan_t second = WIFI_SECOND_CHAN_NONE; esp_wifi_get_channel(&primary, &second); @@ -335,18 +320,17 @@ void setupNetwork() { server.begin(); - // ESPNOW init (레거시 C API) if (esp_now_init() != ESP_OK) { Serial.println("[ESPNOW] init failed -> restart"); ESP.restart(); } - // Core 3.x 콜백 등록 esp_now_register_recv_cb(onEspNowRecv); - ensureBroadcastPeer(); - Serial.printf("[ESPNOW] Ready. Using max payload=%d (v2 confirmed in your environment)\n", ESPNOW_MAX_PAYLOAD); + // 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() { @@ -371,11 +355,14 @@ void setup() { currentBroadcastInterval_ms = random(BROADCAST_MIN_INTERVAL_MS, BROADCAST_MAX_INTERVAL_MS); setupNetwork(); + + Serial.println("[SYSTEM] Using drift-free timing (누적 오차 방지)"); } void loop() { - // Wi-Fi 끊김 복구 - unsigned long loopStart_us = micros(); + unsigned long loopStart_us = micros(); + + // ========== WiFi 재연결 체크 ========== if (WiFi.status() != WL_CONNECTED) { for (auto &c : clients) c.stop(); clients.clear(); @@ -384,7 +371,7 @@ void loop() { return; } - // 새 클라이언트 수락 (끊긴 슬롯 재사용) + // ========== TCP 클라이언트 수락 ========== WiFiClient newcomer = server.available(); if (newcomer) { newcomer.setTimeout(10); @@ -402,15 +389,17 @@ void loop() { if (!placed) clients.push_back(newcomer); } - // TCP 데이터 수신 (PC -> ESP32) + // ========== TCP 데이터 수신 (PC -> ESP32) ========== for (auto &c : clients) { if (c && c.connected() && c.available()) { String line = c.readStringUntil('\n'); - (void)deserializeJson(selfMessageDoc, line); // 실패해도 조용히 무시(필요하면 로그 추가) + (void)deserializeJson(selfMessageDoc, line); } } - // ESPNOW 브로드캐스트 + // ========== 핵심 작업 (누적 오차 방지 방식) ========== + + // 1. ESP-NOW 브로드캐스트 (가장 중요!) broadcastSelfMessageIfDue(); // 2. TCP 모니터링 송신 From 39c9bbf8f22c352770753328d298fad29038585f Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Tue, 24 Feb 2026 16:52:51 +0900 Subject: [PATCH 09/10] Dual core: core 0 for puppet, core 1 for p2p --- .../SPACE_MONA_offboard.ino | 691 +++++++++--------- 1 file changed, 335 insertions(+), 356 deletions(-) diff --git a/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino b/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino index 5533d91..a10942c 100644 --- a/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino +++ b/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino @@ -21,12 +21,43 @@ 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 uint16_t UDP_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, @@ -35,22 +66,25 @@ enum RobotState { STATE_ESCAPING, STATE_EMERGENCY }; -RobotState state = STATE_IDLE; +volatile RobotState state = STATE_IDLE; -// ESP-NOW settings -const int TOTAL_ROBOTS = 12; -const uint32_t MIN_BROADCAST_MS = 50; -const uint32_t MAX_BROADCAST_MS = 100; +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 MONITORING_SEND_INTERVAL_MS = 50; -const uint32_t INITIAL_BROADCAST_INTERVAL_MS = 40; +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; +static uint8_t g_pkt[ESPNOW_MAX_PAYLOAD]; +const uint32_t Peer_LinkDrop_MS = 900; + // 펄스/제어 상수 static const float PULSES_PER_MM = 18.0f; @@ -76,8 +110,10 @@ static const float ROTATION_DEADBAND_DEG = 5.0f; // 미세 회전 무시 각 static const int MIN_MOTOR_PWM = 60; // 모터 구동 최소 출력 // PI 제어 게인 (주행 보정) -static const float K_P = 0.95f; // 비례 게인 -static const float K_I = 0.01f; // 적분 게인 +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; @@ -97,40 +133,9 @@ unsigned long oscillation_timer_start = 0; unsigned long emergency_back_until = 0; unsigned long emergency_spin_until = 0; -// ===== Network (Core 0) ===== -WiFiServer tcpServer(TCP_PORT); -WiFiUDP udp; -std::vector tcpClients; - -// ===== CBBA Communication (Core 0) ===== -DynamicJsonDocument selfMessageDoc(JSON_SIZE); -std::map receivedJSON_MAP; -std::map CommRecvTime_MAP; -SemaphoreHandle_t mapLock; -SemaphoreHandle_t selfMsgLock; // NEW: selfMessageDoc 보호 - -unsigned long lastBroadcast = 0; -volatile bool dirtySelf = false; -volatile bool dirtyNeighbors = false; - -// ===== ESP-NOW counters ===== -volatile uint32_t espnow_rx_bytes = 0; -volatile uint32_t espnow_tx_bytes = 0; - -// Global JSON buffer -static char g_jsonBuf[JSON_SIZE]; -static uint8_t g_pkt[ESPNOW_MAX_PAYLOAD]; - -// ===== Motion Control (Core 1) - Atomic variables for cross-core sharing ===== -volatile float targetAngleDeg = 0; -volatile float targetDistMm = 0; -volatile bool newMotionCommand = false; // Core 0 → Core 1 signal -volatile bool stopRequested = false; - 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++; } @@ -140,80 +145,30 @@ static long target_turn_pulses = 0; static long target_move_pulses = 0; static float integral_error = 0.0f; -TaskHandle_t commTaskHandle = NULL; -TaskHandle_t motionTaskHandle = NULL; +// --- 제어 주기 --- +static const unsigned long CONTROL_INTERVAL_MS = 5; // 5ms 이동 제어 +static const unsigned long LED_UPDATE_INTERVAL_MS = 100; // 100ms LED 갱신 -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; -} +// Puppet 태스크 핸들 +TaskHandle_t puppetTaskHandle = NULL; -static inline void enter_escaping(uint16_t ms = ESCAPING_MS) { - escaping_until_ms = millis() + ms; - Motors_forward(FWD_SPD); - state = STATE_ESCAPING; -} +// ====== P2P 함수들 ========================================== -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; -} - -// ============================================================================= -// ESP-NOW COMMUNICATION (Core 0) -// ============================================================================= bool update_Broadcast_recv_JSON_MAP(const String& senderID, const char* jsonBuf, size_t jsonLen) { - if (jsonLen < 2) return false; + DynamicJsonDocument* doc = new DynamicJsonDocument(JSON_SIZE); + if (deserializeJson(*doc, jsonBuf, jsonLen)) { delete doc; return false; } - // 콜백에서 풀 파싱은 부담 큼 -> 아주 가벼운 형태 체크만 - const char first = jsonBuf[0]; - const char last = jsonBuf[jsonLen - 1]; - if (!((first == '{' && last == '}') || (first == '[' && last == ']'))) { - return false; - } + // 콜백(인터럽트 컨텍스트)에서 오래 블록하지 않도록 즉시 실패 허용 + if (xSemaphoreTake(mapLock, 0) != pdTRUE) { delete doc; return false; } - // 수신 콜백에서 오래 잠그지 않기: 즉시 락 실패 시 드랍 - if (xSemaphoreTake(mapLock, 0) != pdTRUE) { - 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; } - - // 기존 String capacity 재사용(힙 단편화 완화) - String& dst = receivedJSON_MAP[senderID]; - dst.remove(0); - dst.reserve(jsonLen + 1); - dst.concat(jsonBuf, jsonLen); - CommRecvTime_MAP[senderID] = millis(); xSemaphoreGive(mapLock); return true; @@ -231,11 +186,7 @@ void onEspNowRecv(const esp_now_recv_info_t *recv_info, const uint8_t *incoming, int jsonLen = len - (1 + idLen); if (jsonLen <= 0) return; - - if (update_Broadcast_recv_JSON_MAP(senderID, (const char*)(&incoming[1 + idLen]), (size_t)jsonLen)) { - espnow_rx_bytes += (uint32_t)len; - dirtyNeighbors = true; - } + update_Broadcast_recv_JSON_MAP(senderID, (const char*)(&incoming[1 + idLen]), (size_t)jsonLen); } void ensureBroadcastPeer() { @@ -252,30 +203,19 @@ void ensureBroadcastPeer() { } void broadcastSelfMessageIfDue() { - static uint32_t nextInterval = 40; - if (millis() - lastBroadcast < nextInterval) return; - - lastBroadcast = millis(); - nextInterval = random(MIN_BROADCAST_MS, MAX_BROADCAST_MS); - - // Lock selfMessageDoc for reading - if (xSemaphoreTake(selfMsgLock, pdMS_TO_TICKS(5)) != pdTRUE) return; - - if (selfMessageDoc.isNull() || selfMessageDoc.size() == 0) { - xSemaphoreGive(selfMsgLock); - return; - } + 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)); - xSemaphoreGive(selfMsgLock); - if (jsonLen == 0) return; - uint8_t idLen = (uint8_t)SELF_ID.length(); - size_t total = 1 + (size_t)idLen + jsonLen; - + 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; @@ -284,171 +224,144 @@ void broadcastSelfMessageIfDue() { uint8_t bcast[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; esp_now_send(bcast, g_pkt, total); - espnow_tx_bytes += (uint32_t)total; } -// ============================================================================= -// TCP COMMUNICATION (Core 0) -// ============================================================================= -void handleTcpClients() { - WiFiClient newcomer = tcpServer.available(); - if (newcomer) { - newcomer.setTimeout(10); - newcomer.setNoDelay(true); - - bool placed = false; - for (auto &c : tcpClients) { - if (!c || !c.connected()) { - c.stop(); - c = newcomer; - placed = true; - break; - } - } - if (!placed) tcpClients.push_back(newcomer); - } - - for (auto &c : tcpClients) { - if (c && c.connected() && c.available()) { - String line = c.readStringUntil('\n'); - - // Lock selfMessageDoc for writing - if (xSemaphoreTake(selfMsgLock, pdMS_TO_TICKS(10)) == pdTRUE) { - if (deserializeJson(selfMessageDoc, line) == DeserializationError::Ok) { - dirtySelf = true; - } - xSemaphoreGive(selfMsgLock); - } - } - } -} - -void sendMonitorToTcpClients() { - static unsigned long lastTcpSend = 0; - if (millis() - lastTcpSend < 50) return; - - // [FIX #1] 큰 DynamicJsonDocument 제거: - // mapLock을 짧게 잡고 스냅샷만 뜬 뒤, 락 풀고 String으로 한 줄 JSON 구성 - std::vector> snapshot; - snapshot.reserve(16); - - const unsigned long now = millis(); - - if (xSemaphoreTake(mapLock, pdMS_TO_TICKS(5)) == pdTRUE) { - for (auto const &kv : receivedJSON_MAP) { - auto itT = CommRecvTime_MAP.find(kv.first); - if (itT != CommRecvTime_MAP.end() && (now - itT->second <= PEER_LINK_DROP_MS)) { - snapshot.push_back({kv.first, kv.second}); // (senderID, raw json) - } - } - xSemaphoreGive(mapLock); +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(); } - - // 한 줄 JSON: {"agent_id":"11","received_messages":{"03":{...},"04":{...}}} - size_t est = 64; - for (auto &kv : snapshot) est += 6 + kv.first.length() + kv.second.length(); + xSemaphoreGive(mapLock); String out; - out.reserve(est); - out += "{\"agent_id\":\""; - out += SELF_ID; - out += "\",\"received_messages\":{"; - - bool first = true; - for (auto &kv : snapshot) { - if (!first) out += ","; - first = false; - - out += "\""; - out += kv.first; - out += "\":"; - out += kv.second; // raw JSON object/array - } + serializeJson(monitor, out); + out += "\n"; + for (auto& c : clients) + if (c && c.connected()) c.print(out); - out += "}}\n"; + size_t tempMem = currentMemoryUsage_bytes + monitorSize; + if (tempMem > peakMemoryUsage_bytes) peakMemoryUsage_bytes = tempMem; +} - for (auto &c : tcpClients) { - if (c && c.connected()) { - c.print(out); - } +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); - dirtySelf = dirtyNeighbors = false; - lastTcpSend = millis(); + if (cleaned > 0) + Serial.printf("[P2P][MEM] Cleaned %d stale entries, current: %d bytes\n", + cleaned, currentMemoryUsage_bytes); +} - // Cleanup disconnected clients - for (auto &c : tcpClients) { - if (c && !c.connected()) c.stop(); - } - tcpClients.erase(std::remove_if(tcpClients.begin(), tcpClients.end(), - [](WiFiClient& c) { return !c.connected(); }), tcpClients.end()); +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; } -// ============================================================================= -// UDP MOTION COMMANDS (Core 0 receives, Core 1 executes) -// ============================================================================= -void handleUdpPacket() { - static char udpBuffer[256]; - - int packetSize = udp.parsePacket(); - if (!packetSize) return; +// ============================================================ +// ====== Puppet 함수들 ======================================= +// ============================================================ - // Keep only latest packet - while (packetSize) { - int len = udp.read(udpBuffer, 255); - if (len > 0) udpBuffer[len] = 0; - packetSize = udp.parsePacket(); - } +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; +} - // STOP command - if (strncasecmp(udpBuffer, "STOP", 4) == 0) { - stopRequested = true; - return; - } +static inline void enter_escaping(uint16_t ms = ESCAPING_MS) { + escaping_until_ms = millis() + ms; + Motors_forward(FWD_SPD); + state = STATE_ESCAPING; +} - // G command: "G " - if (udpBuffer[0] == 'G' || udpBuffer[0] == 'g') { - float angle = 0, dist = 0; - if (sscanf(udpBuffer + 1, "%f %f", &angle, &dist) == 2) { - if (dist >= MIN_DIST_MM) { - targetAngleDeg = angle; - targetDistMm = dist; - newMotionCommand = true; // Signal to Core 1 - } - } - } +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"); } -// ============================================================================= -// COMMUNICATION TASK (Core 0) - HIGH PRIORITY -// ============================================================================= -void commTask(void* parameter) { - Serial.println("[Core 0] Communication task started"); - - for (;;) { - // WiFi reconnection - if (WiFi.status() != WL_CONNECTED) { - for (auto &c : tcpClients) c.stop(); - tcpClients.clear(); - WiFi.reconnect(); - vTaskDelay(pdMS_TO_TICKS(100)); - continue; +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; +} - // ===== All communication handling ===== - handleTcpClients(); // TCP: PC <-> Robot - broadcastSelfMessageIfDue(); // ESP-NOW: Robot <-> Robot - sendMonitorToTcpClients(); // Send monitor to PC - handleUdpPacket(); // UDP: Motion commands +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); +} - // Short delay to prevent watchdog trigger - vTaskDelay(pdMS_TO_TICKS(1)); +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; } } -// ============================================================================= -// MOTION CONTROL TASK (Core 1) - INDEPENDENT -// ============================================================================= void start_motion(float angle_deg, float dist_mm) { if (state == STATE_AVOID || state == STATE_ESCAPING || state == STATE_EMERGENCY) { return; @@ -473,6 +386,35 @@ void start_motion(float angle_deg, float dist_mm) { } } +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); @@ -523,13 +465,10 @@ void control_loop(int r1, int r2, int r3, int r4, int r5) { Motors_stop(); state = STATE_IDLE; } else { - long err = l_now - r_now; - integral_error += err; - float u = (K_P * (float)err) + (K_I * (float)integral_error); - int left_pwm = constrain((int)lroundf(FWD_SPD - u), MIN_MOTOR_PWM, 255); - int right_pwm = constrain((int)lroundf(FWD_SPD + u), MIN_MOTOR_PWM, 255); - Left_mot_forward(left_pwm); - Right_mot_forward(right_pwm); + int lp, rp; + pi_control(l_now, r_now, &lp, &rp); + Left_mot_forward(lp); + Right_mot_forward(rp); } break; @@ -571,45 +510,39 @@ void control_loop(int r1, int r2, int r3, int r4, int r5) { } } -void motionTask(void* parameter) { - Serial.println("[Core 1] Motion control task started"); - - for (;;) { - // Check for STOP command from Core 0 - if (stopRequested) { - Motors_stop(); - clear_motion_targets(); - state = STATE_IDLE; - stopRequested = false; - } - // Check for new motion command from Core 0 - if (newMotionCommand) { - // Only accept if not in obstacle avoidance - 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) { - start_motion(targetAngleDeg, targetDistMm); - } - } else if (state == STATE_IDLE || state == STATE_TURNING) { - start_motion(targetAngleDeg, targetDistMm); - } - newMotionCommand = false; - } +// ============================================================ +// ====== 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); - // Read IR sensors (this is slow, but doesn't block Core 0 now!) - int r1 = Get_IR(1); - int r2 = Get_IR(2); - int r3 = Get_IR(3); - int r4 = Get_IR(4); - int r5 = Get_IR(5); + unsigned long last_control_time = 0; + unsigned long last_led_update = 0; - // Run motion control - control_loop(r1, r2, r3, r4, r5); + 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); + } - // Motion task runs at ~50Hz (20ms period) - vTaskDelay(pdMS_TO_TICKS(50)); + // 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 양보 } } @@ -638,10 +571,8 @@ void setupNetwork() { 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()); + Serial.printf("\n[WiFi] Connected: %s\n", WiFi.localIP().toString().c_str()); + Serial.printf("[WiFi] SELF_ID: %s\n", SELF_ID.c_str()); // 실제 채널 출력 (ESPNOW는 이 채널과 동일해야 함) uint8_t primary = 0; @@ -653,17 +584,10 @@ void setupNetwork() { tcpServer.begin(); Serial.printf("[TCP] Server on port %d\n", TCP_PORT); - // Start UDP - udp.begin(UDP_PORT); - Serial.printf("[UDP] Server on port %d\n", UDP_PORT); - - // Initialize ESP-NOW if (esp_now_init() != ESP_OK) { Serial.println("[ESPNOW] init failed -> restart"); ESP.restart(); } - - // Core 3.x 콜백 등록 esp_now_register_recv_cb(onEspNowRecv); ensureBroadcastPeer(); @@ -677,9 +601,18 @@ void setup() { randomSeed(esp_random()); mapLock = xSemaphoreCreateMutex(); - selfMsgLock = xSemaphoreCreateMutex(); - // Initialize Mona robot hardware + // 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(); // Setup encoders @@ -687,40 +620,86 @@ void setup() { 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); setupNetwork(); - Serial.println("================================="); - Serial.println("MONA Firmware v2 - Dual Core"); - Serial.println("- Core 0: Communication (TCP/ESP-NOW/UDP)"); - Serial.println("- Core 1: Motion Control (IR/Motors)"); - Serial.println("================================="); - - // Create Communication Task on Core 0 (PRO_CPU) - // Higher priority (2) than default Arduino loop +// ====== Core 1: puppet loop() ================================== xTaskCreatePinnedToCore( - commTask, // Task function - "CommTask", // Name - 8192, // Stack size - NULL, // Parameters - 2, // Priority (higher = more important) - &commTaskHandle, // Task handle - 0 // Core 0 + puppetTask, // 태스크 함수 + "PuppetTask", // 태스크 이름 + 8192, // 스택 크기 (bytes) + NULL, // 파라미터 + 2, // 우선순위 (loop()의 1보다 높게 → 모터 제어 우선) + &puppetTaskHandle, // 태스크 핸들 + 0 // Core 0 고정 ); - // Create Motion Task on Core 1 (APP_CPU) - // Lower priority (1), independent from communication - xTaskCreatePinnedToCore( - motionTask, // Task function - "MotionTask", // Name - 4096, // Stack size - NULL, // Parameters - 1, // Priority - &motionTaskHandle, // Task handle - 1 // Core 1 - ); + 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() { - vTaskDelay(pdMS_TO_TICKS(1000)); + 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 From a75e1a667ca24c9544b62d01ad3dcb959e6e3870 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Tue, 24 Feb 2026 17:02:22 +0900 Subject: [PATCH 10/10] =?UTF-8?q?=EC=BD=94=EB=93=9C=20=EC=A0=95=EB=A6=AC,?= =?UTF-8?q?=20=EC=A3=BC=EC=84=9D=20=EB=B3=80=EA=B2=BD,=20serial=20print=20?= =?UTF-8?q?=EB=B3=80=EA=B2=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../SPACE_MONA_offboard.ino | 169 +++++++----------- 1 file changed, 69 insertions(+), 100 deletions(-) diff --git a/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino b/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino index a10942c..4ba0246 100644 --- a/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino +++ b/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino @@ -17,11 +17,11 @@ #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 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와 프로토콜 달라 포트 공유 가능) +const int UDP_PORT = 8080; // UDP: PC → ESP32 이동 명령 (TCP와 프로토콜 달라 포트 공유 가능) // JSON buffer size const size_t JSON_SIZE = 2048; @@ -39,13 +39,13 @@ 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; +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; +size_t currentMemoryUsage_bytes = 0; +size_t peakMemoryUsage_bytes = 0; static char g_jsonBuf[JSON_SIZE]; @@ -70,31 +70,31 @@ 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 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 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; +const uint32_t SERIAL_STABILIZE_DELAY_MS = 1000; -static const int ESPNOW_MAX_PAYLOAD = 1470; +static const int ESPNOW_MAX_PAYLOAD = 1470; -uint32_t currentBroadcastInterval_ms = BROADCAST_MIN_INTERVAL_MS; -static uint8_t g_pkt[ESPNOW_MAX_PAYLOAD]; +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; +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; +static const float MIN_DIST_MM = 40.0f; // IR/회피 static const int TH = 80; @@ -110,21 +110,20 @@ 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 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 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 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; +static const int OSCILLATION_COUNT_THRESHOLD = 4; int last_turn_direction = 0; int turn_change_count = 0; @@ -139,11 +138,11 @@ 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 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 float integral_error = 0.0f; // --- 제어 주기 --- static const unsigned long CONTROL_INTERVAL_MS = 5; // 5ms 이동 제어 @@ -195,7 +194,7 @@ void ensureBroadcastPeer() { esp_now_peer_info_t p = {}; memcpy(p.peer_addr, bcast, 6); - p.ifidx = WIFI_IF_STA; + p.ifidx = WIFI_IF_STA; p.channel = WiFi.channel(); p.encrypt = false; @@ -363,18 +362,16 @@ void update_status_led() { } void start_motion(float angle_deg, float dist_mm) { - if (state == STATE_AVOID || state == STATE_ESCAPING || state == STATE_EMERGENCY) { - return; - } + 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; + 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)) { + 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); @@ -420,25 +417,15 @@ void control_loop(int r1, int r2, int r3, int r4, int r5) { if (obstacle) { if (state != STATE_AVOID && state != STATE_ESCAPING && state != STATE_EMERGENCY) { - Motors_stop(); - clear_motion_targets(); - state = STATE_AVOID; + 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; + 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); @@ -452,18 +439,15 @@ void control_loop(int r1, int r2, int r3, int r4, int r5) { if (target_move_pulses > 0) { start_left_count = left_encoder_count; start_right_count = right_encoder_count; - integral_error = 0.0f; + integral_error = 0.0f; state = STATE_MOVING; - } else { - state = STATE_IDLE; - } + } else { state = STATE_IDLE; } } break; case STATE_MOVING: if (avg >= target_move_pulses) { - Motors_stop(); - state = STATE_IDLE; + Motors_stop(); state = STATE_IDLE; } else { int lp, rp; pi_control(l_now, r_now, &lp, &rp); @@ -473,37 +457,25 @@ void control_loop(int r1, int r2, int r3, int r4, int r5) { break; case STATE_AVOID: - if ((r1 <= TH_OUTER) && (r2 <= TH) && (r3 <= TH) && (r4 <= TH) && (r5 <= TH_OUTER)) { - enter_escaping(ESCAPING_MS); - break; + 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); + Motors_spin_left(TURN_SPD); check_oscillation_and_escape(-1); } else { - Motors_spin_right(TURN_SPD); - check_oscillation_and_escape(+1); + 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); + 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); + 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; - } + if (obstacle) { Motors_stop(); state = STATE_AVOID; break; } + if ((int32_t)(millis() - escaping_until_ms) >= 0) { Motors_stop(); state = STATE_IDLE; } break; default: break; @@ -546,53 +518,48 @@ void puppetTask(void* parameter) { } } +// ============================================================ +// ====== 공통 초기화 ========================================= +// ============================================================ 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"); + 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] connect timeout -> retry"); + 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 to AP"); + 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()); - // 실제 채널 출력 (ESPNOW는 이 채널과 동일해야 함) 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); - // Start TCP server + // TCP 서버 시작 (P2P용) tcpServer.begin(); - Serial.printf("[TCP] Server on port %d\n", TCP_PORT); + 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(); + Serial.println("[ESPNOW] Init failed -> restart"); ESP.restart(); } esp_now_register_recv_cb(onEspNowRecv); - ensureBroadcastPeer(); - - Serial.printf("[ESPNOW] Ready. Using max payload=%d (v2 confirmed in your environment)\n", ESPNOW_MAX_PAYLOAD); + Serial.printf("[ESPNOW] Ready. Max payload=%d bytes\n", ESPNOW_MAX_PAYLOAD); } void setup() { @@ -614,18 +581,17 @@ void setup() { // MONA 하드웨어 초기화 (Puppet용) Mona_ESP_init(); - - // Setup encoders - pinMode(PIN_ENCODER_LEFT, INPUT); + pinMode(PIN_ENCODER_LEFT, INPUT); pinMode(PIN_ENCODER_RIGHT, INPUT); - attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_LEFT), isr_left_encoder, RISING); + 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 1: puppet loop() ================================== + // ★ Core 0에 Puppet 태스크 생성 xTaskCreatePinnedToCore( puppetTask, // 태스크 함수 "PuppetTask", // 태스크 이름 @@ -641,7 +607,10 @@ void setup() { Serial.println("[SYSTEM] Core 1 → P2P (ESP-NOW + TCP CBBA)"); } + +// ============================================================ // ====== Core 1: P2P loop() ================================== +// ============================================================ void loop() { unsigned long loopStart_us = micros();