From f73a08196938cc2b1c4b9b011d7a3fb015713b49 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 7 Jan 2026 13:20:59 +0900 Subject: [PATCH 01/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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 93224585fb71d9886b890136cfa20f138e0223ca Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 21 Jan 2026 14:12:28 +0900 Subject: [PATCH 07/16] Micropyton_MONA_p2p --- Micropython/MONA_p2p/ads7830.py | 178 +++++++++++++ Micropython/MONA_p2p/espnow_comm.py | 181 ++++++++++++++ Micropython/MONA_p2p/main.py | 150 +++++++++++ Micropython/MONA_p2p/mcp23008.py | 133 ++++++++++ Micropython/MONA_p2p/mona_comm.py | 116 +++++++++ Micropython/MONA_p2p/mona_esp_lib.py | 357 +++++++++++++++++++++++++++ Micropython/MONA_p2p/neopixel_led.py | 61 +++++ Micropython/MONA_p2p/tcp_server.py | 162 ++++++++++++ Micropython/MONA_p2p/wifi_manager.py | 91 +++++++ 9 files changed, 1429 insertions(+) create mode 100644 Micropython/MONA_p2p/ads7830.py create mode 100644 Micropython/MONA_p2p/espnow_comm.py create mode 100644 Micropython/MONA_p2p/main.py create mode 100644 Micropython/MONA_p2p/mcp23008.py create mode 100644 Micropython/MONA_p2p/mona_comm.py create mode 100644 Micropython/MONA_p2p/mona_esp_lib.py create mode 100644 Micropython/MONA_p2p/neopixel_led.py create mode 100644 Micropython/MONA_p2p/tcp_server.py create mode 100644 Micropython/MONA_p2p/wifi_manager.py diff --git a/Micropython/MONA_p2p/ads7830.py b/Micropython/MONA_p2p/ads7830.py new file mode 100644 index 0000000..779455a --- /dev/null +++ b/Micropython/MONA_p2p/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_p2p/espnow_comm.py b/Micropython/MONA_p2p/espnow_comm.py new file mode 100644 index 0000000..cff5565 --- /dev/null +++ b/Micropython/MONA_p2p/espnow_comm.py @@ -0,0 +1,181 @@ +import network +import espnow +import json +import time +from machine import Timer + +# ====== Configuration ====== +ESPNOW_MAX_PAYLOAD = 250 # MicroPython ESP-NOW limit +MIN_BROADCAST_MS = 50 +MAX_BROADCAST_MS = 100 +PEER_LINK_DROP_MS = 900 +BROADCAST_ADDR = b'\xff\xff\xff\xff\xff\xff' + + +class ESPNowComm: + + def __init__(self, self_id: str): + self._self_id = self_id + self._self_id_bytes = self_id.encode('utf-8') + + # Message storage + self._self_message = {} # Message to broadcast + self._received_messages = {} # {sender_id: message_dict} + self._recv_timestamps = {} # {sender_id: timestamp_ms} + + # ESP-NOW objects + self._wlan = None + self._espnow = None + + # Timing + self._last_broadcast_ms = 0 + self._next_interval_ms = MIN_BROADCAST_MS + + # Statistics + self._tx_count = 0 + self._rx_count = 0 + self._tx_fail_count = 0 + + def init(self, wlan: network.WLAN = None): + if wlan is None: + self._wlan = network.WLAN(network.STA_IF) + self._wlan.active(True) + else: + self._wlan = wlan + + # Initialize ESP-NOW + self._espnow = espnow.ESPNow() + self._espnow.active(True) + + # Add broadcast peer + try: + self._espnow.add_peer(BROADCAST_ADDR) + except OSError as e: + # Peer might already exist + pass + + print(f"[ESP-NOW] Initialized. Self ID: {self._self_id}") + + def set_message(self, message: dict): + self._self_message = message + + def get_message(self) -> dict: + return self._self_message + + def get_received_messages(self) -> dict: + now_ms = time.ticks_ms() + active_messages = {} + + for sender_id, msg in self._received_messages.items(): + recv_time = self._recv_timestamps.get(sender_id, 0) + if time.ticks_diff(now_ms, recv_time) <= PEER_LINK_DROP_MS: + active_messages[sender_id] = msg + + return active_messages + + def poll(self): + self._receive_all() + self._broadcast_if_due() + + def _receive_all(self): + if self._espnow is None: + return + + while True: + try: + host, msg = self._espnow.recv(0) # Non-blocking + if msg is None: + break + + self._process_received_packet(msg) + + except OSError: + break + + def _process_received_packet(self, packet: bytes): + if len(packet) < 2: + return + + id_len = packet[0] + if len(packet) < 1 + id_len: + return + + sender_id = packet[1:1+id_len].decode('utf-8', 'ignore') + + # Ignore self messages + if sender_id == self._self_id: + return + + json_data = packet[1+id_len:] + if len(json_data) == 0: + return + + try: + message = json.loads(json_data.decode('utf-8', 'ignore')) + self._received_messages[sender_id] = message + self._recv_timestamps[sender_id] = time.ticks_ms() + self._rx_count += 1 + + except (ValueError, UnicodeError) as e: + # JSON parse error - ignore + pass + + def _broadcast_if_due(self): + if self._espnow is None: + return + + now_ms = time.ticks_ms() + if time.ticks_diff(now_ms, self._last_broadcast_ms) < self._next_interval_ms: + return + + self._last_broadcast_ms = now_ms + + # Randomize next interval + import random + self._next_interval_ms = random.randint(MIN_BROADCAST_MS, MAX_BROADCAST_MS) + + if not self._self_message: + return + + # Build packet: [id_len][id][json] + try: + json_bytes = json.dumps(self._self_message, separators=(',', ':')).encode('utf-8') + except (TypeError, ValueError): + return + + id_len = len(self._self_id_bytes) + total_len = 1 + id_len + len(json_bytes) + + if total_len > ESPNOW_MAX_PAYLOAD: + print(f"[ESP-NOW] Message too large: {total_len} > {ESPNOW_MAX_PAYLOAD}") + self._tx_fail_count += 1 + return + + packet = bytes([id_len]) + self._self_id_bytes + json_bytes + + try: + self._espnow.send(BROADCAST_ADDR, packet) + self._tx_count += 1 + except OSError as e: + self._tx_fail_count += 1 + + def build_monitor_json(self) -> dict: + return { + "agent_id": self._self_id, + "received_messages": self.get_received_messages() + } + + def get_stats(self) -> dict: + return { + "tx_count": self._tx_count, + "rx_count": self._rx_count, + "tx_fail_count": self._tx_fail_count, + "active_peers": len(self.get_received_messages()) + } + + def close(self): + """Cleanup ESP-NOW resources.""" + if self._espnow: + self._espnow.active(False) + self._espnow = None + diff --git a/Micropython/MONA_p2p/main.py b/Micropython/MONA_p2p/main.py new file mode 100644 index 0000000..d768cc8 --- /dev/null +++ b/Micropython/MONA_p2p/main.py @@ -0,0 +1,150 @@ +import time +import gc +from machine import Pin + +# ====== CONFIGURATION ====== +SELF_ID = "2" # Change this for each robot! +SSID = "InMOLab" +PASSWORD = "dlsahfoq104!" +TCP_PORT = 8080 + +# Optional: LED for status indication +LED_PIN = 2 # Built-in LED on most ESP32 boards + +# ====== Global Objects ====== +comm = None +led = None + + +def setup(): + """Initialize hardware and communication.""" + global comm, led + + print("\n" + "="*50) + print(f"MONA ESP-NOW Test - Robot ID: {SELF_ID}") + print("="*50 + "\n") + + # Setup status LED + try: + led = Pin(LED_PIN, Pin.OUT) + led.off() + except: + led = None + print("[Setup] No LED available") + + # Initialize communication + from mona_comm import MonaComm + comm = MonaComm(SELF_ID, SSID, PASSWORD, TCP_PORT) + + if not comm.start(): + print("[Setup] Communication init failed!") + blink_error() + return False + + print("[Setup] Complete!\n") + return True + + +def blink_error(): + """Blink LED rapidly to indicate error.""" + if led is None: + return + for _ in range(10): + led.on() + time.sleep_ms(100) + led.off() + time.sleep_ms(100) + + +def blink_status(count: int = 1): + """Blink LED to indicate activity.""" + if led is None: + return + for _ in range(count): + led.on() + time.sleep_ms(50) + led.off() + time.sleep_ms(50) + + +def main_loop(): + """Main communication loop.""" + global comm + + last_stats_ms = time.ticks_ms() + stats_interval_ms = 5000 # Print stats every 5 seconds + + # Test message (simulating CBBA data) + test_message = { + "id": int(SELF_ID), + "y": {}, # Winning bids (will be populated by simulator) + "z": {}, # Winning agents + "s": {} # Timestamps + } + + print("[Loop] Starting main loop...") + print("[Loop] Waiting for TCP connection from simulator...\n") + + while True: + try: + # Update communication (handles all send/receive) + comm.update() + + # Get TCP message from simulator (if any) + tcp_msg = comm.get_tcp_message() + if tcp_msg: + # Forward to ESP-NOW broadcast + comm.set_message(tcp_msg) + blink_status(1) + + # Get received ESP-NOW messages + received = comm.get_received_messages() + + # Print stats periodically + now_ms = time.ticks_ms() + if time.ticks_diff(now_ms, last_stats_ms) > stats_interval_ms: + last_stats_ms = now_ms + stats = comm.get_stats() + + print(f"[Stats] TX:{stats['tx_count']} RX:{stats['rx_count']} " + f"Fail:{stats['tx_fail_count']} Peers:{stats['active_peers']} " + f"TCP:{stats['tcp_clients']}") + + if received: + print(f"[Peers] Active: {list(received.keys())}") + + # Memory info + gc.collect() + free_mem = gc.mem_free() + print(f"[Mem] Free: {free_mem} bytes") + print() + + # Small delay to prevent tight loop + time.sleep_ms(1) + + except KeyboardInterrupt: + print("\n[Loop] Interrupted by user") + break + + except Exception as e: + print(f"[Loop] Error: {e}") + time.sleep_ms(100) + + +def main(): + """Main entry point.""" + if setup(): + try: + main_loop() + finally: + if comm: + comm.stop() + print("[Main] Shutdown complete") + else: + print("[Main] Setup failed, halting") + + +# Auto-run on import +if __name__ == "__main__": + main() + diff --git a/Micropython/MONA_p2p/mcp23008.py b/Micropython/MONA_p2p/mcp23008.py new file mode 100644 index 0000000..496544a --- /dev/null +++ b/Micropython/MONA_p2p/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_p2p/mona_comm.py b/Micropython/MONA_p2p/mona_comm.py new file mode 100644 index 0000000..923e321 --- /dev/null +++ b/Micropython/MONA_p2p/mona_comm.py @@ -0,0 +1,116 @@ +from wifi_manager import WiFiManager +from espnow_comm import ESPNowComm +from tcp_server import TCPServer +import time +import gc + + +class MonaComm: + + def __init__(self, self_id: str, ssid: str, password: str, tcp_port: int = 8080): + self._self_id = self_id + + # Communication modules + self._wifi = WiFiManager(ssid, password) + self._espnow = ESPNowComm(self_id) + self._tcp = TCPServer(port=tcp_port) + + # State + self._initialized = False + self._last_gc_ms = 0 + self._gc_interval_ms = 5000 # Garbage collect every 5 seconds + + def start(self) -> bool: + # Connect WiFi first + if not self._wifi.connect(): + print("[MonaComm] WiFi connection failed") + return False + + # Initialize ESP-NOW + self._espnow.init(self._wifi.get_wlan()) + + # Start TCP server + self._tcp.start() + + self._initialized = True + print(f"[MonaComm] Started. ID={self._self_id}, IP={self._wifi.get_ip()}") + + return True + + def update(self): + if not self._initialized: + return + + # Check WiFi connection + if not self._wifi.check_connection(): + return + + # Process ESP-NOW + self._espnow.poll() + + # Process TCP with monitor data + monitor_data = self.get_monitor_data() + self._tcp.poll(monitor_data) + + # Update self message from TCP if received + tcp_msg = self._tcp.get_received_message() + if tcp_msg: + self._espnow.set_message(tcp_msg) + self._tcp.clear_received_message() + + # Periodic garbage collection + self._periodic_gc() + + def set_message(self, message: dict): + self._espnow.set_message(message) + + def get_received_messages(self) -> dict: + return self._espnow.get_received_messages() + + def get_monitor_data(self) -> dict: + return self._espnow.build_monitor_json() + + def get_tcp_message(self) -> dict: + return self._tcp.get_received_message() + + def is_connected(self) -> bool: + return self._wifi.is_connected() + + def is_tcp_client_connected(self) -> bool: + return self._tcp.is_client_connected() + + def get_ip(self) -> str: + return self._wifi.get_ip() + + def get_stats(self) -> dict: + espnow_stats = self._espnow.get_stats() + espnow_stats["wifi_connected"] = self._wifi.is_connected() + espnow_stats["tcp_clients"] = self._tcp.get_client_count() + espnow_stats["ip"] = self._wifi.get_ip() + return espnow_stats + + def _periodic_gc(self): + now_ms = time.ticks_ms() + if time.ticks_diff(now_ms, self._last_gc_ms) > self._gc_interval_ms: + self._last_gc_ms = now_ms + gc.collect() + + def stop(self): + self._tcp.close() + self._espnow.close() + self._wifi.disconnect() + self._initialized = False + print("[MonaComm] Stopped") + + +# ====== Convenience function for quick setup ====== + +def create_mona_comm(self_id: str, ssid: str = "InMOLab", + password: str = "dlsahfoq104!", + tcp_port: int = 8080) -> MonaComm: + comm = MonaComm(self_id, ssid, password, tcp_port) + if not comm.start(): + raise RuntimeError("Failed to start MonaComm") + return comm + + diff --git a/Micropython/MONA_p2p/mona_esp_lib.py b/Micropython/MONA_p2p/mona_esp_lib.py new file mode 100644 index 0000000..784d921 --- /dev/null +++ b/Micropython/MONA_p2p/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_p2p/neopixel_led.py b/Micropython/MONA_p2p/neopixel_led.py new file mode 100644 index 0000000..1dcf56d --- /dev/null +++ b/Micropython/MONA_p2p/neopixel_led.py @@ -0,0 +1,61 @@ +""" +WS2812B NeoPixel LED Driver for MicroPython +Uses the built-in neopixel module +""" + +from machine import Pin +from neopixel import NeoPixel + + +class NeoPixelLED: + """Simple NeoPixel LED wrapper class""" + + def __init__(self, pin: int, num_leds: int = 1): + """ + Initialize NeoPixel LED + + Args: + pin: GPIO pin number + num_leds: Number of LEDs in the strip + """ + self._pin = Pin(pin, Pin.OUT) + self._np = NeoPixel(self._pin, num_leds) + self._num_leds = num_leds + self.clear() + + def clear(self): + """Turn off all LEDs""" + for i in range(self._num_leds): + self._np[i] = (0, 0, 0) + self._np.write() + + def fill(self, r: int, g: int, b: int): + """ + Fill all LEDs with the same color + + Args: + r: Red component (0-255) + g: Green component (0-255) + b: Blue component (0-255) + """ + for i in range(self._num_leds): + self._np[i] = (r, g, b) + self._np.write() + + def set_pixel(self, index: int, r: int, g: int, b: int): + """ + Set a specific LED color + + Args: + index: LED index + r: Red component (0-255) + g: Green component (0-255) + b: Blue component (0-255) + """ + if 0 <= index < self._num_leds: + self._np[index] = (r, g, b) + self._np.write() + + def show(self): + """Update the LED strip (write changes)""" + self._np.write() diff --git a/Micropython/MONA_p2p/tcp_server.py b/Micropython/MONA_p2p/tcp_server.py new file mode 100644 index 0000000..646272f --- /dev/null +++ b/Micropython/MONA_p2p/tcp_server.py @@ -0,0 +1,162 @@ +import socket +import select +import json +import time + + +class TCPServer: + + def __init__(self, port: int = 8080, max_clients: int = 4): + self._port = port + self._max_clients = max_clients + + self._server_socket = None + self._clients = [] # List of (socket, buffer) + + # Received message from PC + self._received_message = {} + + # Timing + self._last_send_ms = 0 + self._send_interval_ms = 50 # Same as C++ MONITORING_SEND_INTERVAL_MS + + def start(self): + """Start the TCP server.""" + self._server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self._server_socket.setblocking(False) + + self._server_socket.bind(('0.0.0.0', self._port)) + self._server_socket.listen(self._max_clients) + + print(f"[TCP] Server listening on port {self._port}") + + def poll(self, monitor_data: dict = None): + self._accept_new_clients() + self._receive_from_clients() + + if monitor_data is not None: + self._send_to_clients_if_due(monitor_data) + + def _accept_new_clients(self): + if self._server_socket is None: + return + + try: + client_socket, addr = self._server_socket.accept() + client_socket.setblocking(False) + + # Check if we have room + if len(self._clients) >= self._max_clients: + # Remove disconnected clients first + self._cleanup_clients() + + if len(self._clients) < self._max_clients: + self._clients.append({ + 'socket': client_socket, + 'buffer': b'', + 'addr': addr + }) + print(f"[TCP] Client connected: {addr}") + else: + client_socket.close() + print(f"[TCP] Rejected client (max reached): {addr}") + + except OSError: + # No pending connections + pass + + def _receive_from_clients(self): + for client in self._clients[:]: # Copy list for safe iteration + try: + data = client['socket'].recv(1024) + if data: + client['buffer'] += data + self._process_client_buffer(client) + else: + # Client disconnected + self._remove_client(client) + + except OSError as e: + # Would block or other error + if e.args[0] not in (11, 110): # EAGAIN, ETIMEDOUT + self._remove_client(client) + + def _process_client_buffer(self, client: dict): + while b'\n' in client['buffer']: + line, client['buffer'] = client['buffer'].split(b'\n', 1) + + if line: + try: + message = json.loads(line.decode('utf-8', 'ignore')) + self._received_message = message + except (ValueError, UnicodeError): + pass + + def _send_to_clients_if_due(self, data: dict): + now_ms = time.ticks_ms() + if time.ticks_diff(now_ms, self._last_send_ms) < self._send_interval_ms: + return + + self._last_send_ms = now_ms + + try: + json_line = json.dumps(data, separators=(',', ':')) + '\n' + json_bytes = json_line.encode('utf-8') + except (TypeError, ValueError): + return + + for client in self._clients[:]: + try: + client['socket'].send(json_bytes) + except OSError: + self._remove_client(client) + + def _remove_client(self, client: dict): + try: + client['socket'].close() + except: + pass + + if client in self._clients: + self._clients.remove(client) + print(f"[TCP] Client disconnected: {client.get('addr', 'unknown')}") + + def _cleanup_clients(self): + for client in self._clients[:]: + try: + # Check if socket is still valid + client['socket'].getpeername() + except OSError: + self._remove_client(client) + + def get_received_message(self) -> dict: + return self._received_message + + def clear_received_message(self): + self._received_message = {} + + def is_client_connected(self) -> bool: + return len(self._clients) > 0 + + def get_client_count(self) -> int: + return len(self._clients) + + def close(self): + for client in self._clients: + try: + client['socket'].close() + except: + pass + + self._clients.clear() + + if self._server_socket: + try: + self._server_socket.close() + except: + pass + self._server_socket = None + + print("[TCP] Server closed") + diff --git a/Micropython/MONA_p2p/wifi_manager.py b/Micropython/MONA_p2p/wifi_manager.py new file mode 100644 index 0000000..899d326 --- /dev/null +++ b/Micropython/MONA_p2p/wifi_manager.py @@ -0,0 +1,91 @@ +import network +import time + + +class WiFiManager: + + def __init__(self, ssid: str, password: str): + self._ssid = ssid + self._password = password + self._wlan = None + + # Timing constants (ms) + self._connect_timeout_ms = 10000 + self._retry_delay_ms = 500 + self._check_interval_ms = 1000 + self._last_check_ms = 0 + + def connect(self) -> bool: + self._wlan = network.WLAN(network.STA_IF) + self._wlan.active(True) + + # Disable power save for better performance + try: + self._wlan.config(pm=0) # Disable power management + except: + pass # Not all firmwares support this + + if self._wlan.isconnected(): + print(f"[WiFi] Already connected: {self._wlan.ifconfig()[0]}") + return True + + print(f"[WiFi] Connecting to {self._ssid}...") + self._wlan.connect(self._ssid, self._password) + + start_ms = time.ticks_ms() + while not self._wlan.isconnected(): + if time.ticks_diff(time.ticks_ms(), start_ms) > self._connect_timeout_ms: + print("[WiFi] Connection timeout") + return False + + time.sleep_ms(100) + + ip = self._wlan.ifconfig()[0] + channel = self._get_channel() + print(f"[WiFi] Connected! IP: {ip}, Channel: {channel}") + + return True + + def _get_channel(self) -> int: + """Get current WiFi channel.""" + try: + return self._wlan.config('channel') + except: + return 0 + + def is_connected(self) -> bool: + """Check if WiFi is connected.""" + return self._wlan is not None and self._wlan.isconnected() + + def get_ip(self) -> str: + """Get current IP address.""" + if self.is_connected(): + return self._wlan.ifconfig()[0] + return "0.0.0.0" + + def get_wlan(self) -> network.WLAN: + """Get WLAN object for ESP-NOW initialization.""" + return self._wlan + + def check_connection(self) -> bool: + now_ms = time.ticks_ms() + if time.ticks_diff(now_ms, self._last_check_ms) < self._check_interval_ms: + return self.is_connected() + + self._last_check_ms = now_ms + + if not self.is_connected(): + print("[WiFi] Connection lost, reconnecting...") + self._wlan.disconnect() + time.sleep_ms(self._retry_delay_ms) + return self.connect() + + return True + + def disconnect(self): + """Disconnect from WiFi.""" + if self._wlan: + self._wlan.disconnect() + self._wlan.active(False) + print("[WiFi] Disconnected") + From bb3e53853ab813380d41febf951fe46cbd3463d9 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 21 Jan 2026 14:18:09 +0900 Subject: [PATCH 08/16] Micropyton_MONA_simple_collision_avoider --- .../MONA_simple_collision_avoider/ads7830.py | 178 +++++++++ .../MONA_simple_collision_avoider/main.py | 2 + .../MONA_simple_collision_avoider/mcp23008.py | 133 +++++++ .../mona_esp_lib.py | 357 ++++++++++++++++++ .../neopixel_led.py | 61 +++ .../simple_collision_avoider.py | 120 ++++++ 6 files changed, 851 insertions(+) create mode 100644 Micropython/MONA_simple_collision_avoider/ads7830.py create mode 100644 Micropython/MONA_simple_collision_avoider/main.py create mode 100644 Micropython/MONA_simple_collision_avoider/mcp23008.py create mode 100644 Micropython/MONA_simple_collision_avoider/mona_esp_lib.py create mode 100644 Micropython/MONA_simple_collision_avoider/neopixel_led.py create mode 100644 Micropython/MONA_simple_collision_avoider/simple_collision_avoider.py diff --git a/Micropython/MONA_simple_collision_avoider/ads7830.py b/Micropython/MONA_simple_collision_avoider/ads7830.py new file mode 100644 index 0000000..779455a --- /dev/null +++ b/Micropython/MONA_simple_collision_avoider/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_simple_collision_avoider/main.py b/Micropython/MONA_simple_collision_avoider/main.py new file mode 100644 index 0000000..29b8057 --- /dev/null +++ b/Micropython/MONA_simple_collision_avoider/main.py @@ -0,0 +1,2 @@ +from simple_collision_avoider import main +main() diff --git a/Micropython/MONA_simple_collision_avoider/mcp23008.py b/Micropython/MONA_simple_collision_avoider/mcp23008.py new file mode 100644 index 0000000..496544a --- /dev/null +++ b/Micropython/MONA_simple_collision_avoider/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_simple_collision_avoider/mona_esp_lib.py b/Micropython/MONA_simple_collision_avoider/mona_esp_lib.py new file mode 100644 index 0000000..784d921 --- /dev/null +++ b/Micropython/MONA_simple_collision_avoider/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_simple_collision_avoider/neopixel_led.py b/Micropython/MONA_simple_collision_avoider/neopixel_led.py new file mode 100644 index 0000000..1dcf56d --- /dev/null +++ b/Micropython/MONA_simple_collision_avoider/neopixel_led.py @@ -0,0 +1,61 @@ +""" +WS2812B NeoPixel LED Driver for MicroPython +Uses the built-in neopixel module +""" + +from machine import Pin +from neopixel import NeoPixel + + +class NeoPixelLED: + """Simple NeoPixel LED wrapper class""" + + def __init__(self, pin: int, num_leds: int = 1): + """ + Initialize NeoPixel LED + + Args: + pin: GPIO pin number + num_leds: Number of LEDs in the strip + """ + self._pin = Pin(pin, Pin.OUT) + self._np = NeoPixel(self._pin, num_leds) + self._num_leds = num_leds + self.clear() + + def clear(self): + """Turn off all LEDs""" + for i in range(self._num_leds): + self._np[i] = (0, 0, 0) + self._np.write() + + def fill(self, r: int, g: int, b: int): + """ + Fill all LEDs with the same color + + Args: + r: Red component (0-255) + g: Green component (0-255) + b: Blue component (0-255) + """ + for i in range(self._num_leds): + self._np[i] = (r, g, b) + self._np.write() + + def set_pixel(self, index: int, r: int, g: int, b: int): + """ + Set a specific LED color + + Args: + index: LED index + r: Red component (0-255) + g: Green component (0-255) + b: Blue component (0-255) + """ + if 0 <= index < self._num_leds: + self._np[index] = (r, g, b) + self._np.write() + + def show(self): + """Update the LED strip (write changes)""" + self._np.write() diff --git a/Micropython/MONA_simple_collision_avoider/simple_collision_avoider.py b/Micropython/MONA_simple_collision_avoider/simple_collision_avoider.py new file mode 100644 index 0000000..1d5477a --- /dev/null +++ b/Micropython/MONA_simple_collision_avoider/simple_collision_avoider.py @@ -0,0 +1,120 @@ +import time +from mona_esp_lib import ( + mona_esp_init, + motors_forward, + motors_spin_left, + motors_spin_right, + motors_stop, + detect_object, + set_led, +) + +THRESHOLD = 80 + +# Motor speeds +FORWARD_SPEED = 150 +TURN_SPEED = 100 + +# State machine states +STATE_FORWARD = 0 +STATE_OBSTACLE_FRONT = 1 +STATE_OBSTACLE_RIGHT = 2 +STATE_OBSTACLE_LEFT = 3 + +# Loop delay (ms) +LOOP_DELAY_MS = 5 + + +def main(): + # Initialize the MONA robot + print("Initializing MONA ESP...") + mona_esp_init() + print("MONA ESP ready!") + + # Set initial LED color (green = running) + set_led(1, 0, 50, 0) + set_led(2, 0, 50, 0) + + # State machine variables + state = STATE_FORWARD + old_state = STATE_FORWARD + + # IR sensor values (1-5: left, left-diagonal, front, right-diagonal, right) + ir_values = [False, False, False, False, False] + + print("Starting collision avoidance...") + + try: + while True: + if state == STATE_FORWARD: + motors_forward(FORWARD_SPEED) + # LED: Green when moving forward + if state != old_state: + set_led(1, 0, 50, 0) + set_led(2, 0, 50, 0) + + elif state == STATE_OBSTACLE_FRONT: + # Obstacle in front: spin left + motors_spin_left(TURN_SPEED) + # LED: Red when obstacle detected + if state != old_state: + set_led(1, 50, 0, 0) + set_led(2, 50, 0, 0) + + elif state == STATE_OBSTACLE_RIGHT: + # Obstacle on right: spin left + motors_spin_left(TURN_SPEED) + # LED: Yellow (right side obstacle) + if state != old_state: + set_led(1, 50, 25, 0) + set_led(2, 50, 25, 0) + + elif state == STATE_OBSTACLE_LEFT: + # Obstacle on left: spin right + motors_spin_right(TURN_SPEED) + # LED: Blue (left side obstacle) + if state != old_state: + set_led(1, 0, 0, 50) + set_led(2, 0, 0, 50) + + old_state = state + + # ============== IR Sensor Reading ============== + ir_values[0] = detect_object(1, THRESHOLD) # Left + ir_values[1] = detect_object(2, THRESHOLD) # Left diagonal + ir_values[2] = detect_object(3, THRESHOLD) # Front + ir_values[3] = detect_object(4, THRESHOLD) # Right diagonal + ir_values[4] = detect_object(5, THRESHOLD) # Right + + # ============== State Machine ============== + # Determine next state based on IR readings + + # Priority: Front obstacles first + if ir_values[2] or ir_values[3] or ir_values[4]: + # Front, right-diagonal, or right obstacle detected + state = STATE_OBSTACLE_FRONT + elif ir_values[0]: + # Left obstacle: turn right + state = STATE_OBSTACLE_LEFT + elif ir_values[4]: + # Right obstacle: turn left + state = STATE_OBSTACLE_RIGHT + else: + # No obstacles: move forward + state = STATE_FORWARD + + # Small delay + time.sleep_ms(LOOP_DELAY_MS) + + except KeyboardInterrupt: + # Clean shutdown on Ctrl+C + print("\nStopping...") + motors_stop() + set_led(1, 0, 0, 0) + set_led(2, 0, 0, 0) + print("MONA stopped.") + + +# Run main when executed directly +if __name__ == "__main__": + main() \ No newline at end of file From 6a0500ead6839d04594fb7d53e55b4c7603c2389 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 21 Jan 2026 14:20:06 +0900 Subject: [PATCH 09/16] =?UTF-8?q?puppet=EC=9C=BC=EB=A1=9C=20=EC=9D=B4?= =?UTF-8?q?=EB=A6=84=20=EB=B3=80=EA=B2=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Micropython/MONA_puppet/main.py | 2 +- .../MONA_puppet/{mona_udp_controller.py => mona_puppet.py} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename Micropython/MONA_puppet/{mona_udp_controller.py => mona_puppet.py} (100%) diff --git a/Micropython/MONA_puppet/main.py b/Micropython/MONA_puppet/main.py index 1f5a747..c4e5a80 100644 --- a/Micropython/MONA_puppet/main.py +++ b/Micropython/MONA_puppet/main.py @@ -1,2 +1,2 @@ -from mona_udp_controller import main +from mona_puppet import main main() diff --git a/Micropython/MONA_puppet/mona_udp_controller.py b/Micropython/MONA_puppet/mona_puppet.py similarity index 100% rename from Micropython/MONA_puppet/mona_udp_controller.py rename to Micropython/MONA_puppet/mona_puppet.py From 419208a09bb5dd2c1d64c4fdd77289bdcc9fa9cd Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 21 Jan 2026 18:45:13 +0900 Subject: [PATCH 10/16] Update offboard example to onboard logic in SPACE_MONA_offboard.ino --- .../SPACE_MONA_offboard.ino | 820 ++++++++++++------ 1 file changed, 562 insertions(+), 258 deletions(-) diff --git a/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino b/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino index 5533d91..9e4eabf 100644 --- a/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino +++ b/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino @@ -1,27 +1,35 @@ +#include +#include #include #include #include "Mona_ESP_lib.h" -#include -#include #include -#include -#include #include #include #include #include #include #include -#include -#include -#include + +// Behavior Tree library +#include "bt_nodes.h" + +// CBBA library (ported from cbba.py) +#include "cbba.h" // ====== 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; +const int SELF_ID = 0; // Change this for each robot (0-11) +const uint16_t UDP_PORT = 9000; + +// PC Status Report Configuration +const char* PC_HOST = "192.168.0.17"; // PC IP address +const int PC_STATUS_PORT = 9001; // Port for MONA → PC status reports +const unsigned long STATUS_REPORT_INTERVAL_MS = 200; // Report every 200ms + +// CBBA Configuration (matching cbba.py config) +CBBAConfig cbba_config; // JSON buffer size const size_t JSON_SIZE = 2048; @@ -39,8 +47,8 @@ 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 MIN_BROADCAST_MS = 100; //통신 안정화를 위해 변경 +const uint32_t MAX_BROADCAST_MS = 200; const uint32_t PEER_LINK_DROP_MS = 900; const uint32_t WIFI_RECONNECT_INTERVAL_MS = 300; const uint32_t WIFI_TIMEOUT_MS = 10000; @@ -73,11 +81,6 @@ 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; @@ -98,16 +101,23 @@ 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 Message Codec constants +const float BID_SCALE_FACTOR = 100000.0f; +const unsigned long TIMESTAMP_OFFSET = 0UL; + +// Received ESP-NOW messages storage +struct ReceivedMessage { + StaticJsonDocument<1024> msg; + unsigned long timestamp; +}; +std::map receivedMessages_MAP; // ===== CBBA Communication (Core 0) ===== -DynamicJsonDocument selfMessageDoc(JSON_SIZE); -std::map receivedJSON_MAP; -std::map CommRecvTime_MAP; -SemaphoreHandle_t mapLock; -SemaphoreHandle_t selfMsgLock; // NEW: selfMessageDoc 보호 +StaticJsonDocument selfMessageDoc; +StaticJsonDocument messageToShare; +bool hasMessageToShare = false; unsigned long lastBroadcast = 0; volatile bool dirtySelf = false; @@ -122,10 +132,14 @@ 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 float targetX = 0; +volatile float targetY = 0; +volatile bool hasTargetPosition = false; + +// Position from PC +volatile float positionX = 0; +volatile float positionY = 0; +volatile float heading = 0; volatile long left_encoder_count = 0; volatile long right_encoder_count = 0; @@ -138,9 +152,7 @@ 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; -TaskHandle_t commTaskHandle = NULL; TaskHandle_t motionTaskHandle = NULL; static inline void clear_motion_targets() { @@ -148,7 +160,6 @@ static inline void clear_motion_targets() { 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) { @@ -191,53 +202,209 @@ static inline void check_oscillation_and_escape(int current_direction) { } // ============================================================================= -// ESP-NOW COMMUNICATION (Core 0) +// TASK MANAGER - Manages tasks received from PC // ============================================================================= -bool update_Broadcast_recv_JSON_MAP(const String& senderID, const char* jsonBuf, size_t jsonLen) { - if (jsonLen < 2) return false; - - // 콜백에서 풀 파싱은 부담 큼 -> 아주 가벼운 형태 체크만 - const char first = jsonBuf[0]; - const char last = jsonBuf[jsonLen - 1]; - if (!((first == '{' && last == '}') || (first == '[' && last == ']'))) { - return false; - } +class TaskManager { +public: + TaskManager() : _last_update_ms(0) {} + + void update_from_message(const std::vector>& task_list) { + std::set received_ids; + + for (const auto& task_data : task_list) { + if (task_data.size() < 4) continue; + + int task_id = (int)task_data[0]; + float x = task_data[1]; + float y = task_data[2]; + float amount = task_data[3]; + + received_ids.insert(task_id); + + auto it = _tasks.find(task_id); + if (it != _tasks.end()) { + it->second.x = x; + it->second.y = y; + it->second.amount = amount; + it->second.completed = (amount <= 0); + } else { + _tasks[task_id] = CBBATask(task_id, x, y, amount); + } + } - // 수신 콜백에서 오래 잠그지 않기: 즉시 락 실패 시 드랍 - if (xSemaphoreTake(mapLock, 0) != pdTRUE) { - return false; - } + for (auto& pair : _tasks) { + if (received_ids.find(pair.first) == received_ids.end()) { + pair.second.completed = true; + } + } - // 기존 String capacity 재사용(힙 단편화 완화) - String& dst = receivedJSON_MAP[senderID]; - dst.remove(0); - dst.reserve(jsonLen + 1); - dst.concat(jsonBuf, jsonLen); + auto it = _tasks.begin(); + while (it != _tasks.end()) { + if (it->second.completed) { + it = _tasks.erase(it); + } else { + ++it; + } + } - CommRecvTime_MAP[senderID] = millis(); - xSemaphoreGive(mapLock); - return true; -} + _last_update_ms = millis(); + } -void onEspNowRecv(const esp_now_recv_info_t *recv_info, const uint8_t *incoming, int len) { - (void)recv_info; - if (len <= 1) return; + CBBATask* get_task(int task_id) { + auto it = _tasks.find(task_id); + return (it != _tasks.end()) ? &(it->second) : nullptr; + } + + std::vector get_all_tasks() { + std::vector result; + for (auto& pair : _tasks) { + if (!pair.second.completed) { + result.push_back(pair.second); + } + } + return result; + } + + std::vector get_all_tasks_ptr() { + _task_ptrs.clear(); + for (auto& pair : _tasks) { + if (!pair.second.completed) { + _task_ptrs.push_back(reinterpret_cast(&pair.second)); + } + } + return _task_ptrs; + } + + int get_task_count() const { + int count = 0; + for (const auto& pair : _tasks) { + if (!pair.second.completed) count++; + } + return count; + } + + bool is_task_valid(int task_id) const { + auto it = _tasks.find(task_id); + return (it != _tasks.end() && !it->second.completed); + } + + void clear() { _tasks.clear(); } + +private: + std::map _tasks; + std::vector _task_ptrs; + unsigned long _last_update_ms; +}; + +TaskManager* taskManager = nullptr; + +// ============================================================================= +// MESSAGE CODEC - Encodes/Decodes CBBA messages for ESP-NOW +// ============================================================================= +class MessageCodec { +public: + static bool encode(int agent_id, const JsonDocument& message, JsonDocument& output) { + output.clear(); + if (message.isNull()) return false; + output["id"] = agent_id; + JsonObject y_out = output.createNestedObject("y"); + if (message.containsKey("winning_bids")) { + JsonObjectConst bids = message["winning_bids"].as(); + for (JsonPairConst kv : bids) { + float bid = kv.value().as(); + int task_id = atoi(kv.key().c_str()); + y_out[String(task_id)] = (long)(bid * BID_SCALE_FACTOR); + } + } + JsonObject z_out = output.createNestedObject("z"); + if (message.containsKey("winning_agents")) { + JsonObjectConst agents = message["winning_agents"].as(); + for (JsonPairConst kv : agents) { + int winner = kv.value().as(); + int task_id = atoi(kv.key().c_str()); + z_out[String(task_id)] = winner; + } + } + JsonObject s_out = output.createNestedObject("s"); + if (message.containsKey("message_received_time_stamp")) { + JsonObjectConst timestamps = message["message_received_time_stamp"].as(); + for (JsonPairConst kv : timestamps) { + unsigned long ts = kv.value().as(); + int ag_id = atoi(kv.key().c_str()); + s_out[String(ag_id)] = (long)(ts - TIMESTAMP_OFFSET); + } + } + return true; + } - uint8_t idLen = incoming[0]; + static bool decode(const JsonDocument& payload, JsonDocument& output) { + output.clear(); + if (!payload.containsKey("y")) return false; + output["agent_id"] = payload["id"].as(); + JsonObject bids_out = output.createNestedObject("winning_bids"); + if (payload.containsKey("y")) { + JsonObjectConst y = payload["y"].as(); + for (JsonPairConst kv : y) { + long val = kv.value().as(); + bids_out[kv.key()] = (float)val / BID_SCALE_FACTOR; + } + } + JsonObject agents_out = output.createNestedObject("winning_agents"); + if (payload.containsKey("z")) { + JsonObjectConst z = payload["z"].as(); + for (JsonPairConst kv : z) { + int winner = kv.value().as(); + agents_out[kv.key()] = winner; + } + } + JsonObject ts_out = output.createNestedObject("message_received_time_stamp"); + if (payload.containsKey("s")) { + JsonObjectConst s = payload["s"].as(); + for (JsonPairConst kv : s) { + long ts = kv.value().as(); + ts_out[kv.key()] = (unsigned long)(ts + TIMESTAMP_OFFSET); + } + } + return true; + } +}; + +// ============================================================================= +// ESP-NOW COMMUNICATION (Core 0) +// ============================================================================= +void handleEspNowMessage(const uint8_t* data, int len) { + if (len <= 1) return; + uint8_t idLen = data[0]; if (len < 1 + idLen) return; - String senderID = String((const char*)(&incoming[1]), idLen); + char sender_id_str[16]; + memcpy(sender_id_str, data + 1, idLen); + sender_id_str[idLen] = '\0'; + int senderID = atoi(sender_id_str); if (senderID == SELF_ID) return; - int jsonLen = len - (1 + idLen); - if (jsonLen <= 0) return; + const uint8_t* jsonData = data + 1 + idLen; + size_t jsonLen = len - 1 - idLen; + + StaticJsonDocument<1024> payload; + DeserializationError error = deserializeJson(payload, jsonData, jsonLen); + if (error) return; - if (update_Broadcast_recv_JSON_MAP(senderID, (const char*)(&incoming[1 + idLen]), (size_t)jsonLen)) { + StaticJsonDocument<1024> decoded; + if (MessageCodec::decode(payload, decoded)) { + ReceivedMessage& msg = receivedMessages_MAP[senderID]; + msg.msg = decoded; + msg.timestamp = millis(); espnow_rx_bytes += (uint32_t)len; dirtyNeighbors = true; } } +void onEspNowRecv(const esp_now_recv_info_t *recv_info, const uint8_t *incoming, int len) { + (void)recv_info; + handleEspNowMessage(incoming, len); +} + void ensureBroadcastPeer() { uint8_t bcast[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; if (esp_now_is_peer_exist(bcast)) return; @@ -252,34 +419,32 @@ 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 (!hasMessageToShare) return; - if (selfMessageDoc.isNull() || selfMessageDoc.size() == 0) { - xSemaphoreGive(selfMsgLock); - return; - } + unsigned long now = millis(); + unsigned long interval = random(MIN_BROADCAST_MS, MAX_BROADCAST_MS + 1); + if (now - lastBroadcast < interval) return; + + lastBroadcast = now; ensureBroadcastPeer(); - size_t jsonLen = serializeJson(selfMessageDoc, g_jsonBuf, sizeof(g_jsonBuf)); - xSemaphoreGive(selfMsgLock); - + // Encode message using MessageCodec + StaticJsonDocument<1024> encoded; + if (!MessageCodec::encode(SELF_ID, messageToShare, encoded)) return; + + size_t jsonLen = serializeJson(encoded, g_jsonBuf, sizeof(g_jsonBuf)); if (jsonLen == 0) return; - uint8_t idLen = (uint8_t)SELF_ID.length(); + char idStr[16]; + snprintf(idStr, sizeof(idStr), "%d", SELF_ID); + uint8_t idLen = strlen(idStr); 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], idStr, idLen); memcpy(&g_pkt[1 + idLen], g_jsonBuf, jsonLen); uint8_t bcast[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; @@ -288,167 +453,129 @@ void broadcastSelfMessageIfDue() { } // ============================================================================= -// TCP COMMUNICATION (Core 0) +// Get nearby agents (from ESP-NOW messages) // ============================================================================= -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); - } +std::vector getNearbyAgents() { + std::vector nearby; + unsigned long now = millis(); + for (const auto& pair : receivedMessages_MAP) { + if (now - pair.second.timestamp <= PEER_LINK_DROP_MS) { + nearby.push_back(pair.first); } } + return nearby; } -void sendMonitorToTcpClients() { - static unsigned long lastTcpSend = 0; - if (millis() - lastTcpSend < 50) return; +// ============================================================================= +// UDP COMMUNICATION (Core 0) - Position & Tasks from PC +// ============================================================================= +std::vector> tasksRaw; - // [FIX #1] 큰 DynamicJsonDocument 제거: - // mapLock을 짧게 잡고 스냅샷만 뜬 뒤, 락 풀고 String으로 한 줄 JSON 구성 - std::vector> snapshot; - snapshot.reserve(16); +void handleUdpPacket() { + int packetSize = udp.parsePacket(); + if (!packetSize) return; - const unsigned long now = millis(); + char buffer[2048]; + int len = udp.read(buffer, sizeof(buffer) - 1); + if (len <= 0) return; + buffer[len] = '\0'; - 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); - } + StaticJsonDocument<2048> doc; + DeserializationError error = deserializeJson(doc, buffer, len); + if (error) return; - // 한 줄 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 + // Check if message is for this robot + if (doc.containsKey("id")) { + int msg_id = doc["id"]; + if (msg_id != SELF_ID) return; } - out += "}}\n"; - - for (auto &c : tcpClients) { - if (c && c.connected()) { - c.print(out); - } + // Update position + if (doc.containsKey("x") && doc.containsKey("y")) { + positionX = doc["x"]; + positionY = doc["y"]; + heading = doc.containsKey("yaw") ? doc["yaw"].as() : 0.0f; } - dirtySelf = dirtyNeighbors = false; - lastTcpSend = millis(); - - // Cleanup disconnected clients - for (auto &c : tcpClients) { - if (c && !c.connected()) c.stop(); + // Update tasks + if (doc.containsKey("t")) { + tasksRaw.clear(); + JsonArray tasks = doc["t"]; + for (JsonArray task : tasks) { + if (task.size() >= 4) { + std::vector task_data; + task_data.push_back(task[0]); + task_data.push_back(task[1]); + task_data.push_back(task[2]); + task_data.push_back(task[3]); + tasksRaw.push_back(task_data); + } + } } - 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) +// Send CBBA Status to PC // ============================================================================= -void handleUdpPacket() { - static char udpBuffer[256]; - - int packetSize = udp.parsePacket(); - if (!packetSize) return; +unsigned long lastStatusReportMs = 0; - // Keep only latest packet - while (packetSize) { - int len = udp.read(udpBuffer, 255); - if (len > 0) udpBuffer[len] = 0; - packetSize = udp.parsePacket(); +void sendStatusToPC(CBBA* cbba) { + unsigned long now = millis(); + if (now - lastStatusReportMs < STATUS_REPORT_INTERVAL_MS) return; + lastStatusReportMs = now; + + StaticJsonDocument<512> doc; + doc["type"] = "status"; + doc["id"] = SELF_ID; + doc["assigned"] = cbba->get_assigned_task_id(); + + // Bundle (planned tasks) + JsonArray bundle_arr = doc.createNestedArray("bundle"); + const std::vector& bundle = cbba->get_bundle(); + for (int task_id : bundle) { + bundle_arr.add(task_id); } - // STOP command - if (strncasecmp(udpBuffer, "STOP", 4) == 0) { - stopRequested = true; - return; + // Nearby agents (ESP-NOW connections) + JsonArray nearby_arr = doc.createNestedArray("nearby"); + std::vector nearby = getNearbyAgents(); + for (int agent_id : nearby) { + nearby_arr.add(agent_id); } - // 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 - } - } + // Serialize and send + char json_buffer[512]; + size_t len = serializeJson(doc, json_buffer, sizeof(json_buffer)); + if (len > 0) { + udp.beginPacket(PC_HOST, PC_STATUS_PORT); + udp.write((const uint8_t*)json_buffer, len); + udp.endPacket(); } } // ============================================================================= -// COMMUNICATION TASK (Core 0) - HIGH PRIORITY +// MOTION CONTROL TASK (Core 1) - INDEPENDENT // ============================================================================= -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 +static float normalize_angle(float angle) { + while (angle > M_PI) angle -= 2 * M_PI; + while (angle < -M_PI) angle += 2 * M_PI; + return angle; +} - // Short delay to prevent watchdog trigger - vTaskDelay(pdMS_TO_TICKS(1)); +void compute_motion(float& angle_deg, float& dist_mm) { + if (!hasTargetPosition) { + angle_deg = 0; + dist_mm = 0; + return; } + float dx = targetX - positionX; + float dy = targetY - positionY; + dist_mm = sqrtf(dx * dx + dy * dy); + float desired_heading = atan2f(dy, dx); + float angle_diff = normalize_angle(desired_heading - heading); + angle_deg = angle_diff * 180.0f / M_PI; } -// ============================================================================= -// 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; @@ -456,7 +583,6 @@ void start_motion(float angle_deg, float dist_mm) { 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); @@ -510,7 +636,6 @@ 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; state = STATE_MOVING; } else { state = STATE_IDLE; @@ -523,13 +648,16 @@ 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); + // Use target-based motion update instead of fixed distance + if (hasTargetPosition && avg >= UPDATE_THRESHOLD_PULSES) { + float angle_deg, dist_mm; + compute_motion(angle_deg, dist_mm); + if (dist_mm >= MIN_DIST_MM) { + start_motion(angle_deg, dist_mm); + return; + } + } + Motors_forward(FWD_SPD); } break; @@ -571,31 +699,57 @@ void control_loop(int r1, int r2, int r3, int r4, int r5) { } } +// LED Status indicator +void update_led(RobotState state, bool has_task) { + if (state == STATE_IDLE) { + if (has_task) { + Set_LED(1, 0, 50, 0); + Set_LED(2, 0, 50, 0); + } else { + Set_LED(1, 0, 0, 30); + Set_LED(2, 0, 0, 30); + } + } else if (state == STATE_TURNING) { + Set_LED(1, 0, 0, 50); + Set_LED(2, 0, 0, 50); + } else if (state == STATE_MOVING) { + Set_LED(1, 0, 50, 0); + Set_LED(2, 0, 50, 0); + } else if (state == STATE_AVOID) { + Set_LED(1, 50, 50, 0); + Set_LED(2, 50, 50, 0); + } else if (state == STATE_ESCAPING) { + Set_LED(1, 50, 25, 0); + Set_LED(2, 50, 25, 0); + } else if (state == STATE_EMERGENCY) { + Set_LED(1, 50, 0, 0); + Set_LED(2, 50, 0, 0); + } +} + void motionTask(void* parameter) { Serial.println("[Core 1] Motion control task started"); + int led_counter = 0; for (;;) { - // Check for STOP command from Core 0 - if (stopRequested) { - Motors_stop(); - clear_motion_targets(); - state = STATE_IDLE; - stopRequested = false; + // Check if target is cleared + if (!hasTargetPosition) { + if (state != STATE_AVOID && state != STATE_ESCAPING && state != STATE_EMERGENCY) { + Motors_stop(); + clear_motion_targets(); + state = STATE_IDLE; + } } - // 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); + // Handle new target position + if (hasTargetPosition) { + if (state == STATE_IDLE) { + float angle_deg, dist_mm; + compute_motion(angle_deg, dist_mm); + if (dist_mm >= MIN_DIST_MM) { + start_motion(angle_deg, dist_mm); } - } else if (state == STATE_IDLE || state == STATE_TURNING) { - start_motion(targetAngleDeg, targetDistMm); } - newMotionCommand = false; } // Read IR sensors (this is slow, but doesn't block Core 0 now!) @@ -608,11 +762,62 @@ void motionTask(void* parameter) { // Run motion control control_loop(r1, r2, r3, r4, r5); + // Update LED status + led_counter++; + if (led_counter >= 50) { + update_led(state, hasTargetPosition); + led_counter = 0; + } + // Motion task runs at ~50Hz (20ms period) vTaskDelay(pdMS_TO_TICKS(50)); } } +// ============================================================================= +// BEHAVIOR TREE BUILDER +// ============================================================================= +BTNode* build_default_tree_with_explore() { + return TreeBuilder::ReactiveSequence("Root", { + new GatherLocalInfo(), + TreeBuilder::ReactiveSequence("MainSequence", { + TreeBuilder::ReactiveFallback("TaskAssignmentFallback", { + new AssignTask(), + new Explore() + }), + TreeBuilder::ReactiveFallback("TaskExecutionFallback", { + new IsTaskCompleted(), + TreeBuilder::ReactiveSequence("ExecutionSequence", { + TreeBuilder::ReactiveFallback("MovementFallback", { + new IsArrivedAtTarget(), + new MoveToTarget() + }), + new ExecuteTask() + }) + }) + }) + }); +} + +BTNode* build_default_tree_no_explore() { + return TreeBuilder::ReactiveSequence("Root", { + new GatherLocalInfo(), + TreeBuilder::ReactiveSequence("MainSequence", { + new AssignTask(), + TreeBuilder::ReactiveFallback("TaskExecutionFallback", { + new IsTaskCompleted(), + TreeBuilder::ReactiveSequence("ExecutionSequence", { + TreeBuilder::ReactiveFallback("MovementFallback", { + new IsArrivedAtTarget(), + new MoveToTarget() + }), + new ExecuteTask() + }) + }) + }) + }); +} + void setupNetwork() { WiFi.mode(WIFI_STA); @@ -641,7 +846,7 @@ void setupNetwork() { 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("[WiFi] Board ID (SELF_ID): %d\n", SELF_ID); // 실제 채널 출력 (ESPNOW는 이 채널과 동일해야 함) uint8_t primary = 0; @@ -649,10 +854,6 @@ void setupNetwork() { esp_wifi_get_channel(&primary, &second); Serial.printf("[WiFi] Channel: %d\n", primary); - // 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); @@ -676,8 +877,6 @@ void setup() { delay(SERIAL_STABILIZE_DELAY_MS); randomSeed(esp_random()); - mapLock = xSemaphoreCreateMutex(); - selfMsgLock = xSemaphoreCreateMutex(); // Initialize Mona robot hardware Mona_ESP_init(); @@ -690,23 +889,33 @@ void setup() { 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 - xTaskCreatePinnedToCore( - commTask, // Task function - "CommTask", // Name - 8192, // Stack size - NULL, // Parameters - 2, // Priority (higher = more important) - &commTaskHandle, // Task handle - 0 // Core 0 - ); + Serial.println("=================================================="); + Serial.printf("MONA Onboard Firmware - Agent %d\n", SELF_ID); + Serial.println("ESP-NOW v2 + PC Status Report"); + Serial.println("CBBA + Behavior Tree | Onboard Processing"); + Serial.printf("PC Status: %s:%d\n", PC_HOST, PC_STATUS_PORT); + Serial.println("=================================================="); + + // Initialize CBBA configuration + cbba_config.max_tasks_per_agent = 4; + cbba_config.lambda = 0.999f; + cbba_config.winning_bid_cancel = true; + cbba_config.no_bundle_duration_limit = 5.0f; + cbba_config.keep_moving_during_convergence = false; + cbba_config.work_rate = 1.0f; + cbba_config.max_speed = 0.25f; + + // Initialize Task Manager + taskManager = new TaskManager(); + + // Initialize CBBA + CBBA* cbba = new CBBA(SELF_ID, cbba_config); + + // Initialize Behavior Tree + BTContext* bt_ctx = new BTContext(SELF_ID); + bt_ctx->cbba = cbba; + BTNode* tree_root = build_default_tree_no_explore(); + BehaviorTree* bt = new BehaviorTree(tree_root); // Create Motion Task on Core 1 (APP_CPU) // Lower priority (1), independent from communication @@ -719,6 +928,101 @@ void setup() { &motionTaskHandle, // Task handle 1 // Core 1 ); + + Serial.println("=================================================="); + Serial.println("System ready!"); + Serial.println("=================================================="); + + // Main loop timing + unsigned long last_bt_tick_ms = 0; + const unsigned long bt_interval_ms = 100; + unsigned long last_debug_ms = 0; + const unsigned long debug_interval_ms = 2000; + + // Main loop runs on Core 0 + for (;;) { + unsigned long now = millis(); + + // WiFi reconnection + if (WiFi.status() != WL_CONNECTED) { + Serial.println("[WiFi] Reconnecting..."); + WiFi.reconnect(); + vTaskDelay(pdMS_TO_TICKS(100)); + continue; + } + + // ===== UDP: Receive position & tasks from PC ===== + handleUdpPacket(); + + // Update task manager with received tasks + if (!tasksRaw.empty()) { + taskManager->update_from_message(tasksRaw); + bt_ctx->tasks = taskManager->get_all_tasks_ptr(); + + // Clear completed tasks from CBBA + int current_task = cbba->get_assigned_task_id(); + if (current_task >= 0 && !taskManager->is_task_valid(current_task)) { + cbba->clear_task(current_task); + } + } + + // Update positions for CBBA and BT context + bt_ctx->position_x = positionX; + bt_ctx->position_y = positionY; + bt_ctx->yaw = heading; + cbba->set_position(positionX, positionY); + + // ===== ESP-NOW: Process received messages ===== + unsigned long msg_now = millis(); + std::vector> received_msgs; + auto it = receivedMessages_MAP.begin(); + while (it != receivedMessages_MAP.end()) { + if (msg_now - it->second.timestamp <= PEER_LINK_DROP_MS) { + received_msgs.push_back(it->second.msg); + ++it; + } else { + it = receivedMessages_MAP.erase(it); + } + } + for (auto& msg : received_msgs) { + cbba->receive_message_json(msg); + } + + // ===== ESP-NOW: Broadcast CBBA message ===== + cbba->get_message_to_share_json(messageToShare); + hasMessageToShare = true; + broadcastSelfMessageIfDue(); + + // ===== UDP: Send status to PC ===== + sendStatusToPC(cbba); + + // ===== Behavior Tree: Tick ===== + if (now - last_bt_tick_ms >= bt_interval_ms) { + last_bt_tick_ms = now; + std::vector cbba_tasks = taskManager->get_all_tasks(); + int assigned_id = cbba->decide(cbba_tasks); + bt_ctx->assigned_task_id = assigned_id; + bt->tick(*bt_ctx); + + // Update motion target from BT + if (bt_ctx->has_target_position) { + targetX = bt_ctx->target_position_x; + targetY = bt_ctx->target_position_y; + hasTargetPosition = true; + } else { + hasTargetPosition = false; + } + } + + // ===== Debug output ===== + if (now - last_debug_ms >= debug_interval_ms) { + last_debug_ms = now; + cbba->print_state(); + } + + // Short delay to prevent watchdog trigger + vTaskDelay(pdMS_TO_TICKS(5)); + } } void loop() { From eee9e6091d517cb4e43e2ceea514eee1a9e85638 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 21 Jan 2026 18:49:55 +0900 Subject: [PATCH 11/16] Add Behavior Tree nodes implementation for onboard control --- src/bt_nodes.cpp | 499 +++++++++++++++++++++++++++++++++++++++++++++++ src/bt_nodes.h | 314 +++++++++++++++++++++++++++++ 2 files changed, 813 insertions(+) create mode 100644 src/bt_nodes.cpp create mode 100644 src/bt_nodes.h diff --git a/src/bt_nodes.cpp b/src/bt_nodes.cpp new file mode 100644 index 0000000..0cda58f --- /dev/null +++ b/src/bt_nodes.cpp @@ -0,0 +1,499 @@ +/* + bt_nodes.cpp - Behavior Tree nodes implementation for MONA ESP robot + + Ported from Python implementation (base_bt_nodes.py, bt_nodes.py) + for onboard execution on ESP32. +*/ + +#include "bt_nodes.h" + +// ===================== BTContext Implementation ===================== + +BTContext::BTContext(int id) + : agent_id(id), + position_x(0), position_y(0), yaw(0), + assigned_task_id(-1), + cbba(nullptr), + arrive_threshold(40.0), + work_rate(1.0), + explore_target_x(0), explore_target_y(0), + has_explore_target(false), + explore_start_ms(0), + explore_duration_ms(2000), + has_target_position(false), + target_position_x(0), target_position_y(0) +{ + // Default exploration bounds + explore_bounds.x_min = 40; + explore_bounds.x_max = 1300; + explore_bounds.y_min = 40; + explore_bounds.y_max = 700; +} + +void BTContext::reset_exploration() { + has_explore_target = false; + explore_start_ms = 0; +} + +// ===================== BTNode Base Class ===================== + +BTNode::BTNode(const char* name) + : _name(name), _status(BT_FAILURE) +{ +} + +void BTNode::halt() { + // Default implementation does nothing +} + +void BTNode::reset() { + _status = BT_FAILURE; +} + +// ===================== Sequence ===================== + +Sequence::Sequence(const char* name, std::vector children) + : BTNode(name), _children(children), _current_child_index(0) +{ +} + +BTStatus Sequence::tick(BTContext& ctx) { + while (_current_child_index < _children.size()) { + BTStatus status = _children[_current_child_index]->tick(ctx); + _status = status; + + if (status == BT_RUNNING) { + return BT_RUNNING; + } else if (status == BT_FAILURE) { + _halt_children(); + _current_child_index = 0; + return BT_FAILURE; + } else if (status == BT_SUCCESS) { + _current_child_index++; + } + } + + _current_child_index = 0; + _halt_children(); + return BT_SUCCESS; +} + +void Sequence::halt() { + _current_child_index = 0; + _halt_children(); +} + +void Sequence::reset() { + BTNode::reset(); + _current_child_index = 0; + for (auto* child : _children) { + child->reset(); + } +} + +void Sequence::_halt_children() { + for (auto* child : _children) { + child->halt(); + } +} + +// ===================== ReactiveSequence ===================== + +ReactiveSequence::ReactiveSequence(const char* name, std::vector children) + : BTNode(name), _children(children) +{ +} + +BTStatus ReactiveSequence::tick(BTContext& ctx) { + for (auto* child : _children) { + BTStatus status = child->tick(ctx); + _status = status; + + if (status == BT_FAILURE) { + _halt_children(); + return BT_FAILURE; + } + if (status == BT_RUNNING) { + return BT_RUNNING; + } + } + + _halt_children(); + return BT_SUCCESS; +} + +void ReactiveSequence::halt() { + _halt_children(); +} + +void ReactiveSequence::_halt_children() { + for (auto* child : _children) { + child->halt(); + } +} + +// ===================== Fallback ===================== + +Fallback::Fallback(const char* name, std::vector children) + : BTNode(name), _children(children), _current_child_index(0) +{ +} + +BTStatus Fallback::tick(BTContext& ctx) { + while (_current_child_index < _children.size()) { + BTStatus status = _children[_current_child_index]->tick(ctx); + _status = status; + + if (status == BT_RUNNING) { + return BT_RUNNING; + } else if (status == BT_SUCCESS) { + _halt_children(); + _current_child_index = 0; + return BT_SUCCESS; + } else if (status == BT_FAILURE) { + _current_child_index++; + } + } + + _current_child_index = 0; + _halt_children(); + return BT_FAILURE; +} + +void Fallback::halt() { + _current_child_index = 0; + _halt_children(); +} + +void Fallback::reset() { + BTNode::reset(); + _current_child_index = 0; + for (auto* child : _children) { + child->reset(); + } +} + +void Fallback::_halt_children() { + for (auto* child : _children) { + child->halt(); + } +} + +// ===================== ReactiveFallback ===================== + +ReactiveFallback::ReactiveFallback(const char* name, std::vector children) + : BTNode(name), _children(children) +{ +} + +BTStatus ReactiveFallback::tick(BTContext& ctx) { + for (auto* child : _children) { + BTStatus status = child->tick(ctx); + _status = status; + + if (status == BT_SUCCESS) { + _halt_children(); + return BT_SUCCESS; + } + if (status == BT_RUNNING) { + return BT_RUNNING; + } + } + + _halt_children(); + return BT_FAILURE; +} + +void ReactiveFallback::halt() { + _halt_children(); +} + +void ReactiveFallback::_halt_children() { + for (auto* child : _children) { + child->halt(); + } +} + +// ===================== GatherLocalInfo ===================== + +GatherLocalInfo::GatherLocalInfo(const char* name) + : BTNode(name) +{ +} + +BTStatus GatherLocalInfo::tick(BTContext& ctx) { + // This node mainly serves as a sync point in the tree + // CBBA position is set externally when UDP message is received + _status = BT_SUCCESS; + return BT_SUCCESS; +} + +// ===================== AssignTask ===================== + +AssignTask::AssignTask(const char* name) + : BTNode(name) +{ +} + +BTStatus AssignTask::tick(BTContext& ctx) { + // Check if a task is assigned (CBBA::decide() is called from main code) + int task_id = ctx.assigned_task_id; + + if (task_id >= 0) { + // Verify task is still valid + bool task_valid = false; + for (Task* task : ctx.tasks) { + if (task != nullptr && task->task_id == task_id) { + task_valid = true; + break; + } + } + + if (task_valid) { + _status = BT_SUCCESS; + return BT_SUCCESS; + } + } + + // No valid task assigned + ctx.assigned_task_id = -1; + _status = BT_FAILURE; + return BT_FAILURE; +} + +// ===================== MoveToTarget ===================== + +MoveToTarget::MoveToTarget(const char* name) + : BTNode(name) +{ +} + +BTStatus MoveToTarget::tick(BTContext& ctx) { + if (ctx.assigned_task_id < 0) { + _status = BT_FAILURE; + return BT_FAILURE; + } + + // Find assigned task + Task* assigned_task = nullptr; + for (Task* task : ctx.tasks) { + if (task != nullptr && task->task_id == ctx.assigned_task_id) { + assigned_task = task; + break; + } + } + + if (assigned_task == nullptr) { + _status = BT_FAILURE; + return BT_FAILURE; + } + + // Set motion target + ctx.has_target_position = true; + ctx.target_position_x = assigned_task->x; + ctx.target_position_y = assigned_task->y; + + _status = BT_RUNNING; + return BT_RUNNING; +} + +// ===================== ExecuteTask ===================== + +ExecuteTask::ExecuteTask(const char* name) + : BTNode(name) +{ +} + +BTStatus ExecuteTask::tick(BTContext& ctx) { + if (ctx.assigned_task_id < 0) { + _status = BT_FAILURE; + return BT_FAILURE; + } + + // Find assigned task + Task* assigned_task = nullptr; + for (Task* task : ctx.tasks) { + if (task != nullptr && task->task_id == ctx.assigned_task_id) { + assigned_task = task; + break; + } + } + + if (assigned_task == nullptr) { + _status = BT_FAILURE; + return BT_FAILURE; + } + + // In PC-managed mode, work processing is handled by PC + // Robot just maintains position at task location + ctx.has_target_position = true; + ctx.target_position_x = assigned_task->x; + ctx.target_position_y = assigned_task->y; + + _status = BT_RUNNING; + return BT_RUNNING; +} + +// ===================== Explore ===================== + +Explore::Explore(const char* name) + : BTNode(name) +{ +} + +BTStatus Explore::tick(BTContext& ctx) { + unsigned long now = millis(); + + // Check if we need a new exploration target + if (!ctx.has_explore_target || + (now - ctx.explore_start_ms) >= ctx.explore_duration_ms) { + + // Generate random waypoint within bounds + ctx.explore_target_x = random( + (long)ctx.explore_bounds.x_min, + (long)ctx.explore_bounds.x_max + 1 + ); + ctx.explore_target_y = random( + (long)ctx.explore_bounds.y_min, + (long)ctx.explore_bounds.y_max + 1 + ); + ctx.has_explore_target = true; + ctx.explore_start_ms = now; + } + + // Set motion target to exploration waypoint + ctx.has_target_position = true; + ctx.target_position_x = ctx.explore_target_x; + ctx.target_position_y = ctx.explore_target_y; + + _status = BT_RUNNING; + return BT_RUNNING; +} + +void Explore::halt() { + BTNode::halt(); +} + +// ===================== IsTaskCompleted ===================== + +IsTaskCompleted::IsTaskCompleted(const char* name) + : BTNode(name) +{ +} + +BTStatus IsTaskCompleted::tick(BTContext& ctx) { + if (ctx.assigned_task_id < 0) { + _status = BT_FAILURE; + return BT_FAILURE; + } + + // Check if task is still in the task list + bool task_found = false; + for (Task* task : ctx.tasks) { + if (task != nullptr && task->task_id == ctx.assigned_task_id) { + if (task->completed) { + ctx.assigned_task_id = -1; + _status = BT_SUCCESS; + return BT_SUCCESS; + } + task_found = true; + break; + } + } + + if (!task_found) { + // Task no longer in list - completed + ctx.assigned_task_id = -1; + _status = BT_SUCCESS; + return BT_SUCCESS; + } + + _status = BT_FAILURE; + return BT_FAILURE; +} + +// ===================== IsArrivedAtTarget ===================== + +IsArrivedAtTarget::IsArrivedAtTarget(const char* name) + : BTNode(name) +{ +} + +BTStatus IsArrivedAtTarget::tick(BTContext& ctx) { + if (ctx.assigned_task_id < 0) { + _status = BT_FAILURE; + return BT_FAILURE; + } + + // Find assigned task + Task* assigned_task = nullptr; + for (Task* task : ctx.tasks) { + if (task != nullptr && task->task_id == ctx.assigned_task_id) { + assigned_task = task; + break; + } + } + + if (assigned_task == nullptr) { + _status = BT_FAILURE; + return BT_FAILURE; + } + + // Calculate distance to task + float dx = assigned_task->x - ctx.position_x; + float dy = assigned_task->y - ctx.position_y; + float distance = sqrt(dx * dx + dy * dy); + + if (distance < ctx.arrive_threshold) { + _status = BT_SUCCESS; + return BT_SUCCESS; + } + + _status = BT_FAILURE; + return BT_FAILURE; +} + +// ===================== BehaviorTree ===================== + +BehaviorTree::BehaviorTree(BTNode* root) + : _root(root) +{ +} + +BehaviorTree::~BehaviorTree() { + // Note: In production, implement proper cleanup +} + +BTStatus BehaviorTree::tick(BTContext& ctx) { + ctx.has_target_position = false; + + if (_root == nullptr) { + return BT_FAILURE; + } + + return _root->tick(ctx); +} + +void BehaviorTree::reset() { + if (_root != nullptr) { + _root->reset(); + } +} + +// ===================== TreeBuilder ===================== + +BTNode* TreeBuilder::Sequence(const char* name, std::vector children) { + return new ::Sequence(name, children); +} + +BTNode* TreeBuilder::ReactiveSequence(const char* name, std::vector children) { + return new ::ReactiveSequence(name, children); +} + +BTNode* TreeBuilder::Fallback(const char* name, std::vector children) { + return new ::Fallback(name, children); +} + +BTNode* TreeBuilder::ReactiveFallback(const char* name, std::vector children) { + return new ::ReactiveFallback(name, children); +} diff --git a/src/bt_nodes.h b/src/bt_nodes.h new file mode 100644 index 0000000..ed1532f --- /dev/null +++ b/src/bt_nodes.h @@ -0,0 +1,314 @@ +/* + bt_nodes.h - Behavior Tree nodes for MONA ESP robot + + Ported from Python implementation (base_bt_nodes.py, bt_nodes.py) + for onboard execution on ESP32. +*/ + +#ifndef BT_NODES_H +#define BT_NODES_H + +#include +#include +#include +#include + +// Forward declaration for CBBA (defined in cbba.h) +class CBBA; + +// ===================== BT Status ===================== +enum BTStatus { + BT_SUCCESS = 1, + BT_FAILURE = 2, + BT_RUNNING = 3 +}; + +// ===================== Task Class ===================== +/** + * Simple task representation for onboard processing. + * Defined here so BT nodes can access task properties. + */ +class Task { +public: + int task_id; + float x; + float y; + float amount; + bool completed; + + Task() : task_id(-1), x(0), y(0), amount(0), completed(false) {} + + Task(int id, float x_pos, float y_pos, float amt) + : task_id(id), x(x_pos), y(y_pos), amount(amt), completed(false) {} + + float distance_to(float px, float py) const { + float dx = x - px; + float dy = y - py; + return sqrt(dx * dx + dy * dy); + } +}; + +// ===================== BT Context (Blackboard) ===================== +/** + * Shared context/blackboard for all BT nodes. + * Contains agent state, task information, and inter-node communication. + */ +struct BTContext { + // Agent identification + int agent_id; + + // Position and orientation (updated from PC via UDP) + float position_x; + float position_y; + float yaw; + + // Task information + std::vector tasks; + int assigned_task_id; + + // CBBA reference for decision making (void* to avoid circular include) + void* cbba; + + // Thresholds and parameters + float arrive_threshold; + float work_rate; + + // Exploration parameters + float explore_target_x; + float explore_target_y; + bool has_explore_target; + unsigned long explore_start_ms; + unsigned long explore_duration_ms; + struct { + float x_min, x_max, y_min, y_max; + } explore_bounds; + + // Motion output (set by action nodes, read by motion controller) + bool has_target_position; + float target_position_x; + float target_position_y; + + // Constructor with defaults + BTContext(int id = 0); + + // Reset exploration state + void reset_exploration(); +}; + +// ===================== Base Node Class ===================== +/** + * Abstract base class for all behavior tree nodes. + */ +class BTNode { +public: + BTNode(const char* name); + virtual ~BTNode() = default; + + // Main execution method - must be implemented by derived classes + virtual BTStatus tick(BTContext& ctx) = 0; + + // Called when node is halted (e.g., parent fails/succeeds) + virtual void halt(); + + // Reset node state + virtual void reset(); + + // Getters + const char* get_name() const { return _name; } + BTStatus get_status() const { return _status; } + +protected: + const char* _name; + BTStatus _status; +}; + +// ===================== Control Nodes ===================== + +/** + * Sequence: Executes children in order until one fails. + * Remembers which child was running for next tick. + */ +class Sequence : public BTNode { +public: + Sequence(const char* name, std::vector children); + BTStatus tick(BTContext& ctx) override; + void halt() override; + void reset() override; + +private: + std::vector _children; + size_t _current_child_index; + + void _halt_children(); +}; + +/** + * ReactiveSequence: Re-evaluates all children from the start each tick. + * Does not remember running child position. + */ +class ReactiveSequence : public BTNode { +public: + ReactiveSequence(const char* name, std::vector children); + BTStatus tick(BTContext& ctx) override; + void halt() override; + +private: + std::vector _children; + + void _halt_children(); +}; + +/** + * Fallback (Selector): Executes children until one succeeds. + * Remembers which child was running for next tick. + */ +class Fallback : public BTNode { +public: + Fallback(const char* name, std::vector children); + BTStatus tick(BTContext& ctx) override; + void halt() override; + void reset() override; + +private: + std::vector _children; + size_t _current_child_index; + + void _halt_children(); +}; + +/** + * ReactiveFallback: Re-evaluates all children from the start each tick. + * Does not remember running child position. + */ +class ReactiveFallback : public BTNode { +public: + ReactiveFallback(const char* name, std::vector children); + BTStatus tick(BTContext& ctx) override; + void halt() override; + +private: + std::vector _children; + + void _halt_children(); +}; + +// ===================== Action Nodes ===================== + +/** + * GatherLocalInfo: Updates context with local sensing information. + * In onboard implementation, this mainly syncs CBBA position. + */ +class GatherLocalInfo : public BTNode { +public: + GatherLocalInfo(const char* name = "GatherLocalInfo"); + BTStatus tick(BTContext& ctx) override; +}; + +/** + * AssignTask: Uses CBBA to assign a task to this agent. + * Returns SUCCESS if task assigned, FAILURE otherwise. + */ +class AssignTask : public BTNode { +public: + AssignTask(const char* name = "AssignTask"); + BTStatus tick(BTContext& ctx) override; +}; + +/** + * MoveToTarget: Sets motion target to assigned task position. + * Returns RUNNING while moving. + */ +class MoveToTarget : public BTNode { +public: + MoveToTarget(const char* name = "MoveToTarget"); + BTStatus tick(BTContext& ctx) override; +}; + +/** + * ExecuteTask: Performs work at task location while following. + * In PC-managed mode, just maintains position at task. + * Returns RUNNING while executing. + */ +class ExecuteTask : public BTNode { +public: + ExecuteTask(const char* name = "ExecuteTask"); + BTStatus tick(BTContext& ctx) override; +}; + +/** + * Explore: Random exploration when no task is assigned. + * Generates random waypoints within bounds. + * Returns RUNNING while exploring. + */ +class Explore : public BTNode { +public: + Explore(const char* name = "Explore"); + BTStatus tick(BTContext& ctx) override; + void halt() override; +}; + +// ===================== Condition Nodes ===================== + +/** + * IsTaskCompleted: Checks if assigned task is completed. + * Task is completed if it's no longer in the task list (removed by PC). + * Returns SUCCESS if completed, FAILURE otherwise. + */ +class IsTaskCompleted : public BTNode { +public: + IsTaskCompleted(const char* name = "IsTaskCompleted"); + BTStatus tick(BTContext& ctx) override; +}; + +/** + * IsArrivedAtTarget: Checks if agent is within threshold of task. + * Returns SUCCESS if arrived, FAILURE otherwise. + */ +class IsArrivedAtTarget : public BTNode { +public: + IsArrivedAtTarget(const char* name = "IsArrivedAtTarget"); + BTStatus tick(BTContext& ctx) override; +}; + +// ===================== Behavior Tree ===================== +/** + * Main behavior tree class. + * Owns the root node and provides tick interface. + */ +class BehaviorTree { +public: + BehaviorTree(BTNode* root); + ~BehaviorTree(); + + // Execute one tick of the behavior tree + BTStatus tick(BTContext& ctx); + + // Reset tree state + void reset(); + +private: + BTNode* _root; +}; + +// ===================== Tree Builder Helper ===================== +/** + * Helper class for building behavior trees in a readable way. + * + * Usage example (matches default_bt.xml): + * BTNode* tree = TreeBuilder::ReactiveSequence("Root", { + * new GatherLocalInfo(), + * TreeBuilder::ReactiveFallback("TaskAssign", { + * new AssignTask(), + * new Explore() + * }) + * }); + */ +class TreeBuilder { +public: + static BTNode* Sequence(const char* name, std::vector children); + static BTNode* ReactiveSequence(const char* name, std::vector children); + static BTNode* Fallback(const char* name, std::vector children); + static BTNode* ReactiveFallback(const char* name, std::vector children); +}; + +#endif // BT_NODES_H From 33c82bec8c59d158044e49a79cde0afb904e25d9 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 21 Jan 2026 18:51:47 +0900 Subject: [PATCH 12/16] Add CBBA algorithm for onboard control --- src/cbba.cpp | 707 +++++++++++++++++++++++++++++++++++++++++++++++++++ src/cbba.h | 282 ++++++++++++++++++++ 2 files changed, 989 insertions(+) create mode 100644 src/cbba.cpp create mode 100644 src/cbba.h diff --git a/src/cbba.cpp b/src/cbba.cpp new file mode 100644 index 0000000..2a50532 --- /dev/null +++ b/src/cbba.cpp @@ -0,0 +1,707 @@ +/* + cbba.cpp - Consensus-Based Bundle Algorithm implementation + + Ported from Python implementation (cbba.py) + for onboard execution on ESP32. + + [MODIFIED] Added filtering of completed tasks in message sharing +*/ + +#include "cbba.h" +#include + +// ===================== CBBATask Implementation ===================== + +void CBBATask::copy_from(const void* task_ptr) { + const CBBATask* src = static_cast(task_ptr); + if (src) { + task_id = src->task_id; + x = src->x; + y = src->y; + amount = src->amount; + completed = src->completed; + } +} + +// ===================== CBBA Implementation ===================== + +CBBA::CBBA(int agent_id, const CBBAConfig& config) + : _agent_id(agent_id), + _pos_x(0), _pos_y(0), + _config(config), + _phase(CBBAPhase::BUILD_BUNDLE), + _assigned_task_id(-1), + _no_bundle_duration(0), + _last_decide_time(0) +{ + _z.clear(); + _y.clear(); + _s.clear(); + _bundle.clear(); + _path.clear(); +} + +void CBBA::set_position(float x, float y) { + _pos_x = x; + _pos_y = y; +} + +int CBBA::decide(const std::vector& tasks) { + unsigned long now = millis(); + float dt = (now - _last_decide_time) / 1000.0f; + _last_decide_time = now; + + // Cache local tasks + _local_tasks = tasks; + + // ===== [MODIFIED] Clean up completed tasks from _y and _z ===== + _cleanup_completed_tasks(); + + // Check if the existing task is done + if (_assigned_task_id >= 0) { + CBBATask* assigned = _find_task(_assigned_task_id); + if (assigned == nullptr || assigned->completed) { + if (!_path.empty() && _path[0] == _assigned_task_id) { + _path.erase(_path.begin()); + if (!_bundle.empty()) { + _bundle.erase(_bundle.begin()); + } + } + _assigned_task_id = -1; + _phase = CBBAPhase::BUILD_BUNDLE; + } + } + + if (_bundle.empty()) { + _phase = CBBAPhase::BUILD_BUNDLE; + } + + if (tasks.empty() && _bundle.empty()) { + return -1; + } + + // Winning bid cancel on timeout + if (_config.winning_bid_cancel) { + if (_bundle.empty()) { + _no_bundle_duration += dt; + } + + if (_no_bundle_duration > _config.no_bundle_duration_limit) { + _z.clear(); + _y.clear(); + _s.clear(); + _no_bundle_duration = 0; + + Serial.println("[CBBA] Bid timeout - reset all bids"); + } + } + + // ===== Phase 1: Build Bundle ===== + if (_phase == CBBAPhase::BUILD_BUNDLE) { + _build_bundle(tasks); + _phase = CBBAPhase::ASSIGNMENT_CONSENSUS; + _assigned_task_id = -1; + return -1; + } + + // ===== Phase 2: Assignment Consensus ===== + if (_phase == CBBAPhase::ASSIGNMENT_CONSENSUS) { + _update_time_stamp(); + + for (const CBBATask& task : tasks) { + for (const CBBAMessage& msg : _messages_received) { + if (msg.agent_id == _agent_id) continue; + _apply_consensus_rules(task.task_id, msg); + } + } + + _messages_received.clear(); + + std::vector updated_bundle, updated_path; + _update_bundle_and_path(updated_bundle, updated_path); + + if (_config.winning_bid_cancel) { + if (!updated_bundle.empty()) { + _no_bundle_duration = 0; + } + } + + if (updated_bundle == _bundle) { + _assigned_task_id = _path.empty() ? -1 : _path[0]; + return _assigned_task_id; + } else { + _bundle = updated_bundle; + _path = updated_path; + _assigned_task_id = -1; + _phase = CBBAPhase::BUILD_BUNDLE; + } + } + + if (_config.keep_moving_during_convergence) { + _assigned_task_id = _path.empty() ? -1 : _path[0]; + return _assigned_task_id; + } + + return -1; +} + +// ===== [NEW] Clean up completed tasks from internal maps ===== +void CBBA::_cleanup_completed_tasks() { + // Build set of valid (non-completed) task IDs + std::set valid_task_ids; + for (const auto& task : _local_tasks) { + if (!task.completed) { + valid_task_ids.insert(task.task_id); + } + } + + // Remove completed tasks from _y (winning bids) + auto y_it = _y.begin(); + while (y_it != _y.end()) { + if (valid_task_ids.find(y_it->first) == valid_task_ids.end()) { + y_it = _y.erase(y_it); + } else { + ++y_it; + } + } + + // Remove completed tasks from _z (winning agents) + auto z_it = _z.begin(); + while (z_it != _z.end()) { + if (valid_task_ids.find(z_it->first) == valid_task_ids.end()) { + z_it = _z.erase(z_it); + } else { + ++z_it; + } + } +} + +void CBBA::_build_bundle(const std::vector& tasks) { + int max_bundle_size = std::min(_config.max_tasks_per_agent, (int)tasks.size()); + + while ((int)_bundle.size() < max_bundle_size) { + std::map my_bid_list; + std::map best_insertion_idx_list; + _get_my_bid_value_list(tasks, my_bid_list, best_insertion_idx_list); + + int best_task_id = _get_best_task(my_bid_list); + + if (best_task_id < 0) { + break; + } + + int best_idx = best_insertion_idx_list[best_task_id]; + + if (best_idx >= (int)_bundle.size()) { + _bundle.push_back(best_task_id); + } else { + _bundle.insert(_bundle.begin() + best_idx, best_task_id); + } + + if (best_idx >= (int)_path.size()) { + _path.push_back(best_task_id); + } else { + _path.insert(_path.begin() + best_idx, best_task_id); + } + + _y[best_task_id] = my_bid_list[best_task_id]; + _z[best_task_id] = _agent_id; + } +} + +void CBBA::_get_my_bid_value_list( + const std::vector& tasks, + std::map& my_bid_list, + std::map& best_insertion_idx_list) +{ + float S_p = _calculate_score_along_path(_path); + + my_bid_list.clear(); + best_insertion_idx_list.clear(); + + for (const CBBATask& task : tasks) { + if (_is_task_in_path(task.task_id)) { + continue; + } + + if (task.completed) { + continue; + } + + std::vector marginal_scores; + + for (size_t idx = 0; idx <= _path.size(); idx++) { + std::vector alt_path = _path; + alt_path.insert(alt_path.begin() + idx, task.task_id); + + float S_alt = _calculate_score_along_path(alt_path); + marginal_scores.push_back(S_alt - S_p); + } + + int best_idx = 0; + float best_score = marginal_scores[0]; + for (size_t i = 1; i < marginal_scores.size(); i++) { + if (marginal_scores[i] > best_score) { + best_score = marginal_scores[i]; + best_idx = i; + } + } + + my_bid_list[task.task_id] = best_score; + best_insertion_idx_list[task.task_id] = best_idx; + } +} + +float CBBA::_calculate_score_along_path(const std::vector& path) { + float current_x = _pos_x; + float current_y = _pos_y; + float expected_reward = 0; + float distance_from_start = 0; + + for (int task_id : path) { + CBBATask* task = _find_task(task_id); + if (task == nullptr) continue; + + float dx = task->x - current_x; + float dy = task->y - current_y; + float dist = sqrt(dx * dx + dy * dy); + distance_from_start += dist; + + // Convert distance to time (matching Python) + float time_to_task = distance_from_start / _config.max_speed; + float work_time = task->amount / _config.work_rate; + float total_time = time_to_task + work_time; + + // Apply discount factor + float discount = pow(_config.lambda, total_time); + expected_reward += task->amount * discount; + + current_x = task->x; + current_y = task->y; + } + + return expected_reward; +} + +int CBBA::_get_best_task(const std::map& my_bid_list) { + int best_task = -1; + float best_bid = -1e9; + + for (const auto& pair : my_bid_list) { + int task_id = pair.first; + float my_bid = pair.second; + + // Get current winning bid + float current_winning = 0; + auto y_it = _y.find(task_id); + if (y_it != _y.end()) { + current_winning = y_it->second; + } + + // Only consider if my bid is higher than current winning bid + if (my_bid > current_winning && my_bid > best_bid) { + best_bid = my_bid; + best_task = task_id; + } + } + + return best_task; +} + +void CBBA::_update_time_stamp() { + unsigned long now = millis(); + _s[_agent_id] = now; + + // Merge timestamps from nearby agents + for (int neighbor_id : _nearby_agents) { + if (_s.find(neighbor_id) == _s.end()) { + _s[neighbor_id] = 0; + } + } +} + +void CBBA::_apply_consensus_rules(int task_id, const CBBAMessage& msg) { + int i = _agent_id; // me + int k = msg.agent_id; // sender + + // Get sender's info about this task + float y_k = 0; + int z_k = -1; + + auto y_it = msg.winning_bids.find(task_id); + if (y_it != msg.winning_bids.end()) { + y_k = y_it->second; + } + + auto z_it = msg.winning_agents.find(task_id); + if (z_it != msg.winning_agents.end()) { + z_k = z_it->second; + } + + // Get my info about this task + float y_i = 0; + int z_i = -1; + + auto my_y_it = _y.find(task_id); + if (my_y_it != _y.end()) { + y_i = my_y_it->second; + } + + auto my_z_it = _z.find(task_id); + if (my_z_it != _z.end()) { + z_i = my_z_it->second; + } + + // Merge timestamps + _merge_timestamps(msg.timestamps); + + // Helper lambda for timestamp comparison + auto get_ts = [](const std::map& s, int agent) -> unsigned long { + auto it = s.find(agent); + return (it != s.end()) ? it->second : 0; + }; + + const auto& s_k = msg.timestamps; + const auto& s_i = _s; + + // Apply CBBA consensus rules (Rules 1-17 from the paper) + if (z_k == k) { + // Sender thinks they are the winner + if (z_i == i) { + // Rule 1: I also think I'm the winner + if (y_k > y_i) { + _update(task_id, y_k, z_k); + } + } else if (z_i == k) { + // Rule 2: I already think sender is winner + _update(task_id, y_k, z_k); + } else if (z_i < 0) { + // Rule 5: I have no winner + _update(task_id, y_k, z_k); + } else { + // Rule 3-4: I think someone else is winner + int m = z_i; + if (get_ts(s_k, m) > get_ts(s_i, m)) { + _update(task_id, y_k, z_k); + } else if (y_k > y_i) { + _update(task_id, y_k, z_k); + } + } + } else if (z_k == i) { + // Sender thinks I am the winner + if (z_i == i) { + // Rule 6: I also think I'm the winner - leave + _leave(); + } else if (z_i == k) { + // Rule 7: I think sender is winner + _reset_task(task_id); + } else if (z_i < 0) { + // Rule 8: I have no winner + // Leave (no action) + } else { + // Rule 9: I think someone else (m) is winner + int m = z_i; + if (get_ts(s_k, m) > get_ts(s_i, m)) { + _reset_task(task_id); + } + } + } else { + // Sender thinks someone else (m) is winner + int m = z_k; + + if (z_i == i) { + // Rule 14: I think I'm winner + if (get_ts(s_k, m) > get_ts(s_i, m) && y_k > y_i) { + _update(task_id, y_k, z_k); + } + } else if (z_i == k) { + // Rule 10: I think sender is winner + if (get_ts(s_k, m) > get_ts(s_i, m)) { + _update(task_id, y_k, z_k); + } else { + _reset_task(task_id); + } + } else if (z_i == m) { + // Rule 11: I also think m is winner + if (get_ts(s_k, m) > get_ts(s_i, m)) { + _update(task_id, y_k, z_k); + } + } else if (z_i < 0) { + // Rule 13: I have no winner + if (get_ts(s_k, m) > get_ts(s_i, m)) { + _update(task_id, y_k, z_k); + } + } else { + // Rule 12: I think someone else (n) is winner + int n = z_i; + if (get_ts(s_k, m) > get_ts(s_i, m) && get_ts(s_k, n) > get_ts(s_i, n)) { + _update(task_id, y_k, z_k); + } else if (get_ts(s_k, m) > get_ts(s_i, m) && y_k > y_i) { + _update(task_id, y_k, z_k); + } else if (get_ts(s_k, n) > get_ts(s_i, n) && get_ts(s_i, m) > get_ts(s_k, m)) { + _reset_task(task_id); + } + } + } +} + +void CBBA::_update(int task_id, float y_k, int z_k) { + _y[task_id] = y_k; + _z[task_id] = z_k; +} + +void CBBA::_reset_task(int task_id) { + _y[task_id] = 0; + _z[task_id] = -1; +} + +void CBBA::_leave() { + // No action (matches Python implementation) +} + +bool CBBA::_update_bundle_and_path(std::vector& updated_bundle, std::vector& updated_path) { + int n_bar = _bundle.size(); + + for (size_t idx = 0; idx < _bundle.size(); idx++) { + int task_id = _bundle[idx]; + auto z_it = _z.find(task_id); + if (z_it == _z.end() || z_it->second != _agent_id) { + n_bar = idx; + break; + } + } + + updated_bundle.clear(); + updated_path.clear(); + + for (int i = 0; i < n_bar; i++) { + updated_bundle.push_back(_bundle[i]); + if (i < (int)_path.size()) { + updated_path.push_back(_path[i]); + } + } + + return (updated_bundle != _bundle); +} + +void CBBA::receive_message(const CBBAMessage& message) { + if (message.agent_id == _agent_id) return; + + _messages_received.push_back(message); + + bool found = false; + for (int id : _nearby_agents) { + if (id == message.agent_id) { + found = true; + break; + } + } + if (!found) { + _nearby_agents.push_back(message.agent_id); + } +} + +void CBBA::receive_message_json(const JsonDocument& doc) { + CBBAMessage msg; + + if (doc.containsKey("agent_id")) { + msg.agent_id = doc["agent_id"]; + } else { + return; + } + + if (msg.agent_id == _agent_id) return; + + if (doc.containsKey("winning_bids")) { + JsonObjectConst bids = doc["winning_bids"].as(); + for (JsonPairConst kv : bids) { + int task_id = atoi(kv.key().c_str()); + float bid = kv.value().as(); + msg.winning_bids[task_id] = bid; + } + } + + if (doc.containsKey("winning_agents")) { + JsonObjectConst agents = doc["winning_agents"].as(); + for (JsonPairConst kv : agents) { + int task_id = atoi(kv.key().c_str()); + int agent = kv.value().as(); + msg.winning_agents[task_id] = agent; + } + } + + if (doc.containsKey("message_received_time_stamp")) { + JsonObjectConst timestamps = doc["message_received_time_stamp"].as(); + for (JsonPairConst kv : timestamps) { + int agent_id = atoi(kv.key().c_str()); + unsigned long ts = kv.value().as(); + msg.timestamps[agent_id] = ts; + } + } + + receive_message(msg); +} + +void CBBA::get_message_to_share(CBBAMessage& message) { + message.agent_id = _agent_id; + + // ===== [MODIFIED] Only include valid (non-completed) tasks ===== + std::set valid_task_ids; + for (const auto& task : _local_tasks) { + if (!task.completed) { + valid_task_ids.insert(task.task_id); + } + } + + message.winning_bids.clear(); + for (const auto& pair : _y) { + if (valid_task_ids.find(pair.first) != valid_task_ids.end()) { + message.winning_bids[pair.first] = pair.second; + } + } + + message.winning_agents.clear(); + for (const auto& pair : _z) { + if (valid_task_ids.find(pair.first) != valid_task_ids.end()) { + message.winning_agents[pair.first] = pair.second; + } + } + + message.timestamps = _s; +} + +void CBBA::get_message_to_share_json(JsonDocument& doc) { + doc.clear(); + doc["agent_id"] = _agent_id; + + // ===== [MODIFIED] Only include valid (non-completed) tasks ===== + std::set valid_task_ids; + for (const auto& task : _local_tasks) { + if (!task.completed) { + valid_task_ids.insert(task.task_id); + } + } + + JsonObject bids = doc.createNestedObject("winning_bids"); + for (const auto& pair : _y) { + // Only transmit bids for valid tasks + if (valid_task_ids.find(pair.first) != valid_task_ids.end()) { + bids[String(pair.first)] = pair.second; + } + } + + JsonObject agents = doc.createNestedObject("winning_agents"); + for (const auto& pair : _z) { + // Only transmit winners for valid tasks + if (valid_task_ids.find(pair.first) != valid_task_ids.end()) { + agents[String(pair.first)] = pair.second; + } + } + + JsonObject timestamps = doc.createNestedObject("message_received_time_stamp"); + for (const auto& pair : _s) { + timestamps[String(pair.first)] = pair.second; + } +} + +void CBBA::add_nearby_agent(int agent_id) { + if (agent_id == _agent_id) return; + + for (int id : _nearby_agents) { + if (id == agent_id) return; + } + _nearby_agents.push_back(agent_id); +} + +void CBBA::clear_nearby_agents() { + _nearby_agents.clear(); +} + +void CBBA::reset() { + _z.clear(); + _y.clear(); + _s.clear(); + _bundle.clear(); + _path.clear(); + _phase = CBBAPhase::BUILD_BUNDLE; + _assigned_task_id = -1; + _no_bundle_duration = 0; + _messages_received.clear(); + _nearby_agents.clear(); +} + +void CBBA::clear_task(int task_id) { + auto it = std::find(_bundle.begin(), _bundle.end(), task_id); + if (it != _bundle.end()) { + _bundle.erase(it); + } + + it = std::find(_path.begin(), _path.end(), task_id); + if (it != _path.end()) { + _path.erase(it); + } + + _y.erase(task_id); + _z.erase(task_id); + + if (_assigned_task_id == task_id) { + _assigned_task_id = -1; + } +} + +CBBATask* CBBA::_find_task(int task_id) { + for (auto& task : _local_tasks) { + if (task.task_id == task_id) { + return &task; + } + } + return nullptr; +} + +bool CBBA::_is_task_in_path(int task_id) const { + for (int id : _path) { + if (id == task_id) return true; + } + return false; +} + +void CBBA::_merge_timestamps(const std::map& other) { + for (const auto& pair : other) { + auto it = _s.find(pair.first); + if (it == _s.end() || pair.second > it->second) { + _s[pair.first] = pair.second; + } + } +} + +void CBBA::print_state() const { + Serial.printf("[CBBA] Agent %d | Phase: %s | Assigned: %d\n", + _agent_id, + (_phase == CBBAPhase::BUILD_BUNDLE) ? "BUILD" : "CONSENSUS", + _assigned_task_id); + + Serial.print(" Bundle: ["); + for (size_t i = 0; i < _bundle.size(); i++) { + if (i > 0) Serial.print(", "); + Serial.print(_bundle[i]); + } + Serial.println("]"); + + Serial.print(" Winners (z): {"); + bool first = true; + for (const auto& pair : _z) { + if (!first) Serial.print(", "); + Serial.printf("%d:%d", pair.first, pair.second); + first = false; + } + Serial.println("}"); + + Serial.print(" Bids (y): {"); + first = true; + for (const auto& pair : _y) { + if (!first) Serial.print(", "); + Serial.printf("%d:%.2f", pair.first, pair.second); + first = false; + } + Serial.println("}"); +} \ No newline at end of file diff --git a/src/cbba.h b/src/cbba.h new file mode 100644 index 0000000..84177cc --- /dev/null +++ b/src/cbba.h @@ -0,0 +1,282 @@ +/* + cbba.h - Consensus-Based Bundle Algorithm for MONA ESP robot + + Ported from Python implementation (cbba.py) + for onboard execution on ESP32. + + This implementation follows the CBBA paper algorithm: + - Phase 1: Build Bundle (Algorithm 3) + - Phase 2: Assignment Consensus (Rules 1-17) + + [MODIFIED] Added _cleanup_completed_tasks() for filtering completed tasks +*/ + +#ifndef CBBA_H +#define CBBA_H + +#include +#include +#include +#include +#include +#include +#include + +// Forward declaration +class Task; + +// ===================== CBBA Configuration ===================== +struct CBBAConfig { + int max_tasks_per_agent; // Maximum tasks in bundle + float lambda; // Task reward discount factor + bool winning_bid_cancel; // Enable bid cancellation on timeout + float no_bundle_duration_limit; // Seconds before bid reset + bool keep_moving_during_convergence; + float work_rate; // Agent work rate for score calculation + float max_speed; // Agent max speed (mm/s) + + CBBAConfig() + : max_tasks_per_agent(4), + lambda(0.999f), + winning_bid_cancel(true), + no_bundle_duration_limit(5.0f), + keep_moving_during_convergence(false), + work_rate(1.0f), + max_speed(0.25f) {} +}; + +// ===================== CBBA Phase ===================== +enum class CBBAPhase { + BUILD_BUNDLE = 1, + ASSIGNMENT_CONSENSUS = 2 +}; + +// ===================== Task Class ===================== +/** + * Task representation for CBBA. + * Must match the Task class used in bt_nodes.h + */ +class CBBATask { +public: + int task_id; + float x; + float y; + float amount; + bool completed; + + CBBATask() : task_id(-1), x(0), y(0), amount(0), completed(false) {} + + CBBATask(int id, float x_pos, float y_pos, float amt) + : task_id(id), x(x_pos), y(y_pos), amount(amt), completed(false) {} + + float distance_to(float px, float py) const { + float dx = x - px; + float dy = y - py; + return sqrt(dx * dx + dy * dy); + } + + // Copy from Task pointer (from bt_nodes.h) + void copy_from(const void* task_ptr); +}; + +// ===================== CBBA Message ===================== +/** + * Message structure for ESP-NOW communication + */ +struct CBBAMessage { + int agent_id; + std::map winning_bids; // y: task_id -> bid value + std::map winning_agents; // z: task_id -> agent_id + std::map timestamps; // s: agent_id -> timestamp + + void clear() { + agent_id = -1; + winning_bids.clear(); + winning_agents.clear(); + timestamps.clear(); + } +}; + +// ===================== CBBA Class ===================== +/** + * Consensus-Based Bundle Algorithm implementation. + * Follows the algorithm from the CBBA paper with full consensus rules. + */ +class CBBA { +public: + /** + * Constructor + * @param agent_id This agent's ID + * @param config CBBA configuration parameters + */ + CBBA(int agent_id, const CBBAConfig& config = CBBAConfig()); + + /** + * Set agent's current position + */ + void set_position(float x, float y); + + /** + * Main decision function - called each tick + * @param tasks List of available tasks + * @return Assigned task ID, or -1 if no assignment + */ + int decide(const std::vector& tasks); + + /** + * Process received message from another agent + * @param message Received CBBA message + */ + void receive_message(const CBBAMessage& message); + + /** + * Process received message from JSON (ESP-NOW) + * @param doc JSON document containing message + */ + void receive_message_json(const JsonDocument& doc); + + /** + * Get message to share with other agents + * [MODIFIED] Only includes valid (non-completed) tasks + * @param message Output message structure + */ + void get_message_to_share(CBBAMessage& message); + + /** + * Get message to share as JSON (for ESP-NOW) + * [MODIFIED] Only includes valid (non-completed) tasks + * @param doc Output JSON document + */ + void get_message_to_share_json(JsonDocument& doc); + + /** + * Add a nearby agent ID (for timestamp updates) + */ + void add_nearby_agent(int agent_id); + + /** + * Clear nearby agents list + */ + void clear_nearby_agents(); + + /** + * Reset the algorithm state + */ + void reset(); + + /** + * Clear a specific task from bundle (when task is completed) + */ + void clear_task(int task_id); + + // Getters + bool has_task() const { return !_bundle.empty(); } + int get_assigned_task_id() const { return _assigned_task_id; } + CBBAPhase get_phase() const { return _phase; } + const std::vector& get_bundle() const { return _bundle; } + const std::vector& get_path() const { return _path; } + + // Debug + void print_state() const; + +private: + // Agent info + int _agent_id; + float _pos_x, _pos_y; + CBBAConfig _config; + + // CBBA state + std::map _z; // Winning agent list (task_id -> agent_id) + std::map _y; // Winning bid list (task_id -> bid value) + std::map _s; // Timestamp list (agent_id -> timestamp) + std::vector _bundle; // Bundle (list of task IDs) + std::vector _path; // Path (list of task IDs, same as bundle for now) + + CBBAPhase _phase; + int _assigned_task_id; + float _no_bundle_duration; + unsigned long _last_decide_time; + + // Nearby agents for timestamp update + std::vector _nearby_agents; + + // Received messages buffer + std::vector _messages_received; + + // Local tasks cache + std::vector _local_tasks; + + // ===== Phase 1: Build Bundle (Algorithm 3) ===== + void _build_bundle(const std::vector& tasks); + + /** + * Get bid values for all tasks (Algorithm 3, Line 7) + * @param tasks Available tasks + * @param my_bid_list Output: task_id -> bid value + * @param best_insertion_idx_list Output: task_id -> best insertion index + */ + void _get_my_bid_value_list( + const std::vector& tasks, + std::map& my_bid_list, + std::map& best_insertion_idx_list + ); + + /** + * Calculate score along path (Eqn 11 in CBBA paper) + */ + float _calculate_score_along_path(const std::vector& path); + + /** + * Get best task to add (Algorithm 3, Lines 8-9) + * @return Task ID or -1 if none + */ + int _get_best_task(const std::map& my_bid_list); + + // ===== Phase 2: Consensus ===== + + /** + * Update timestamp (Eqn 5) + */ + void _update_time_stamp(); + + /** + * Apply consensus rules for a task + */ + void _apply_consensus_rules(int task_id, const CBBAMessage& msg); + + /** + * Update winning bid and agent (used in consensus rules) + */ + void _update(int task_id, float y_k, int z_k); + + /** + * Reset winning bid and agent + */ + void _reset_task(int task_id); + + /** + * Leave action (currently no-op, matches Python) + */ + void _leave(); + + /** + * Update bundle and path after consensus + * @return true if bundle changed + */ + bool _update_bundle_and_path(std::vector& updated_bundle, std::vector& updated_path); + + /** + * [NEW] Clean up completed tasks from internal maps (_y, _z) + * Called at the beginning of decide() to remove stale entries + */ + void _cleanup_completed_tasks(); + + // Utility + CBBATask* _find_task(int task_id); + bool _is_task_in_path(int task_id) const; + + // Merge timestamps (max of each) + void _merge_timestamps(const std::map& other); +}; + +#endif // CBBA_H \ No newline at end of file From 71d6e4612eb86e218d1898c75a099b8aea0cc057 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Wed, 21 Jan 2026 18:53:07 +0900 Subject: [PATCH 13/16] =?UTF-8?q?offboard=20=EC=97=90=EC=84=9C=20onboard?= =?UTF-8?q?=EB=A1=9C=20=EC=9D=B4=EB=A6=84=20=EB=B3=80=EA=B2=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../SPACE_MONA_onboard.ino} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/{SPACE_MONA_offboard/SPACE_MONA_offboard.ino => SPACE_MONA_onboard/SPACE_MONA_onboard.ino} (100%) diff --git a/examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino b/examples/SPACE_MONA_onboard/SPACE_MONA_onboard.ino similarity index 100% rename from examples/SPACE_MONA_offboard/SPACE_MONA_offboard.ino rename to examples/SPACE_MONA_onboard/SPACE_MONA_onboard.ino From 0d0d4b939ef1020bf376e2b3fa4322e59f8eb739 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Thu, 22 Jan 2026 00:10:41 +0900 Subject: [PATCH 14/16] MONA_micropython_offboard --- Micropython/MONA_offboard/ads7830.py | 178 +++++ Micropython/MONA_offboard/main.py | 2 + Micropython/MONA_offboard/mcp23008.py | 133 ++++ Micropython/MONA_offboard/mona_esp_lib.py | 357 +++++++++ Micropython/MONA_offboard/mona_offboard.py | 877 +++++++++++++++++++++ Micropython/MONA_offboard/neopixel_led.py | 61 ++ 6 files changed, 1608 insertions(+) create mode 100644 Micropython/MONA_offboard/ads7830.py create mode 100644 Micropython/MONA_offboard/main.py create mode 100644 Micropython/MONA_offboard/mcp23008.py create mode 100644 Micropython/MONA_offboard/mona_esp_lib.py create mode 100644 Micropython/MONA_offboard/mona_offboard.py create mode 100644 Micropython/MONA_offboard/neopixel_led.py diff --git a/Micropython/MONA_offboard/ads7830.py b/Micropython/MONA_offboard/ads7830.py new file mode 100644 index 0000000..779455a --- /dev/null +++ b/Micropython/MONA_offboard/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_offboard/main.py b/Micropython/MONA_offboard/main.py new file mode 100644 index 0000000..9b7b5a1 --- /dev/null +++ b/Micropython/MONA_offboard/main.py @@ -0,0 +1,2 @@ +from mona_offboard import main +main() diff --git a/Micropython/MONA_offboard/mcp23008.py b/Micropython/MONA_offboard/mcp23008.py new file mode 100644 index 0000000..496544a --- /dev/null +++ b/Micropython/MONA_offboard/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_offboard/mona_esp_lib.py b/Micropython/MONA_offboard/mona_esp_lib.py new file mode 100644 index 0000000..784d921 --- /dev/null +++ b/Micropython/MONA_offboard/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_offboard/mona_offboard.py b/Micropython/MONA_offboard/mona_offboard.py new file mode 100644 index 0000000..5777a84 --- /dev/null +++ b/Micropython/MONA_offboard/mona_offboard.py @@ -0,0 +1,877 @@ +import network +import socket +import time +import json +import random +import _thread +from machine import Pin, reset +import espnow +import gc + +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, +) + +# ====== USER CONFIG ====== +SSID = "Your SSID" +PASSWORD = "Your Password" +SELF_ID = "0" # 각 로봇마다 변경 (0-11) +TCP_PORT = 8080 +UDP_PORT = 8080 + +# ===================== 상태 정의 ===================== +STATE_IDLE = 0 +STATE_TURNING = 1 +STATE_MOVING = 2 +STATE_AVOID = 3 +STATE_ESCAPING = 4 +STATE_EMERGENCY = 5 + +# ===================== ESP-NOW 설정 ===================== +TOTAL_ROBOTS = 12 +MIN_BROADCAST_MS = 50 +MAX_BROADCAST_MS = 100 +PEER_LINK_DROP_MS = 900 +ESPNOW_MAX_PAYLOAD = 250 # MicroPython ESP-NOW 제한 + +# ===================== 제어 상수 ===================== +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 + +# 비상 동작 +EMERGENCY_SPIN_SPD = 200 +ESCAPING_MS = 1000 +EMERGENCY_SPIN_MS = 1200 +BACK_MS = 120 +OSCILLATION_WINDOW_MS = 1200 +OSCILLATION_COUNT_THRESHOLD = 4 + +# ===================== 전역 변수 (Core 간 공유) ===================== +# 상태 +state = STATE_IDLE + +# 엔코더 카운트 +left_encoder_count = 0 +right_encoder_count = 0 + +# 모션 제어 변수 +start_left_count = 0 +start_right_count = 0 +target_turn_pulses = 0 +target_move_pulses = 0 +turn_direction = 0 + +# Core 0 → Core 1 통신 +target_angle_deg = 0.0 +target_dist_mm = 0.0 +new_motion_command = False +stop_requested = False + +# 탈출/비상 타이머 +escaping_until_ms = 0 +emergency_back_until = 0 +emergency_spin_until = 0 + +# 진동 감지 +last_turn_direction = 0 +turn_change_count = 0 +oscillation_timer_start = 0 + +# ESP-NOW 통신 +received_json_map = {} # {sender_id: json_string} +comm_recv_time_map = {} # {sender_id: timestamp} +self_message_doc = {} # 자신의 CBBA 메시지 +last_broadcast = 0 +dirty_self = False +dirty_neighbors = False + +# 네트워크 객체 +wlan = None +esp = None +tcp_server = None +tcp_clients = [] +udp_socket = None + +# Lock for thread safety +map_lock = None +self_msg_lock = None + +# 엔코더 핀 객체 +encoder_left_pin = None +encoder_right_pin = None + + +# ===================== 엔코더 인터럽트 ===================== +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 clear_motion_targets(): + global target_turn_pulses, target_move_pulses + global start_left_count, start_right_count + + target_turn_pulses = 0 + target_move_pulses = 0 + start_left_count = left_encoder_count + start_right_count = right_encoder_count + + +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 reset_wifi(): + """WiFi 완전 리셋""" + global wlan + + # 기존 연결 정리 + try: + if wlan is not None: + wlan.disconnect() + wlan.active(False) + time.sleep_ms(100) + except: + pass + + wlan = None + gc.collect() + time.sleep_ms(200) + + +def connect_wifi(): + global wlan + + print("[WiFi] Initializing...") + + # WiFi 완전 리셋 + reset_wifi() + + # 새로 초기화 + wlan = network.WLAN(network.STA_IF) + + # 먼저 비활성화 후 활성화 + wlan.active(False) + time.sleep_ms(100) + wlan.active(True) + time.sleep_ms(100) + + # 절전 모드 비활성화 + try: + wlan.config(pm=0) # Power management off + except: + pass # 일부 펌웨어에서 지원 안 할 수 있음 + + print(f"[WiFi] Connecting to {SSID}...") + + # 연결 시도 + try: + wlan.connect(SSID, PASSWORD) + except OSError as e: + print(f"[WiFi] Connect error: {e}") + return False + + # 연결 대기 + timeout = 40 # 20초 + while not wlan.isconnected() and timeout > 0: + time.sleep(0.5) + print(".", end="") + timeout -= 1 + + if wlan.isconnected(): + print(f"\n[WiFi] Connected!") + print(f"[WiFi] IP: {wlan.ifconfig()[0]}") + print(f"[WiFi] Board ID: {SELF_ID}") + return True + else: + print("\n[WiFi] Connection failed!") + return False + + +def setup_espnow(): + global esp, wlan + + # ESP-NOW는 WiFi가 활성화된 상태에서 초기화해야 함 + if wlan is None or not wlan.active(): + print("[ESP-NOW] Error: WiFi not active") + return False + + try: + esp = espnow.ESPNow() + esp.active(True) + + # 브로드캐스트 피어 추가 + broadcast_mac = b'\xff\xff\xff\xff\xff\xff' + try: + esp.add_peer(broadcast_mac) + except OSError: + pass # 이미 추가됨 + + print("[ESP-NOW] Initialized") + return True + except Exception as e: + print(f"[ESP-NOW] Init error: {e}") + return False + + +def setup_tcp_server(): + global tcp_server + + try: + tcp_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + tcp_server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + tcp_server.bind(('0.0.0.0', TCP_PORT)) + tcp_server.listen(5) + tcp_server.setblocking(False) + + print(f"[TCP] Server on port {TCP_PORT}") + return True + except Exception as e: + print(f"[TCP] Setup error: {e}") + return False + + +def setup_udp(): + global udp_socket + + try: + udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + udp_socket.bind(('0.0.0.0', UDP_PORT)) + udp_socket.setblocking(False) + + print(f"[UDP] Server on port {UDP_PORT}") + return True + except Exception as e: + print(f"[UDP] Setup error: {e}") + return False + + +# ===================== ESP-NOW 통신 ===================== +def update_broadcast_recv_json_map(sender_id, json_buf): + global dirty_neighbors, map_lock + + if len(json_buf) < 2: + return False + + # JSON 형식 체크 + if not ((json_buf[0] == '{' and json_buf[-1] == '}') or + (json_buf[0] == '[' and json_buf[-1] == ']')): + return False + + if map_lock is None: + return False + + if map_lock.acquire(False): # Non-blocking + try: + received_json_map[sender_id] = json_buf + comm_recv_time_map[sender_id] = time.ticks_ms() + dirty_neighbors = True + finally: + map_lock.release() + return True + + return False + + +def handle_espnow_recv(): + """ESP-NOW 메시지 수신 처리""" + global esp + + if esp is None: + return + + try: + # Non-blocking receive + host, msg = esp.recv(0) # timeout=0 for non-blocking + if msg is None: + return + + if len(msg) <= 1: + return + + # 패킷 파싱: [id_len][id][json...] + id_len = msg[0] + if len(msg) < 1 + id_len: + return + + sender_id = msg[1:1+id_len].decode('utf-8') + if sender_id == SELF_ID: + return + + json_data = msg[1+id_len:].decode('utf-8') + if len(json_data) > 0: + update_broadcast_recv_json_map(sender_id, json_data) + + except OSError: + pass # 데이터 없음 + except Exception as e: + print(f"[ESP-NOW] Recv error: {e}") + + +def broadcast_self_message_if_due(): + """자신의 CBBA 메시지 브로드캐스트""" + global last_broadcast, esp, self_msg_lock + + if esp is None: + return + + now = time.ticks_ms() + interval = random.randint(MIN_BROADCAST_MS, MAX_BROADCAST_MS) + + if time.ticks_diff(now, last_broadcast) < interval: + return + + last_broadcast = now + + if self_msg_lock is None: + return + + json_str = None + if self_msg_lock.acquire(False): + try: + if self_message_doc: + json_str = json.dumps(self_message_doc) + finally: + self_msg_lock.release() + + if json_str is None or len(json_str) == 0: + return + + # 패킷 구성: [id_len][id][json] + id_bytes = SELF_ID.encode('utf-8') + id_len = len(id_bytes) + + packet = bytes([id_len]) + id_bytes + json_str.encode('utf-8') + + if len(packet) > ESPNOW_MAX_PAYLOAD: + return + + try: + broadcast_mac = b'\xff\xff\xff\xff\xff\xff' + esp.send(broadcast_mac, packet) + except Exception as e: + print(f"[ESP-NOW] Send error: {e}") + + +# ===================== TCP 통신 ===================== +def handle_tcp_clients(): + """TCP 클라이언트 연결 및 데이터 수신""" + global tcp_server, tcp_clients, self_message_doc, dirty_self, self_msg_lock + + if tcp_server is None: + return + + # 새 연결 수락 + try: + client, addr = tcp_server.accept() + client.setblocking(False) + tcp_clients.append(client) + print(f"[TCP] New client: {addr}") + except OSError: + pass # 새 연결 없음 + + # 기존 클라이언트 데이터 처리 + for client in tcp_clients[:]: # 복사본으로 순회 + try: + data = client.recv(2048) + if data: + line = data.decode('utf-8').strip() + if line and self_msg_lock is not None: + if self_msg_lock.acquire(True, 0.01): + try: + self_message_doc = json.loads(line) + dirty_self = True + except (json.JSONDecodeError, ValueError): + pass + finally: + self_msg_lock.release() + else: + # 연결 종료 + client.close() + if client in tcp_clients: + tcp_clients.remove(client) + except OSError: + pass # 데이터 없음 + except Exception as e: + print(f"[TCP] Client error: {e}") + try: + client.close() + except: + pass + if client in tcp_clients: + tcp_clients.remove(client) + + +def send_monitor_to_tcp_clients(): + """모니터링 데이터를 TCP 클라이언트에 전송""" + global dirty_self, dirty_neighbors, map_lock + + if not tcp_clients: + return + + if map_lock is None: + return + + now = time.ticks_ms() + + # 스냅샷 생성 + snapshot = {} + if map_lock.acquire(True, 0.005): + try: + for sender_id, json_str in received_json_map.items(): + recv_time = comm_recv_time_map.get(sender_id, 0) + if time.ticks_diff(now, recv_time) <= PEER_LINK_DROP_MS: + snapshot[sender_id] = json_str + finally: + map_lock.release() + + # JSON 구성 + output = { + "agent_id": SELF_ID, + "received_messages": {} + } + + for sender_id, json_str in snapshot.items(): + try: + output["received_messages"][sender_id] = json.loads(json_str) + except: + pass + + out_str = json.dumps(output) + "\n" + out_bytes = out_str.encode('utf-8') + + # 모든 클라이언트에 전송 + for client in tcp_clients[:]: + try: + client.send(out_bytes) + except Exception as e: + print(f"[TCP] Send error: {e}") + try: + client.close() + except: + pass + if client in tcp_clients: + tcp_clients.remove(client) + + dirty_self = False + dirty_neighbors = False + + +# ===================== UDP 명령 처리 ===================== +def handle_udp_packet(): + """UDP 모션 명령 수신""" + global target_angle_deg, target_dist_mm, new_motion_command, stop_requested + global udp_socket + + if udp_socket is None: + return + + try: + data, addr = udp_socket.recvfrom(256) + except OSError: + return # 데이터 없음 + + if not data: + return + + packet = data.decode('utf-8').strip() + + # STOP 명령 + if packet.upper().startswith("STOP"): + stop_requested = True + return + + # G 명령: "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: + target_angle_deg = angle + target_dist_mm = dist + new_motion_command = True + except ValueError: + pass + + +# ===================== 모션 제어 ===================== +def start_motion(angle_deg, dist_mm): + """모션 시작""" + global state, target_turn_pulses, target_move_pulses + global start_left_count, start_right_count, turn_direction + + if state in (STATE_AVOID, STATE_ESCAPING, STATE_EMERGENCY): + return + + start_left_count = left_encoder_count + start_right_count = right_encoder_count + + 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 + motors_forward(FWD_SPD) # 즉시 모터 시작 + else: + state = STATE_IDLE + motors_stop() + + +def control_loop(r1, r2, r3, r4, r5): + """모션 제어 루프""" + global state, start_left_count, start_right_count + + # 장애물 감지 + 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_now = abs(left_encoder_count - start_left_count) + r_now = abs(right_encoder_count - 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 = left_encoder_count + start_right_count = right_encoder_count + state = STATE_MOVING + motors_forward(FWD_SPD) # 즉시 직진 시작! + else: + state = STATE_IDLE + + elif state == STATE_MOVING: + if avg >= target_move_pulses: + motors_stop() + state = STATE_IDLE + else: + # 단순 직진 (PI 제어 제거) + motors_forward(FWD_SPD) + + 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 + + +# ===================== Core 1: 모션 태스크 ===================== +def motion_task(): + """Core 1에서 실행 - 모션 제어""" + global state, new_motion_command, stop_requested + global encoder_left_pin, encoder_right_pin + + print("[Core 1] Motion control task started") + + # 엔코더 인터럽트 설정 (Core 1에서) + 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) + + last_control_time = time.ticks_ms() + + while True: + try: + # STOP 명령 처리 + if stop_requested: + motors_stop() + clear_motion_targets() + state = STATE_IDLE + stop_requested = False + + # 새 모션 명령 처리 + if new_motion_command: + if state == STATE_MOVING: + l_now = abs(left_encoder_count - start_left_count) + r_now = abs(right_encoder_count - start_right_count) + if ((l_now + r_now) // 2) >= UPDATE_THRESHOLD_PULSES: + start_motion(target_angle_deg, target_dist_mm) + elif state == STATE_IDLE: + start_motion(target_angle_deg, target_dist_mm) + new_motion_command = False + + # IR 센서 읽기 + r1 = get_ir(1) + r2 = get_ir(2) + r3 = get_ir(3) + r4 = get_ir(4) + r5 = get_ir(5) + + # 모션 제어 + control_loop(r1, r2, r3, r4, r5) + + # 제어 주기 유지 (약 5ms 목표) - 기존 20ms에서 단축 + 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 Exception as e: + print(f"[Core 1] Error: {e}") + time.sleep_ms(100) + + +# ===================== 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 map_lock, self_msg_lock, wlan + + print("=" * 40) + print("MONA Firmware v2 - MicroPython Dual Core") + print("(PI Control Removed)") + print("=" * 40) + + # 락 초기화 + map_lock = _thread.allocate_lock() + self_msg_lock = _thread.allocate_lock() + + # WiFi 먼저 초기화 (하드웨어 전에) + retry_count = 0 + max_retries = 3 + + while retry_count < max_retries: + if connect_wifi(): + break + retry_count += 1 + print(f"[WiFi] Retry {retry_count}/{max_retries}...") + time.sleep(2) + + if not wlan or not wlan.isconnected(): + print("[WiFi] Failed after retries. Restarting...") + time.sleep(3) + reset() + + # 하드웨어 초기화 (WiFi 연결 후) + print("Initializing MONA ESP...") + mona_esp_init() + + # ESP-NOW 초기화 + setup_espnow() + + # TCP 서버 시작 + setup_tcp_server() + + # UDP 서버 시작 + setup_udp() + + print("=" * 40) + print("- Core 0: Communication (TCP/ESP-NOW/UDP)") + print("- Core 1: Motion Control (IR/Motors)") + print("- Control Loop: 5ms (200Hz)") + print("=" * 40) + + # Core 1에서 모션 태스크 시작 + _thread.start_new_thread(motion_task, ()) + + # Core 0에서 통신 태스크 실행 (메인 스레드) + led_counter = 0 + last_monitor_send = 0 + + try: + while True: + try: + # WiFi 재연결 + if wlan and not wlan.isconnected(): + print("[WiFi] Reconnecting...") + try: + wlan.connect(SSID, PASSWORD) + except: + pass + time.sleep_ms(100) + continue + + # TCP 클라이언트 처리 + handle_tcp_clients() + + # ESP-NOW 수신 + handle_espnow_recv() + + # ESP-NOW 브로드캐스트 + broadcast_self_message_if_due() + + # TCP 모니터링 전송 (50ms마다) + now = time.ticks_ms() + if time.ticks_diff(now, last_monitor_send) >= 50: + send_monitor_to_tcp_clients() + last_monitor_send = now + + # UDP 명령 처리 + handle_udp_packet() + + # LED 업데이트 (100회마다) + led_counter += 1 + if led_counter >= 100: + update_status_led() + led_counter = 0 + + time.sleep_ms(1) + + except Exception as e: + print(f"[Main] Error: {e}") + time.sleep_ms(100) + + except KeyboardInterrupt: + print("\nStopping...") + motors_stop() + set_led(1, 0, 0, 0) + set_led(2, 0, 0, 0) + if tcp_server: + tcp_server.close() + if udp_socket: + udp_socket.close() + print("MONA stopped.") + + +if __name__ == "__main__": + main() + + diff --git a/Micropython/MONA_offboard/neopixel_led.py b/Micropython/MONA_offboard/neopixel_led.py new file mode 100644 index 0000000..1dcf56d --- /dev/null +++ b/Micropython/MONA_offboard/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 f827d71c10802d9ae4b669e558d3c900f1e20756 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Sat, 24 Jan 2026 21:46:26 +0900 Subject: [PATCH 15/16] =?UTF-8?q?max=20task=20per=20agent=20=EC=9C=A0?= =?UTF-8?q?=EC=A7=80=ED=95=A0=20=EC=88=98=20=EC=9E=88=EB=8F=84=EB=A1=9D=20?= =?UTF-8?q?=EC=88=98=EC=A0=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/cbba.cpp | 138 ++++++++++++++++++++++++++++----------------------- src/cbba.h | 8 +++ 2 files changed, 83 insertions(+), 63 deletions(-) diff --git a/src/cbba.cpp b/src/cbba.cpp index 2a50532..c1d3014 100644 --- a/src/cbba.cpp +++ b/src/cbba.cpp @@ -54,21 +54,15 @@ int CBBA::decide(const std::vector& tasks) { // Cache local tasks _local_tasks = tasks; - // ===== [MODIFIED] Clean up completed tasks from _y and _z ===== + // ===== Clean up completed tasks from _y and _z ===== _cleanup_completed_tasks(); - // Check if the existing task is done + // ===== Remove completed tasks from bundle and path ===== + _remove_completed_from_bundle(); if (_assigned_task_id >= 0) { CBBATask* assigned = _find_task(_assigned_task_id); if (assigned == nullptr || assigned->completed) { - if (!_path.empty() && _path[0] == _assigned_task_id) { - _path.erase(_path.begin()); - if (!_bundle.empty()) { - _bundle.erase(_bundle.begin()); - } - } _assigned_task_id = -1; - _phase = CBBAPhase::BUILD_BUNDLE; } } @@ -84,6 +78,8 @@ int CBBA::decide(const std::vector& tasks) { if (_config.winning_bid_cancel) { if (_bundle.empty()) { _no_bundle_duration += dt; + } else { + _no_bundle_duration = 0; // Reset when we have tasks } if (_no_bundle_duration > _config.no_bundle_duration_limit) { @@ -96,53 +92,79 @@ int CBBA::decide(const std::vector& tasks) { } } - // ===== Phase 1: Build Bundle ===== - if (_phase == CBBAPhase::BUILD_BUNDLE) { + // ===== Always try to fill bundle to max ===== + int max_bundle_size = std::min(_config.max_tasks_per_agent, (int)tasks.size()); + if ((int)_bundle.size() < max_bundle_size) { _build_bundle(tasks); - _phase = CBBAPhase::ASSIGNMENT_CONSENSUS; - _assigned_task_id = -1; - return -1; } // ===== Phase 2: Assignment Consensus ===== - if (_phase == CBBAPhase::ASSIGNMENT_CONSENSUS) { - _update_time_stamp(); - - for (const CBBATask& task : tasks) { - for (const CBBAMessage& msg : _messages_received) { - if (msg.agent_id == _agent_id) continue; - _apply_consensus_rules(task.task_id, msg); - } + _update_time_stamp(); + + for (const CBBATask& task : tasks) { + for (const CBBAMessage& msg : _messages_received) { + if (msg.agent_id == _agent_id) continue; + _apply_consensus_rules(task.task_id, msg); } - - _messages_received.clear(); - - std::vector updated_bundle, updated_path; - _update_bundle_and_path(updated_bundle, updated_path); - - if (_config.winning_bid_cancel) { - if (!updated_bundle.empty()) { - _no_bundle_duration = 0; - } + } + + _messages_received.clear(); + + std::vector updated_bundle, updated_path; + bool bundle_changed = _update_bundle_and_path(updated_bundle, updated_path); + + if (_config.winning_bid_cancel) { + if (!updated_bundle.empty()) { + _no_bundle_duration = 0; } + } + + if (bundle_changed) { + _bundle = updated_bundle; + _path = updated_path; - if (updated_bundle == _bundle) { - _assigned_task_id = _path.empty() ? -1 : _path[0]; - return _assigned_task_id; - } else { - _bundle = updated_bundle; - _path = updated_path; - _assigned_task_id = -1; - _phase = CBBAPhase::BUILD_BUNDLE; + // After consensus changes, try to refill bundle again + max_bundle_size = std::min(_config.max_tasks_per_agent, (int)tasks.size()); + if ((int)_bundle.size() < max_bundle_size) { + _build_bundle(tasks); } } - if (_config.keep_moving_during_convergence) { - _assigned_task_id = _path.empty() ? -1 : _path[0]; - return _assigned_task_id; + // Set assigned task to first in path + _assigned_task_id = _path.empty() ? -1 : _path[0]; + + return _assigned_task_id; +} + +// ===== [NEW] Remove completed tasks from bundle and path ===== +void CBBA::_remove_completed_from_bundle() { + // Build set of valid (non-completed) task IDs + std::set valid_task_ids; + for (const auto& task : _local_tasks) { + if (!task.completed) { + valid_task_ids.insert(task.task_id); + } } - return -1; + // Remove completed tasks from bundle + auto bundle_it = _bundle.begin(); + while (bundle_it != _bundle.end()) { + if (valid_task_ids.find(*bundle_it) == valid_task_ids.end()) { + bundle_it = _bundle.erase(bundle_it); + } else { + ++bundle_it; + } + } + + // Remove completed tasks from path + auto path_it = _path.begin(); + while (path_it != _path.end()) { + if (valid_task_ids.find(*path_it) == valid_task_ids.end()) { + path_it = _path.erase(path_it); + } else { + ++path_it; + } + } } // ===== [NEW] Clean up completed tasks from internal maps ===== @@ -675,10 +697,11 @@ void CBBA::_merge_timestamps(const std::map& other) { } void CBBA::print_state() const { - Serial.printf("[CBBA] Agent %d | Phase: %s | Assigned: %d\n", + Serial.printf("[CBBA] Agent %d | Assigned: %d | Bundle size: %d/%d\n", _agent_id, - (_phase == CBBAPhase::BUILD_BUNDLE) ? "BUILD" : "CONSENSUS", - _assigned_task_id); + _assigned_task_id, + (int)_bundle.size(), + _config.max_tasks_per_agent); Serial.print(" Bundle: ["); for (size_t i = 0; i < _bundle.size(); i++) { @@ -687,21 +710,10 @@ void CBBA::print_state() const { } Serial.println("]"); - Serial.print(" Winners (z): {"); - bool first = true; - for (const auto& pair : _z) { - if (!first) Serial.print(", "); - Serial.printf("%d:%d", pair.first, pair.second); - first = false; - } - Serial.println("}"); - - Serial.print(" Bids (y): {"); - first = true; - for (const auto& pair : _y) { - if (!first) Serial.print(", "); - Serial.printf("%d:%.2f", pair.first, pair.second); - first = false; + Serial.print(" Path: ["); + for (size_t i = 0; i < _path.size(); i++) { + if (i > 0) Serial.print(", "); + Serial.print(_path[i]); } - Serial.println("}"); + Serial.println("]"); } \ No newline at end of file diff --git a/src/cbba.h b/src/cbba.h index 84177cc..0772403 100644 --- a/src/cbba.h +++ b/src/cbba.h @@ -9,6 +9,7 @@ - Phase 2: Assignment Consensus (Rules 1-17) [MODIFIED] Added _cleanup_completed_tasks() for filtering completed tasks + [MODIFIED] Added _remove_completed_from_bundle() for incremental bundle maintenance */ #ifndef CBBA_H @@ -101,6 +102,11 @@ struct CBBAMessage { /** * Consensus-Based Bundle Algorithm implementation. * Follows the algorithm from the CBBA paper with full consensus rules. + * + * [MODIFIED] Bundle is now incrementally maintained: + * - When a task is completed, it's removed from bundle + * - New tasks are immediately added to fill bundle back to max + * - This ensures bundle stays at max_tasks_per_agent whenever possible */ class CBBA { public: @@ -270,6 +276,8 @@ class CBBA { * Called at the beginning of decide() to remove stale entries */ void _cleanup_completed_tasks(); + + void _remove_completed_from_bundle(); // Utility CBBATask* _find_task(int task_id); From 099c42281af350de71d081145f25df68242e1213 Mon Sep 17 00:00:00 2001 From: Chaeyoung1011 Date: Fri, 20 Feb 2026 15:43:52 +0900 Subject: [PATCH 16/16] Delete Dual core --- .../SPACE_MONA_puppet/SPACE_MONA_puppet.ino | 88 +++---------------- 1 file changed, 14 insertions(+), 74 deletions(-) diff --git a/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino b/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino index f63b34b..45d67a9 100644 --- a/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino +++ b/examples/SPACE_MONA_puppet/SPACE_MONA_puppet.ino @@ -45,11 +45,11 @@ static const int PIN_ENCODER_RIGHT = 39; static const float ROTATION_DEADBAND_DEG = 5.0f; // 미세 회전 무시 각도 static const int MIN_MOTOR_PWM = 60; // 모터 구동 최소 출력 -// ===================== PI 제어 게인 (개선) ===================== -static const float K_P = 0.5f; // 비례 게인 감소 (0.95 → 0.5) -static const float K_I = 0.002f; // 적분 게인 대폭 감소 (0.01 → 0.002) +// ===================== PI 제어 게인 ===================== +static const float K_P = 0.5f; +static const float K_I = 0.002f; -// 적분 제한 추가 +// 적분 제한 static const float INTEGRAL_LIMIT = 500.0f; // 오차 데드밴드 @@ -73,11 +73,9 @@ unsigned long oscillation_timer_start = 0; unsigned long emergency_back_until = 0; unsigned long emergency_spin_until = 0; -// ===================== 엔코더 ===================== volatile long left_encoder_count = 0; volatile long right_encoder_count = 0; -// Core 3.x에서는 인터럽트 핸들러에 IRAM_ATTR이 필수입니다. void IRAM_ATTR isr_left_encoder() { left_encoder_count++; } void IRAM_ATTR isr_right_encoder() { right_encoder_count++; } @@ -87,11 +85,6 @@ 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; @@ -151,19 +144,13 @@ static inline void check_oscillation_and_escape(int 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); } @@ -198,39 +185,6 @@ void update_status_led() { } } -// ===================== 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(); @@ -238,10 +192,8 @@ void control_loop(int r1, int r2, int r3, int r4, int r5); void setup() { Serial.begin(115200); - Mona_ESP_init(); - // Core 3.x 대응: 인터럽트 설정 pinMode(PIN_ENCODER_LEFT, INPUT); pinMode(PIN_ENCODER_RIGHT, INPUT); attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_LEFT), isr_left_encoder, RISING); @@ -257,42 +209,31 @@ 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!"); + Serial.println("MONA ready puppet!"); } void loop() { unsigned long now = millis(); - // UDP 패킷 처리 handle_udp_packet(); // 제어 주기 확인 (5ms 간격) if (now - last_control_time >= CONTROL_INTERVAL_MS) { last_control_time = now; - - // 캐시된 IR 값 사용 - int r1, r2, r3, r4, r5; - get_cached_ir(&r1, &r2, &r3, &r4, &r5); - + + // 싱글코어: IR 센서를 loop()에서 직접 읽음 + 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); } - // LED 업데이트 (100ms 간격) if (now - last_led_update >= LED_UPDATE_INTERVAL_MS) { last_led_update = now; @@ -312,7 +253,7 @@ void start_motion(float angle_deg, float dist_mm) { 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); @@ -407,7 +348,6 @@ void control_loop(int r1, int r2, int r3, int r4, int r5) { Motors_stop(); state = STATE_IDLE; } else { - // 개선된 PI 제어 사용 int left_pwm, right_pwm; pi_control(l_now, r_now, &left_pwm, &right_pwm); Left_mot_forward(left_pwm);